/* * Copyright (c) 2011-2022, The DART development contributors * All rights reserved. * * The list of contributors can be found at: * https://github.com/dartsim/dart/blob/master/LICENSE * * This file is provided under the following "BSD-style" License: * Redistribution and use in source and binary forms, with or * without modification, are permitted provided that the following * conditions are met: * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF * USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ #include #include #include "dart/config.hpp" #include "dart/math/Helpers.hpp" #include "TestHelpers.hpp" using namespace Eigen; using namespace dart; using namespace dart::dynamics; //============================================================================== TEST(InverseKinematics, SolveForFreeJoint) { // Very simple test of InverseKinematics module, applied to a FreeJoint to // ensure that the target is reachable SkeletonPtr skel = Skeleton::create(); skel->createJointAndBodyNodePair(); std::shared_ptr ik = skel->getBodyNode(0)->getIK(true); Eigen::Isometry3d tf(Eigen::Isometry3d::Identity()); tf.translation() = Eigen::Vector3d(0.0, 0.0, 0.8); tf.rotate( Eigen::AngleAxisd(math::constantsd::pi() / 8, Eigen::Vector3d(0, 1, 0))); ik->getTarget()->setTransform(tf); ik->getErrorMethod().setBounds( Eigen::Vector6d::Constant(-1e-8), Eigen::Vector6d::Constant(1e-8)); ik->getSolver()->setNumMaxIterations(100); EXPECT_FALSE(equals( ik->getTarget()->getTransform().matrix(), skel->getBodyNode(0)->getTransform().matrix(), 1e-1)); EXPECT_TRUE(ik->getSolver()->solve()); EXPECT_TRUE(equals( ik->getTarget()->getTransform().matrix(), skel->getBodyNode(0)->getTransform().matrix(), 1e-8)); } //============================================================================== class FailingSolver : public optimizer::Solver { public: FailingSolver(double constant) : mConstant(constant) { // Do nothing } ~FailingSolver() override = default; bool solve() override { std::shared_ptr problem = mProperties.mProblem; if (nullptr == problem) { dtwarn << "[FailingSolver::solve] Attempting to solve a nullptr " << "problem! We will return false.\n"; return false; } const auto dim = problem->getDimension(); const Eigen::VectorXd wrongSolution = Eigen::VectorXd::Constant(static_cast(dim), mConstant); problem->setOptimalSolution(wrongSolution); return false; } std::string getType() const override { return "FailingSolver"; } std::shared_ptr clone() const override { return std::make_shared(mConstant); } protected: double mConstant; }; //============================================================================== TEST(InverseKinematics, DoNotApplySolutionOnFailure) { SkeletonPtr skel = Skeleton::create(); skel->createJointAndBodyNodePair(); std::shared_ptr ik = skel->getBodyNode(0)->getIK(true); ik->setSolver(std::make_shared(10)); ik->getTarget()->setTransform(Eigen::Isometry3d::Identity()); const auto dofs = static_cast(skel->getNumDofs()); skel->resetPositions(); EXPECT_FALSE(ik->solveAndApply(false)); EXPECT_TRUE(equals(skel->getPositions(), Eigen::VectorXd::Zero(dofs).eval())); EXPECT_FALSE(ik->solveAndApply(true)); EXPECT_FALSE( equals(skel->getPositions(), Eigen::VectorXd::Zero(dofs).eval())); }