From 46478e9ea3288c8f39993560f3a8e13f161ebe13 Mon Sep 17 00:00:00 2001 From: "M.X. Grey" Date: Thu, 21 Apr 2016 16:31:58 -0400 Subject: [PATCH 1/4] Introducing fixes for whole body IK and EndEffector/ShapeNode Jacobian updating --- dart/constraint/BalanceConstraint.cpp | 26 +++++- dart/constraint/BalanceConstraint.h | 9 ++ dart/dynamics/BodyNode.cpp | 23 +++-- dart/dynamics/InverseKinematics.cpp | 93 +++++++++++++++----- dart/dynamics/InverseKinematics.h | 39 +++++++- dart/dynamics/JacobianNode.cpp | 8 +- dart/dynamics/detail/TemplatedJacobianNode.h | 4 +- dart/gui/osg/examples/osgHuboPuppet.cpp | 4 +- unittests/testForwardKinematics.cpp | 74 +++++++++++++--- 9 files changed, 227 insertions(+), 53 deletions(-) diff --git a/dart/constraint/BalanceConstraint.cpp b/dart/constraint/BalanceConstraint.cpp index 30f4f5bb5a8bc..47d35e56c4df2 100644 --- a/dart/constraint/BalanceConstraint.cpp +++ b/dart/constraint/BalanceConstraint.cpp @@ -255,7 +255,7 @@ void BalanceConstraint::evalGradient(const Eigen::VectorXd& _x, } else { - mNullSpaceCache.setZero(); + mNullSpaceCache.setZero(nDofs, nDofs); } size_t numEE = skel->getNumEndEffectors(); @@ -292,7 +292,7 @@ void BalanceConstraint::evalGradient(const Eigen::VectorXd& _x, } else { - mNullSpaceCache.setZero(); + mNullSpaceCache.setZero(nDofs, nDofs); } for(size_t i=0; i<2; ++i) @@ -321,6 +321,8 @@ void BalanceConstraint::evalGradient(const Eigen::VectorXd& _x, } } } + + convertJacobianMethodOutputToGradient(_grad); } //============================================================================== @@ -400,5 +402,25 @@ void BalanceConstraint::clearCaches() mLastCOM = Eigen::Vector3d::Constant(std::nan("")); } +//============================================================================== +void BalanceConstraint::convertJacobianMethodOutputToGradient( + Eigen::Map& grad) +{ + const dart::dynamics::SkeletonPtr& skel = mIK.lock()->getSkeleton(); + skel->setVelocities(grad); + + mInitialPositionsCache = skel->getPositions(); + + for(size_t i=0; i < skel->getNumJoints(); ++i) + skel->getJoint(i)->integratePositions(1.0); + + // Clear out the velocities so we don't interfere with other Jacobian methods + for(size_t i=0; i < skel->getNumDofs(); ++i) + skel->setVelocity(i, 0.0); + + grad = skel->getPositions(); + grad -= mInitialPositionsCache; +} + } // namespace constraint } // namespace dart diff --git a/dart/constraint/BalanceConstraint.h b/dart/constraint/BalanceConstraint.h index 49b6afe81239f..a764a8ebea0a1 100644 --- a/dart/constraint/BalanceConstraint.h +++ b/dart/constraint/BalanceConstraint.h @@ -145,6 +145,11 @@ class BalanceConstraint : public optimizer::Function, protected: + /// Convert the gradient that gets generated via Jacobian methods into a + /// gradient that can be used by a GradientDescentSolver. + void convertJacobianMethodOutputToGradient( + Eigen::Map& grad); + /// Pointer to the hierarchical IK that owns this Function. Note that this /// Function does not work correctly without a HierarchicalIK. std::weak_ptr mIK; @@ -191,6 +196,10 @@ class BalanceConstraint : public optimizer::Function, /// Cache for an individual null space Eigen::MatrixXd mPartialNullSpaceCache; + + /// Cache used by convertJacobianMethodOutputToGradient to avoid reallocating + /// this vector on each iteration. + Eigen::VectorXd mInitialPositionsCache; }; } // namespace constraint diff --git a/dart/dynamics/BodyNode.cpp b/dart/dynamics/BodyNode.cpp index c91cd39fda42e..ab7da0b998d1c 100644 --- a/dart/dynamics/BodyNode.cpp +++ b/dart/dynamics/BodyNode.cpp @@ -1416,6 +1416,21 @@ void BodyNode::init(const SkeletonPtr& _skeleton) void BodyNode::processNewEntity(Entity* _newChildEntity) { // If the Entity is a JacobianNode, add it to the list of JacobianNodes + + // Dev Note (MXG): There are two places where child JacobianNodes get added. + // This is one place, and the constructor of the JacobianNode class is another + // place. They get added in two different places because: + // 1. This location only works for child BodyNodes. When a non-BodyNode gets + // constructed, its Entity becomes a child of this BodyNode frame during + // the Entity construction, so it cannot be dynamically cast to a + // JacobianNode at that time. But this is not an issue for BodyNodes, + // because BodyNodes become children of this Frame after construction is + // finished. + // 2. The JacobianNode constructor only works for non-BodyNodes. When a + // JacobianNode is being used as a base for a BodyNode, it does not know + // the parent BodyNode. + // + // We should consider doing something to unify the frameworks that currently if(JacobianNode* node = dynamic_cast(_newChildEntity)) mChildJacobianNodes.insert(node); @@ -1451,13 +1466,7 @@ void BodyNode::processRemovedEntity(Entity* _oldChildEntity) mChildBodyNodes.erase(it); if(JacobianNode* node = dynamic_cast(_oldChildEntity)) - { - std::unordered_set::iterator node_it = - mChildJacobianNodes.find(node); - - if(node_it != mChildJacobianNodes.end()) - mChildJacobianNodes.erase(node_it); - } + mChildJacobianNodes.erase(node); if(find(mNonBodyNodeEntities.begin(), mNonBodyNodeEntities.end(), _oldChildEntity) != mNonBodyNodeEntities.end()) diff --git a/dart/dynamics/InverseKinematics.cpp b/dart/dynamics/InverseKinematics.cpp index 1b0e3f7e04ee4..942c501a1447e 100644 --- a/dart/dynamics/InverseKinematics.cpp +++ b/dart/dynamics/InverseKinematics.cpp @@ -93,16 +93,25 @@ bool InverseKinematics::solve(bool _applySolution) bounds[i] = skel->getDof(mDofs[i])->getPositionUpperLimit(); mProblem->setUpperBounds(bounds); + // Many GradientMethod implementations use Joint::integratePositions, so we + // need to clear out any velocities that might be in the Skeleton and then + // reset those velocities later. + Eigen::VectorXd originalVelocities = skel->getVelocities(); + for(size_t i=0; i < skel->getNumDofs(); ++i) + skel->getDof(i)->setVelocity(0.0); + if(_applySolution) { bool wasSolved = mSolver->solve(); setPositions(mProblem->getOptimalSolution()); + skel->setVelocities(originalVelocities); return wasSolved; } Eigen::VectorXd originalPositions = getPositions(); bool wasSolved = mSolver->solve(); setPositions(originalPositions); + skel->setVelocities(originalVelocities); return wasSolved; } @@ -698,6 +707,32 @@ InverseKinematics::GradientMethod::getComponentWeights() const return mGradientP.mComponentWeights; } +//============================================================================== +void InverseKinematics::GradientMethod::convertJacobianMethodOutputToGradient( + Eigen::VectorXd& grad, const std::vector& dofs) +{ + const SkeletonPtr& skel = mIK->getNode()->getSkeleton(); + mInitialPositionsCache = skel->getPositions(dofs); + + for(size_t i=0; i < dofs.size(); ++i) + skel->getDof(dofs[i])->setVelocity(grad[i]); + // Velocities of unused DOFs should already be set to zero. + + for(size_t i=0; i < dofs.size(); ++i) + { + Joint* joint = skel->getDof(dofs[i])->getJoint(); + joint->integratePositions(1.0); + + // Reset this joint's velocities to zero to avoid double-integrating + const size_t numJointDofs = joint->getNumDofs(); + for(size_t j=0; j < numJointDofs; ++j) + joint->setVelocity(j, 0.0); + } + + grad = skel->getPositions(dofs); + grad -= mInitialPositionsCache; +} + //============================================================================== InverseKinematics::GradientMethod::Properties InverseKinematics::GradientMethod::getGradientMethodProperties() const @@ -767,6 +802,7 @@ void InverseKinematics::JacobianDLS::computeGradient( J.transpose()*J).inverse() * J.transpose() * _error; } + convertJacobianMethodOutputToGradient(_grad, mIK->getDofs()); applyWeights(_grad); clampGradient(_grad); } @@ -814,6 +850,7 @@ void InverseKinematics::JacobianTranspose::computeGradient( const math::Jacobian& J = mIK->computeJacobian(); _grad = J.transpose() * _error; + convertJacobianMethodOutputToGradient(_grad, mIK->getDofs()); applyWeights(_grad); clampGradient(_grad); } @@ -991,30 +1028,44 @@ const std::vector& IK::Analytical::getSolutions( } //============================================================================== -static void applyExtraDofGradient(Eigen::VectorXd& grad, - const Eigen::Vector6d& error, - const InverseKinematics* ik, - const std::vector& extraDofs, - const Eigen::VectorXd& compWeights, - double compClamp) +void InverseKinematics::Analytical::addExtraDofGradient( + Eigen::VectorXd& grad, + const Eigen::Vector6d& error, + ExtraDofUtilization_t /*utilization*/) { - const math::Jacobian& J = ik->computeJacobian(); - const std::vector& gradMap = ik->getDofMap(); + mExtraDofGradCache.resize(mExtraDofs.size()); + const math::Jacobian& J = mIK->computeJacobian(); + const std::vector& gradMap = mIK->getDofMap(); - for(size_t i=0; i < extraDofs.size(); ++i) + for(size_t i=0; i < mExtraDofs.size(); ++i) { - size_t depIndex = extraDofs[i]; + size_t depIndex = mExtraDofs[i]; int gradIndex = gradMap[depIndex]; if(gradIndex == -1) continue; - double weight = compWeights.size() > gradIndex ? - compWeights[gradIndex] : 1.0; + mExtraDofGradCache[i] = J.col(gradIndex).transpose()*error; + } - double dq = weight*J.col(gradIndex).transpose()*error; + convertJacobianMethodOutputToGradient(mExtraDofGradCache, mExtraDofs); - if(std::abs(dq) > compClamp) - dq = dq < 0 ? -compClamp : compClamp; + for(size_t i=0; i < mExtraDofs.size(); ++i) + { + size_t depIndex = mExtraDofs[i]; + int gradIndex = gradMap[depIndex]; + if(gradIndex == -1) + continue; + + double weight = mGradientP.mComponentWeights.size() > gradIndex ? + mGradientP.mComponentWeights[gradIndex] : 1.0; + + double dq = weight * mExtraDofGradCache[i]; + + if(std::abs(dq) > mGradientP.mComponentWiseClamp) + { + dq = dq < 0? -mGradientP.mComponentWiseClamp + : mGradientP.mComponentWiseClamp; + } grad[gradIndex] = dq; } @@ -1039,9 +1090,7 @@ void InverseKinematics::Analytical::computeGradient( const Eigen::Vector6d& error = norm > mAnalyticalP.mExtraErrorLengthClamp? mAnalyticalP.mExtraErrorLengthClamp * _error/norm : _error; - applyExtraDofGradient(_grad, error, mIK, mExtraDofs, - mGradientP.mComponentWeights, - mGradientP.mComponentWiseClamp); + addExtraDofGradient(_grad, error, PRE_ANALYTICAL); const std::vector& gradMap = mIK->getDofMap(); for(size_t i=0; i < mExtraDofs.size(); ++i) @@ -1060,7 +1109,6 @@ void InverseKinematics::Analytical::computeGradient( return; const Eigen::VectorXd& bestSolution = mSolutions[0].mConfig; - int bestValidity = mSolutions[0].mValidity; mConfigCache = getPositions(); const std::vector& analyticalToDependent = mDofMap; @@ -1079,8 +1127,7 @@ void InverseKinematics::Analytical::computeGradient( } if(POST_ANALYTICAL == mAnalyticalP.mExtraDofUtilization - && mExtraDofs.size() > 0 - && (bestValidity != VALID) ) + && mExtraDofs.size() > 0 ) { setPositions(bestSolution); @@ -1094,9 +1141,7 @@ void InverseKinematics::Analytical::computeGradient( if(norm > mAnalyticalP.mExtraErrorLengthClamp) postError = mAnalyticalP.mExtraErrorLengthClamp*postError/norm; - applyExtraDofGradient(_grad, postError, mIK, mExtraDofs, - mGradientP.mComponentWeights, - mGradientP.mComponentWiseClamp); + addExtraDofGradient(_grad, postError, POST_ANALYTICAL); } } diff --git a/dart/dynamics/InverseKinematics.h b/dart/dynamics/InverseKinematics.h index d68552987bb41..b097790a3ddbb 100644 --- a/dart/dynamics/InverseKinematics.h +++ b/dart/dynamics/InverseKinematics.h @@ -498,6 +498,18 @@ class InverseKinematics : public common::Subject /// Get the weights of this GradientMethod. const Eigen::VectorXd& getComponentWeights() const; + /// Convert the gradient that gets generated by Jacobian methods into a + /// gradient that can be used by a GradientDescentSolver. + /// + /// Not all Joint types can be integrated using standard addition (e.g. + /// FreeJoint and BallJoint), therefore Jacobian-based differential methods + /// will tend to generate gradients that cannot be correctly used by a + /// simple addition-based GradientDescentSolver. Running your gradient + /// through this function before returning it will make the gradient + /// suitable for a standard solver. + void convertJacobianMethodOutputToGradient( + Eigen::VectorXd& grad, const std::vector& dofs); + /// Get the Properties of this GradientMethod Properties getGradientMethodProperties() const; @@ -522,6 +534,11 @@ class InverseKinematics : public common::Subject /// Properties for this GradientMethod Properties mGradientP; + private: + + /// Cache used by convertJacobianMethodOutputToGradient to avoid + /// reallocating this vector on each iteration. + Eigen::VectorXd mInitialPositionsCache; }; /// JacobianDLS refers to the Damped Least Squares Jacobian Pseudoinverse @@ -660,7 +677,8 @@ class InverseKinematics : public common::Subject { UNUSED = 0, PRE_ANALYTICAL, - POST_ANALYTICAL + POST_ANALYTICAL, + PRE_AND_POST_ANALYTICAL }; struct Solution @@ -817,6 +835,22 @@ class InverseKinematics : public common::Subject protected: + /// This function will compute a gradient which utilizes the extra DOFs + /// that go unused by the Analytical solution and then it will add the + /// components of that gradient to the output parameter: grad. + /// + /// You can override this function to customize how the extra DOFs are used. + /// The default behavior is to use a simple Jacobian Transpose method. + /// + /// The utilization flag will be PRE_ANALYTICAL if the function is being + /// called before the Analytical solution is computed; it will be + /// POST_ANALYTICAL if the function is being called after the Analytical + /// solution is computed. + virtual void addExtraDofGradient( + Eigen::VectorXd& grad, + const Eigen::Vector6d& error, + ExtraDofUtilization_t utilization); + /// Go through the mSolutions vector and tag entries with LIMIT_VIOLATED if /// any components of their configuration are outside of their position /// limits. This will NOT clear the LIMIT_VIOLATED flag from entries of @@ -860,6 +894,9 @@ class InverseKinematics : public common::Subject /// later. This is different from mConfigCache which will not be used to /// restore the configuration. Eigen::VectorXd mRestoreConfigCache; + + /// A cache for storing the gradient for the extra DOFs + Eigen::VectorXd mExtraDofGradCache; }; /// If this IK module is set to active, then it will be utilized by any diff --git a/dart/dynamics/JacobianNode.cpp b/dart/dynamics/JacobianNode.cpp index 8be5a1eccb368..4a6a634861084 100644 --- a/dart/dynamics/JacobianNode.cpp +++ b/dart/dynamics/JacobianNode.cpp @@ -45,7 +45,10 @@ namespace dynamics { // This destructor needs to be defined somewhere that the definition of // InverseKinematics is visible, because it's needed by the // std::unique_ptr class member -JacobianNode::~JacobianNode() = default; +JacobianNode::~JacobianNode() +{ + mBodyNode->mChildJacobianNodes.erase(this); +} //============================================================================== const std::shared_ptr& @@ -92,7 +95,8 @@ JacobianNode::JacobianNode(BodyNode* bn) mIsBodyJacobianSpatialDerivDirty(true), mIsWorldJacobianClassicDerivDirty(true) { - // Do nothing + if(this != bn) + bn->mChildJacobianNodes.insert(this); } //============================================================================== diff --git a/dart/dynamics/detail/TemplatedJacobianNode.h b/dart/dynamics/detail/TemplatedJacobianNode.h index f2cff60f404a6..806cad590e03f 100644 --- a/dart/dynamics/detail/TemplatedJacobianNode.h +++ b/dart/dynamics/detail/TemplatedJacobianNode.h @@ -37,11 +37,11 @@ #ifndef DART_DYNAMICS_DETAIL_TEMPLATEDJACOBIAN_H_ #define DART_DYNAMICS_DETAIL_TEMPLATEDJACOBIAN_H_ +#include "dart/dynamics/TemplatedJacobianNode.h" + namespace dart { namespace dynamics { -#include "dart/dynamics/TemplatedJacobianNode.h" - //============================================================================== template math::Jacobian diff --git a/dart/gui/osg/examples/osgHuboPuppet.cpp b/dart/gui/osg/examples/osgHuboPuppet.cpp index 07db569a1228a..5b8b354774b8c 100644 --- a/dart/gui/osg/examples/osgHuboPuppet.cpp +++ b/dart/gui/osg/examples/osgHuboPuppet.cpp @@ -1172,7 +1172,7 @@ void setupEndEffectors(const SkeletonPtr& hubo) l_hand->getIK()->setGradientMethod("Body_LSP"); l_hand->getIK()->getAnalytical()->setExtraDofUtilization( - IK::Analytical::PRE_ANALYTICAL); + IK::Analytical::POST_ANALYTICAL); l_hand->getIK()->getAnalytical()->setExtraErrorLengthClamp(extra_error_clamp); @@ -1198,7 +1198,7 @@ void setupEndEffectors(const SkeletonPtr& hubo) r_hand->getIK()->setGradientMethod("Body_RSP"); r_hand->getIK()->getAnalytical()->setExtraDofUtilization( - IK::Analytical::PRE_ANALYTICAL); + IK::Analytical::POST_ANALYTICAL); r_hand->getIK()->getAnalytical()->setExtraErrorLengthClamp(extra_error_clamp); diff --git a/unittests/testForwardKinematics.cpp b/unittests/testForwardKinematics.cpp index b675a2008c48f..422d1f490e436 100644 --- a/unittests/testForwardKinematics.cpp +++ b/unittests/testForwardKinematics.cpp @@ -130,7 +130,8 @@ TEST(FORWARD_KINEMATICS, TWO_ROLLS) Eigen::MatrixXd finiteDifferenceJacobian( const SkeletonPtr& skeleton, const Eigen::VectorXd& q, - const std::vector& active_indices) + const std::vector& active_indices, + JacobianNode* node) { Eigen::MatrixXd J(3, q.size()); for(int i=0; i < q.size(); ++i) @@ -143,16 +144,14 @@ Eigen::MatrixXd finiteDifferenceJacobian( q_up[i] += 0.5*dq; q_down[i] -= 0.5*dq; - BodyNode* last_bn = skeleton->getBodyNode(skeleton->getNumBodyNodes()-1); - skeleton->setPositions(active_indices, q_up); - Eigen::Vector3d x_up = last_bn->getTransform().translation(); + Eigen::Vector3d x_up = node->getTransform().translation(); skeleton->setPositions(active_indices, q_down); - Eigen::Vector3d x_down = last_bn->getTransform().translation(); + Eigen::Vector3d x_down = node->getTransform().translation(); skeleton->setPositions(active_indices, q); - J.col(i) = last_bn->getWorldTransform().linear().transpose() * (x_up - x_down) / dq; + J.col(i) = node->getWorldTransform().linear().transpose() * (x_up - x_down) / dq; } return J; @@ -162,12 +161,12 @@ Eigen::MatrixXd finiteDifferenceJacobian( Eigen::MatrixXd standardJacobian( const SkeletonPtr& skeleton, const Eigen::VectorXd& q, - const std::vector& active_indices) + const std::vector& active_indices, + JacobianNode* node) { skeleton->setPositions(active_indices, q); - BodyNode* last_bn = skeleton->getBodyNode(skeleton->getNumBodyNodes()-1); - Eigen::MatrixXd J = skeleton->getJacobian(last_bn).bottomRows<3>(); + Eigen::MatrixXd J = skeleton->getJacobian(node).bottomRows<3>(); Eigen::MatrixXd reduced_J(3, q.size()); for(int i=0; i < q.size(); ++i) @@ -193,14 +192,63 @@ TEST(FORWARD_KINEMATICS, JACOBIAN_PARTIAL_CHANGE) active_indices.push_back(i); Eigen::VectorXd q = Eigen::VectorXd::Random(active_indices.size()); - Eigen::MatrixXd fd_J = finiteDifferenceJacobian(skeleton1, q, active_indices); - Eigen::MatrixXd J = standardJacobian(skeleton2, q, active_indices); + + Eigen::MatrixXd fd_J = finiteDifferenceJacobian( + skeleton1, q, active_indices, + skeleton1->getBodyNode(skeleton1->getNumBodyNodes()-1)); + + Eigen::MatrixXd J = standardJacobian( + skeleton2, q, active_indices, + skeleton2->getBodyNode(skeleton2->getNumBodyNodes()-1)); + + EXPECT_TRUE((fd_J - J).norm() < tolerance); + + q = Eigen::VectorXd::Random(active_indices.size()); + + fd_J = finiteDifferenceJacobian( + skeleton1, q, active_indices, + skeleton1->getBodyNode(skeleton1->getNumBodyNodes()-1)); + + J = standardJacobian( + skeleton2, q, active_indices, + skeleton2->getBodyNode(skeleton2->getNumBodyNodes()-1)); + + EXPECT_TRUE((fd_J - J).norm() < tolerance); +} + +//============================================================================== +TEST(FORWARD_KINEMATICS, JACOBIAN_END_EFFECTOR_CHANGE) +{ + // This is a regression test for issue #499 + const double tolerance = 1e-8; + + dart::utils::DartLoader loader; + SkeletonPtr skeleton1 = + loader.parseSkeleton(DART_DATA_PATH"urdf/KR5/KR5 sixx R650.urdf"); + + BodyNode* last_bn1 = skeleton1->getBodyNode(skeleton1->getNumBodyNodes()-1); + EndEffector* ee1 = last_bn1->createEndEffector(); + + SkeletonPtr skeleton2 = skeleton1->clone(); + BodyNode* last_bn2 = skeleton2->getBodyNode(skeleton2->getNumBodyNodes()-1); + EndEffector* ee2 = last_bn2->createEndEffector(); + + std::vector active_indices; + for(size_t i=0; i < 3; ++i) + active_indices.push_back(i); + + Eigen::VectorXd q = Eigen::VectorXd::Random(active_indices.size()); + + Eigen::MatrixXd fd_J = finiteDifferenceJacobian( + skeleton1, q, active_indices, ee1); + + Eigen::MatrixXd J = standardJacobian(skeleton2, q, active_indices, ee2); EXPECT_TRUE((fd_J - J).norm() < tolerance); q = Eigen::VectorXd::Random(active_indices.size()); - fd_J = finiteDifferenceJacobian(skeleton1, q, active_indices); - J = standardJacobian(skeleton2, q, active_indices); + fd_J = finiteDifferenceJacobian(skeleton1, q, active_indices, ee1); + J = standardJacobian(skeleton2, q, active_indices, ee2); EXPECT_TRUE((fd_J - J).norm() < tolerance); } From 7ba63d3e9af8d9d64d6de807322da9f1fd83b6f4 Mon Sep 17 00:00:00 2001 From: "M.X. Grey" Date: Fri, 22 Apr 2016 10:18:22 -0400 Subject: [PATCH 2/4] Finish the comment --- dart/dynamics/BodyNode.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/dart/dynamics/BodyNode.cpp b/dart/dynamics/BodyNode.cpp index ab7da0b998d1c..1b222e31cee1a 100644 --- a/dart/dynamics/BodyNode.cpp +++ b/dart/dynamics/BodyNode.cpp @@ -1430,7 +1430,8 @@ void BodyNode::processNewEntity(Entity* _newChildEntity) // JacobianNode is being used as a base for a BodyNode, it does not know // the parent BodyNode. // - // We should consider doing something to unify the frameworks that currently + // We should consider doing something to unify these two pipelines that are + // currently independent of each other. if(JacobianNode* node = dynamic_cast(_newChildEntity)) mChildJacobianNodes.insert(node); From 29769639df057ef30bc812df8fbc3d9675de64a5 Mon Sep 17 00:00:00 2001 From: Jeongseok Lee Date: Sun, 24 Apr 2016 11:46:37 -0400 Subject: [PATCH 3/4] Add `std::` to size_t and find() --- dart/constraint/BalanceConstraint.cpp | 4 ++-- dart/dynamics/BodyNode.cpp | 14 +++++++------- dart/dynamics/InverseKinematics.cpp | 16 ++++++++-------- dart/dynamics/InverseKinematics.h | 2 +- unittests/testForwardKinematics.cpp | 4 ++-- 5 files changed, 20 insertions(+), 20 deletions(-) diff --git a/dart/constraint/BalanceConstraint.cpp b/dart/constraint/BalanceConstraint.cpp index 382d35ae712d7..ae340c611e372 100644 --- a/dart/constraint/BalanceConstraint.cpp +++ b/dart/constraint/BalanceConstraint.cpp @@ -411,11 +411,11 @@ void BalanceConstraint::convertJacobianMethodOutputToGradient( mInitialPositionsCache = skel->getPositions(); - for(size_t i=0; i < skel->getNumJoints(); ++i) + for(std::size_t i=0; i < skel->getNumJoints(); ++i) skel->getJoint(i)->integratePositions(1.0); // Clear out the velocities so we don't interfere with other Jacobian methods - for(size_t i=0; i < skel->getNumDofs(); ++i) + for(std::size_t i=0; i < skel->getNumDofs(); ++i) skel->setVelocity(i, 0.0); grad = skel->getPositions(); diff --git a/dart/dynamics/BodyNode.cpp b/dart/dynamics/BodyNode.cpp index aa4442ec6fba7..f77b7c442d725 100644 --- a/dart/dynamics/BodyNode.cpp +++ b/dart/dynamics/BodyNode.cpp @@ -1440,8 +1440,8 @@ void BodyNode::processNewEntity(Entity* _newChildEntity) // Check if it's a child BodyNode (if not, then it's just some other arbitrary // type of Entity) - if(find(mChildBodyNodes.begin(), mChildBodyNodes.end(), _newChildEntity) != - mChildBodyNodes.end()) + if(std::find(mChildBodyNodes.begin(), mChildBodyNodes.end(), + _newChildEntity) != mChildBodyNodes.end()) return; // Check if it's already accounted for in our Non-BodyNode Entities @@ -1460,17 +1460,17 @@ void BodyNode::processNewEntity(Entity* _newChildEntity) //============================================================================== void BodyNode::processRemovedEntity(Entity* _oldChildEntity) { - std::vector::iterator it = find(mChildBodyNodes.begin(), - mChildBodyNodes.end(), - _oldChildEntity); + std::vector::iterator it = std::find(mChildBodyNodes.begin(), + mChildBodyNodes.end(), + _oldChildEntity); if(it != mChildBodyNodes.end()) mChildBodyNodes.erase(it); if(JacobianNode* node = dynamic_cast(_oldChildEntity)) mChildJacobianNodes.erase(node); - if(find(mNonBodyNodeEntities.begin(), mNonBodyNodeEntities.end(), - _oldChildEntity) != mNonBodyNodeEntities.end()) + if(std::find(mNonBodyNodeEntities.begin(), mNonBodyNodeEntities.end(), + _oldChildEntity) != mNonBodyNodeEntities.end()) mNonBodyNodeEntities.erase(_oldChildEntity); } diff --git a/dart/dynamics/InverseKinematics.cpp b/dart/dynamics/InverseKinematics.cpp index 1bb65bf7454fa..b6f64df013d9f 100644 --- a/dart/dynamics/InverseKinematics.cpp +++ b/dart/dynamics/InverseKinematics.cpp @@ -97,7 +97,7 @@ bool InverseKinematics::solve(bool _applySolution) // need to clear out any velocities that might be in the Skeleton and then // reset those velocities later. Eigen::VectorXd originalVelocities = skel->getVelocities(); - for(size_t i=0; i < skel->getNumDofs(); ++i) + for(std::size_t i=0; i < skel->getNumDofs(); ++i) skel->getDof(i)->setVelocity(0.0); if(_applySolution) @@ -709,23 +709,23 @@ InverseKinematics::GradientMethod::getComponentWeights() const //============================================================================== void InverseKinematics::GradientMethod::convertJacobianMethodOutputToGradient( - Eigen::VectorXd& grad, const std::vector& dofs) + Eigen::VectorXd& grad, const std::vector& dofs) { const SkeletonPtr& skel = mIK->getNode()->getSkeleton(); mInitialPositionsCache = skel->getPositions(dofs); - for(size_t i=0; i < dofs.size(); ++i) + for(std::size_t i=0; i < dofs.size(); ++i) skel->getDof(dofs[i])->setVelocity(grad[i]); // Velocities of unused DOFs should already be set to zero. - for(size_t i=0; i < dofs.size(); ++i) + for(std::size_t i=0; i < dofs.size(); ++i) { Joint* joint = skel->getDof(dofs[i])->getJoint(); joint->integratePositions(1.0); // Reset this joint's velocities to zero to avoid double-integrating - const size_t numJointDofs = joint->getNumDofs(); - for(size_t j=0; j < numJointDofs; ++j) + const std::size_t numJointDofs = joint->getNumDofs(); + for(std::size_t j=0; j < numJointDofs; ++j) joint->setVelocity(j, 0.0); } @@ -1049,9 +1049,9 @@ void InverseKinematics::Analytical::addExtraDofGradient( convertJacobianMethodOutputToGradient(mExtraDofGradCache, mExtraDofs); - for(size_t i=0; i < mExtraDofs.size(); ++i) + for(std::size_t i=0; i < mExtraDofs.size(); ++i) { - size_t depIndex = mExtraDofs[i]; + std::size_t depIndex = mExtraDofs[i]; int gradIndex = gradMap[depIndex]; if(gradIndex == -1) continue; diff --git a/dart/dynamics/InverseKinematics.h b/dart/dynamics/InverseKinematics.h index 5e47ae1c6437a..ed05e7a74e8fb 100644 --- a/dart/dynamics/InverseKinematics.h +++ b/dart/dynamics/InverseKinematics.h @@ -508,7 +508,7 @@ class InverseKinematics : public common::Subject /// through this function before returning it will make the gradient /// suitable for a standard solver. void convertJacobianMethodOutputToGradient( - Eigen::VectorXd& grad, const std::vector& dofs); + Eigen::VectorXd& grad, const std::vector& dofs); /// Get the Properties of this GradientMethod Properties getGradientMethodProperties() const; diff --git a/unittests/testForwardKinematics.cpp b/unittests/testForwardKinematics.cpp index 698508b80f8ab..32029b7f26259 100644 --- a/unittests/testForwardKinematics.cpp +++ b/unittests/testForwardKinematics.cpp @@ -233,8 +233,8 @@ TEST(FORWARD_KINEMATICS, JACOBIAN_END_EFFECTOR_CHANGE) BodyNode* last_bn2 = skeleton2->getBodyNode(skeleton2->getNumBodyNodes()-1); EndEffector* ee2 = last_bn2->createEndEffector(); - std::vector active_indices; - for(size_t i=0; i < 3; ++i) + std::vector active_indices; + for(std::size_t i=0; i < 3; ++i) active_indices.push_back(i); Eigen::VectorXd q = Eigen::VectorXd::Random(active_indices.size()); From e6b8ff93158da120939ab5c68678a9a5f6baa89d Mon Sep 17 00:00:00 2001 From: "M.X. Grey" Date: Mon, 25 Apr 2016 12:51:50 -0400 Subject: [PATCH 4/4] Made changes based on Reviewable comments --- dart/dynamics/InverseKinematics.cpp | 12 +- dart/dynamics/InverseKinematics.h | 1680 ++++++++++++++------------- unittests/testForwardKinematics.cpp | 2 +- 3 files changed, 860 insertions(+), 834 deletions(-) diff --git a/dart/dynamics/InverseKinematics.cpp b/dart/dynamics/InverseKinematics.cpp index b6f64df013d9f..d2679e3840649 100644 --- a/dart/dynamics/InverseKinematics.cpp +++ b/dart/dynamics/InverseKinematics.cpp @@ -95,7 +95,7 @@ bool InverseKinematics::solve(bool _applySolution) // Many GradientMethod implementations use Joint::integratePositions, so we // need to clear out any velocities that might be in the Skeleton and then - // reset those velocities later. + // reset those velocities later. This has been opened as issue #699. Eigen::VectorXd originalVelocities = skel->getVelocities(); for(std::size_t i=0; i < skel->getNumDofs(); ++i) skel->getDof(i)->setVelocity(0.0); @@ -866,7 +866,7 @@ InverseKinematics::Analytical::Solution::Solution( //============================================================================== InverseKinematics::Analytical::UniqueProperties::UniqueProperties( - ExtraDofUtilization_t extraDofUtilization, double extraErrorLengthClamp) + ExtraDofUtilization extraDofUtilization, double extraErrorLengthClamp) : mExtraDofUtilization(extraDofUtilization), mExtraErrorLengthClamp(extraErrorLengthClamp) { @@ -875,7 +875,7 @@ InverseKinematics::Analytical::UniqueProperties::UniqueProperties( //============================================================================== InverseKinematics::Analytical::UniqueProperties::UniqueProperties( - ExtraDofUtilization_t extraDofUtilization, + ExtraDofUtilization extraDofUtilization, double extraErrorLengthClamp, QualityComparison qualityComparator) : mExtraDofUtilization(extraDofUtilization), @@ -1031,7 +1031,7 @@ const std::vector& IK::Analytical::getSolutions( void InverseKinematics::Analytical::addExtraDofGradient( Eigen::VectorXd& grad, const Eigen::Vector6d& error, - ExtraDofUtilization_t /*utilization*/) + ExtraDofUtilization /*utilization*/) { mExtraDofGradCache.resize(mExtraDofs.size()); const math::Jacobian& J = mIK->computeJacobian(); @@ -1160,13 +1160,13 @@ Eigen::VectorXd InverseKinematics::Analytical::getPositions() const //============================================================================== void InverseKinematics::Analytical::setExtraDofUtilization( - ExtraDofUtilization_t _utilization) + ExtraDofUtilization _utilization) { mAnalyticalP.mExtraDofUtilization = _utilization; } //============================================================================== -IK::Analytical::ExtraDofUtilization_t +IK::Analytical::ExtraDofUtilization IK::Analytical::getExtraDofUtilization() const { return mAnalyticalP.mExtraDofUtilization; diff --git a/dart/dynamics/InverseKinematics.h b/dart/dynamics/InverseKinematics.h index ed05e7a74e8fb..bd2f47307bb10 100644 --- a/dart/dynamics/InverseKinematics.h +++ b/dart/dynamics/InverseKinematics.h @@ -146,758 +146,18 @@ class InverseKinematics : public common::Subject /// by pointer instead of being cloned. InverseKinematicsPtr clone(JacobianNode* _newNode) const; - /// This class should be inherited by optimizer::Function classes that have a - /// dependency on the InverseKinematics module that they belong to. If you - /// pass an InverseKinematics::Function into the Problem of an - /// InverseKinematics module, then it will be properly cloned whenever the - /// InverseKinematics module that it belongs to gets cloned. Any Function - /// classes in the Problem that do not inherit InverseKinematics::Function - /// will just be copied over by reference. - class Function - { - public: - - /// Enable this function to be cloned to a new IK module. - virtual optimizer::FunctionPtr clone(InverseKinematics* _newIK) const = 0; - - /// Virtual destructor - virtual ~Function() = default; - }; - - /// ErrorMethod is a base class for different ways of computing the error of - /// an InverseKinematics module. - class ErrorMethod : public common::Subject - { - public: - - typedef std::pair Bounds; - - /// The Properties struct contains settings that are commonly used by - /// methods that compute error for inverse kinematics. - struct Properties - { - /// Default constructor - Properties(const Bounds& _bounds = - Bounds(Eigen::Vector6d::Constant(-DefaultIKTolerance), - Eigen::Vector6d::Constant( DefaultIKTolerance)), - - double _errorClamp = DefaultIKErrorClamp, - - const Eigen::Vector6d& _errorWeights = Eigen::compose( - Eigen::Vector3d::Constant(DefaultIKAngularWeight), - Eigen::Vector3d::Constant(DefaultIKLinearWeight))); - - /// Bounds that define the acceptable range of the Node's transform - /// relative to its target frame. - std::pair mBounds; - - /// The error vector will be clamped to this length with each iteration. - /// This is used to enforce sane behavior, even when there are extremely - /// large error vectors. - double mErrorLengthClamp; - - /// These weights will be applied to the error vector component-wise. This - /// allows you to set some components of error as more important than - /// others, or to scale their coordinate spaces. For example, you will - /// often want the first three components (orientation error) to have - /// smaller weights than the last three components (translation error). - Eigen::Vector6d mErrorWeights; - - // To get byte-aligned Eigen vectors - EIGEN_MAKE_ALIGNED_OPERATOR_NEW - }; - - /// Constructor - ErrorMethod(InverseKinematics* _ik, - const std::string& _methodName, - const Properties& _properties = Properties()); - - /// Virtual destructor - virtual ~ErrorMethod() = default; - - /// Enable this ErrorMethod to be cloned to a new IK module. - virtual std::unique_ptr clone( - InverseKinematics* _newIK) const = 0; - - /// Override this function with your implementation of the error vector - /// computation. The expectation is that the first three components of the - /// vector will correspond to orientation error (in an angle-axis format) - /// while the last three components correspond to translational error. - /// - /// When implementing this function, you should assume that the Skeleton's - /// current joint positions corresponds to the positions that you - /// must use to compute the error. This function will only get called when - /// an update is needed. - virtual Eigen::Vector6d computeError() = 0; - - /// Override this function with your implementation of computing the desired - /// given the current transform and error vector. If you want the desired - /// transform to always be equal to the Target's transform, you can simply - /// call ErrorMethod::computeDesiredTransform to implement this function. - virtual Eigen::Isometry3d computeDesiredTransform( - const Eigen::Isometry3d& _currentTf, const Eigen::Vector6d& _error) = 0; - - /// This function is used to handle caching the error vector. - const Eigen::Vector6d& evalError(const Eigen::VectorXd& _q); - - /// Get the name of this ErrorMethod. - const std::string& getMethodName() const; - - /// Set all the error bounds. - void setBounds( - const Eigen::Vector6d& _lower = - Eigen::Vector6d::Constant(-DefaultIKTolerance), - const Eigen::Vector6d& _upper = - Eigen::Vector6d::Constant( DefaultIKTolerance)); - - /// Set all the error bounds. - void setBounds(const std::pair& _bounds); - - /// Get all the error bounds. - const std::pair& getBounds() const; - - /// Set the error bounds for orientation. - void setAngularBounds( - const Eigen::Vector3d& _lower = - Eigen::Vector3d::Constant(-DefaultIKTolerance), - const Eigen::Vector3d& _upper = - Eigen::Vector3d::Constant( DefaultIKTolerance)); - - /// Set the error bounds for orientation. - void setAngularBounds( - const std::pair& _bounds); - - /// Get the error bounds for orientation. - std::pair getAngularBounds() const; - - /// Set the error bounds for translation. - void setLinearBounds( - const Eigen::Vector3d& _lower = - Eigen::Vector3d::Constant(-DefaultIKTolerance), - const Eigen::Vector3d& _upper = - Eigen::Vector3d::Constant( DefaultIKTolerance)); - - /// Set the error bounds for translation. - void setLinearBounds( - const std::pair& _bounds); - - /// Get the error bounds for translation. - std::pair getLinearBounds() const; - - /// Set the clamp that will be applied to the length of the error vector - /// each iteration. - void setErrorLengthClamp(double _clampSize = DefaultIKErrorClamp); - - /// Set the clamp that will be applied to the length of the error vector - /// each iteration. - double getErrorLengthClamp() const; - - /// Set the weights that will be applied to each component of the error - /// vector. - void setErrorWeights(const Eigen::Vector6d& _weights); - - /// Get the weights that will be applied to each component of the error - /// vector. - const Eigen::Vector6d& getErrorWeights() const; - - /// Set the weights that will be applied to each angular component of the - /// error vector. - void setAngularErrorWeights( - const Eigen::Vector3d& _weights = - Eigen::Vector3d::Constant(DefaultIKAngularWeight)); - - /// Get the weights that will be applied to each angular component of the - /// error vector. - Eigen::Vector3d getAngularErrorWeights() const; - - /// Set the weights that will be applied to each linear component of the - /// error vector. - void setLinearErrorWeights( - const Eigen::Vector3d& _weights = - Eigen::Vector3d::Constant(DefaultIKLinearWeight)); - - /// Get the weights that will be applied to each linear component of the - /// error vector. - Eigen::Vector3d getLinearErrorWeights() const; - - /// Get the Properties of this ErrorMethod - Properties getErrorMethodProperties() const; - - /// Clear the cache to force the error to be recomputed. It should generally - /// not be necessary to call this function. - void clearCache(); - - protected: - - /// Pointer to the IK module of this ErrorMethod - common::sub_ptr mIK; - - /// Name of this error method - std::string mMethodName; - - /// The last joint positions passed into this ErrorMethod - Eigen::VectorXd mLastPositions; - - /// The last error vector computed by this ErrorMethod - Eigen::Vector6d mLastError; - - /// The properties of this ErrorMethod - Properties mErrorP; - - public: - // To get byte-aligned Eigen vectors - EIGEN_MAKE_ALIGNED_OPERATOR_NEW - - }; - - /// The TaskSpaceRegion is a nicely generalized method for computing the error - /// of an IK Problem. - class TaskSpaceRegion : public ErrorMethod - { - public: - - struct UniqueProperties - { - /// Setting this to true (which is default) will tell it to compute the - /// error based on the center of the Task Space Region instead of the edge - /// of the Task Space Region. This often results in faster convergence, as - /// the Node will enter the Task Space Region more aggressively. - /// - /// Once the Node is inside the Task Space Region, the error vector will - /// drop to zero, regardless of whether this flag is true or false. - bool mComputeErrorFromCenter; - - /// Default constructor - UniqueProperties(bool computeErrorFromCenter = true); - }; - - struct Properties : ErrorMethod::Properties, UniqueProperties - { - /// Default constructor - Properties( - const ErrorMethod::Properties& errorProperties = - ErrorMethod::Properties(), - const UniqueProperties& taskSpaceProperties = UniqueProperties()); - }; - - /// Default Constructor - explicit TaskSpaceRegion(InverseKinematics* _ik, - const Properties& _properties = Properties()); - - /// Virtual destructor - virtual ~TaskSpaceRegion() = default; - - // Documentation inherited - std::unique_ptr clone(InverseKinematics* _newIK) const override; - - // Documentation inherited - Eigen::Isometry3d computeDesiredTransform( - const Eigen::Isometry3d& _currentTf, - const Eigen::Vector6d& _error) override; - - // Documentation inherited - Eigen::Vector6d computeError() override; - - /// Set whether this TaskSpaceRegion should compute its error vector from - /// the center of the region. - void setComputeFromCenter(bool computeFromCenter); - - /// Get whether this TaskSpaceRegion is set to compute its error vector from - /// the center of the region. - bool isComputingFromCenter() const; - - /// Get the Properties of this TaskSpaceRegion - Properties getTaskSpaceRegionProperties() const; - - protected: - - /// Properties of this TaskSpaceRegion - UniqueProperties mTaskSpaceP; + // For the definitions of these classes, see the bottom of this header + class Function; - }; - - /// GradientMethod is a base class for different ways of computing the - /// gradient of an InverseKinematics module. - class GradientMethod : public common::Subject - { - public: - - struct Properties - { - /// The component-wise clamp for this GradientMethod - double mComponentWiseClamp; - - /// The weights for this GradientMethod - Eigen::VectorXd mComponentWeights; - - /// Default constructor - Properties(double clamp = DefaultIKGradientComponentClamp, - const Eigen::VectorXd& weights = Eigen::VectorXd()); - }; - - /// Constructor - GradientMethod(InverseKinematics* _ik, - const std::string& _methodName, - const Properties& _properties); - - /// Virtual destructor - virtual ~GradientMethod() = default; - - /// Enable this GradientMethod to be cloned to a new IK module - virtual std::unique_ptr clone( - InverseKinematics* _newIK) const = 0; - - /// Override this function with your implementation of the gradient - /// computation. The direction that this gradient points in should make the - /// error **worse** if applied to the joint positions, because the - /// Problem is configured as a gradient **descent** error minimization - /// Problem. - /// - /// The error vector that is passed in will be determined by the IK module's - /// ErrorMethod. The expectation is that the first three components of the - /// vector correspond to orientation error (in an angle-axis format) while - /// the last three components correspond to translational error. - /// - /// When implementing this function, you should assume that the Skeleton's - /// current joint positions corresponds to the positions that you - /// must use to compute the error. This function will only get called when - /// an update is needed. - virtual void computeGradient(const Eigen::Vector6d& _error, - Eigen::VectorXd& _grad) = 0; - - /// This function is used to handle caching the gradient vector and - /// interfacing with the solver. - void evalGradient(const Eigen::VectorXd& _q, - Eigen::Map _grad); - - /// Get the name of this GradientMethod. - const std::string& getMethodName() const; - - /// Clamp the gradient based on the clamp settings of this GradientMethod. - void clampGradient(Eigen::VectorXd& _grad) const; - - /// Set the component-wise clamp for this GradientMethod. Each component - /// of the gradient will be individually clamped to this size. - void setComponentWiseClamp(double _clamp = DefaultIKGradientComponentClamp); - - /// Get the component-wise clamp for this GradientMethod. - double getComponentWiseClamp() const; - - /// Apply weights to the gradient based on the weight settings of this - /// GradientMethod. - void applyWeights(Eigen::VectorXd& _grad) const; - - /// Set the weights that will be applied to each component of the gradient. - /// If the number of components in _weights is smaller than the number of - /// components in the gradient, then a weight of 1.0 will be applied to all - /// components that are out of the range of _weights. Passing in an empty - /// vector for _weights will effectively make all the gradient components - /// unweighted. - void setComponentWeights(const Eigen::VectorXd& _weights); - - /// Get the weights of this GradientMethod. - const Eigen::VectorXd& getComponentWeights() const; - - /// Convert the gradient that gets generated by Jacobian methods into a - /// gradient that can be used by a GradientDescentSolver. - /// - /// Not all Joint types can be integrated using standard addition (e.g. - /// FreeJoint and BallJoint), therefore Jacobian-based differential methods - /// will tend to generate gradients that cannot be correctly used by a - /// simple addition-based GradientDescentSolver. Running your gradient - /// through this function before returning it will make the gradient - /// suitable for a standard solver. - void convertJacobianMethodOutputToGradient( - Eigen::VectorXd& grad, const std::vector& dofs); - - /// Get the Properties of this GradientMethod - Properties getGradientMethodProperties() const; - - /// Clear the cache to force the gradient to be recomputed. It should - /// generally not be necessary to call this function. - void clearCache(); - - protected: - - /// The IK module that this GradientMethod belongs to. - common::sub_ptr mIK; - - /// The name of this method - std::string mMethodName; - - /// The last positions that was passed to this GradientMethod - Eigen::VectorXd mLastPositions; - - /// The last gradient that was computed by this GradientMethod - Eigen::VectorXd mLastGradient; - - /// Properties for this GradientMethod - Properties mGradientP; - - private: - - /// Cache used by convertJacobianMethodOutputToGradient to avoid - /// reallocating this vector on each iteration. - Eigen::VectorXd mInitialPositionsCache; - }; - - /// JacobianDLS refers to the Damped Least Squares Jacobian Pseudoinverse - /// (specifically, Moore-Penrose Pseudoinverse). This is a very precise method - /// for computing the gradient and is especially suitable for performing IK on - /// industrial manipulators that need to follow very exact workspace paths. - /// However, it is vulnerable to be jittery around singularities (though the - /// damping helps with this), and each cycle might take more time to compute - /// than the JacobianTranspose method (although the JacobianDLS method will - /// usually converge in fewer cycles than JacobianTranspose). - class JacobianDLS : public GradientMethod - { - public: + // Methods for computing IK error + class ErrorMethod; + class TaskSpaceRegion; - struct UniqueProperties - { - /// Damping coefficient - double mDamping; - - /// Default constructor - UniqueProperties(double damping = DefaultIKDLSCoefficient); - }; - - struct Properties : GradientMethod::Properties, UniqueProperties - { - /// Default constructor - Properties( - const GradientMethod::Properties& gradientProperties = - GradientMethod::Properties(), - const UniqueProperties& dlsProperties = UniqueProperties()); - }; - - /// Constructor - explicit JacobianDLS(InverseKinematics* _ik, - const Properties& properties = Properties()); - - /// Virtual destructor - virtual ~JacobianDLS() = default; - - // Documentation inherited - std::unique_ptr clone( - InverseKinematics* _newIK) const override; - - // Documentation inherited - void computeGradient(const Eigen::Vector6d& _error, - Eigen::VectorXd& _grad) override; - - /// Set the damping coefficient. A higher damping coefficient will smooth - /// out behavior around singularities but will also result in less precision - /// in general. The default value is appropriate for most use cases. - void setDampingCoefficient(double _damping = DefaultIKDLSCoefficient); - - /// Get the damping coefficient. - double getDampingCoefficient() const; - - /// Get the Properties of this JacobianDLS - Properties getJacobianDLSProperties() const; - - protected: - - /// Properties of this Damped Least Squares method - UniqueProperties mDLSProperties; - - }; - - /// JacobianTranspose will simply apply the transpose of the Jacobian to the - /// error vector in order to compute the gradient. This method tends to be - /// very smooth but imprecise, requiring more iterations before converging - /// and being less precise in general. This method is suitable for animations - /// where smoothness is prefered over precision. - class JacobianTranspose : public GradientMethod - { - public: - - /// Constructor - explicit JacobianTranspose(InverseKinematics* _ik, - const Properties& properties = Properties()); - - /// Virtual destructor - virtual ~JacobianTranspose() = default; - - // Documentation inherited - std::unique_ptr clone( - InverseKinematics* _newIK) const override; - - // Documentation inherited - void computeGradient(const Eigen::Vector6d& _error, - Eigen::VectorXd& _grad) override; - }; - - /// Analytical is a base class that should be inherited by methods that are - /// made to solve the IK analytically instead of iteratively. This provides an - /// extended API that is relevant to Analytical solvers but not iterative - /// solvers. - /// - /// Creating an Analytical solver will have the side effect of removing the - /// error clamp and error weights from your ErrorMethod. If you still want - /// your error computations to be clamped and weighted, you should set it - /// again after creating the Analytical solver. Clamping and weighting the - /// error vector often helps iterative methods to converge smoothly, but it is - /// counter-productive for analytical methods which do not typically rely on - /// convergence; analytical methods can usually solve the entire error vector - /// directly. - class Analytical : public GradientMethod - { - public: - - /// Bitwise enumerations that are used to describe some properties of each - /// solution produced by the analytical IK. - enum Validity_t - { - VALID = 0, ///< The solution is completely valid and reaches the target - OUT_OF_REACH = 1 << 0, ///< The solution does not reach the target - LIMIT_VIOLATED = 1 << 1 ///< The solution has one or more joint positions that violate the joint limits - }; - - /// If there are extra DOFs in the IK module which your Analytical solver - /// implementation does not make use of, those DOFs can be used to - /// supplement the analytical solver using Jacobian transpose iteration. - /// This enumeration is used to indicate whether you want those DOFs to be - /// used before applying the analytical solution, after applying the - /// analytical solution, or not be used at all. - /// - /// Jacobian transpose is used for the extra DOFs because it is inexpensive - /// and robust to degenerate Jacobians which are common in low dimensional - /// joint spaces. The primary advantage of pseudoinverse methods over - /// Jacobian transpose methods is their precision, but analytical methods - /// are even more precise than pseudoinverse methods, so that precision is - /// not needed in this case. - /// - /// If you want the extra DOFs to use a different method than Jacobian - /// transpose, you can create two seperate IK modules (one which is - /// analytical and one with the iterative method of your choice) and combine - /// them in a HierarchicalIK. - enum ExtraDofUtilization_t - { - UNUSED = 0, - PRE_ANALYTICAL, - POST_ANALYTICAL, - PRE_AND_POST_ANALYTICAL - }; - - struct Solution - { - /// Default constructor - Solution(const Eigen::VectorXd& _config = Eigen::VectorXd(), - int _validity = VALID); - - /// Configuration computed by the Analytical solver - Eigen::VectorXd mConfig; - - /// Bitmap for whether this configuration is valid. Bitwise-compare it to - /// the enumerations in Validity_t to whether this configuration is valid. - int mValidity; - }; - - // std::function template for comparing the quality of configurations - typedef std::function QualityComparison; - - struct UniqueProperties - { - /// Flag for how to use the extra DOFs in the IK module. - ExtraDofUtilization_t mExtraDofUtilization; - - /// How much to clamp the extra error that gets applied to DOFs - double mExtraErrorLengthClamp; - - /// Function for comparing the qualities of solutions - QualityComparison mQualityComparator; - - /// Default constructor. Uses a default quality comparison function. - UniqueProperties(ExtraDofUtilization_t extraDofUtilization = UNUSED, - double extraErrorLengthClamp = DefaultIKErrorClamp); - - /// Constructor that allows you to set the quality comparison function. - UniqueProperties(ExtraDofUtilization_t extraDofUtilization, - double extraErrorLengthClamp, - QualityComparison qualityComparator); - - /// Reset the quality comparison function to its default behavior. - void resetQualityComparisonFunction(); - }; - - struct Properties : GradientMethod::Properties, UniqueProperties - { - // Default constructor - Properties( - const GradientMethod::Properties& gradientProperties = - GradientMethod::Properties(), - const UniqueProperties& analyticalProperties = UniqueProperties()); - - // Construct Properties by specifying the UniqueProperties. The - // GradientMethod::Properties components will be set to defaults. - Properties(const UniqueProperties& analyticalProperties); - }; - - /// Constructor - Analytical(InverseKinematics* _ik, const std::string& _methodName, - const Properties& _properties); - - /// Virtual destructor - virtual ~Analytical() = default; - - /// Get the solutions for this IK module, along with a tag indicating - /// whether each solution is valid. This function will assume that you want - /// to use the desired transform given by the IK module's current - /// ErrorMethod. - const std::vector& getSolutions(); - - /// Get the solutions for this IK module, along with a tag indicating - /// whether each solution is valid. This function will compute the - /// configurations using the given desired transform instead of using the - /// IK module's current ErrorMethod. - const std::vector& getSolutions( - const Eigen::Isometry3d& _desiredTf); - - /// You should not need to override this function. Instead, you should - /// override computeSolutions. - void computeGradient(const Eigen::Vector6d& _error, - Eigen::VectorXd& _grad) override; - - /// Use this function to fill the entries of the mSolutions variable. Be - /// sure to clear the mSolutions vector at the start, and to also return the - /// mSolutions vector at the end. Note that you are not expected to evaluate - /// any of the solutions for their quality. However, you should set the - /// Solution::mValidity flag to OUT_OF_REACH for each solution that does not - /// actually reach the desired transform, and you should call - /// checkSolutionJointLimits() and the end of the function, which will set - /// the LIMIT_VIOLATED flags of any configurations that are outside of the - /// position limits. - virtual const std::vector& computeSolutions( - const Eigen::Isometry3d& _desiredBodyTf) = 0; - - /// Get a list of the DOFs that will be included in the entries of the - /// solutions returned by getSolutions(). Ideally, this should match up with - /// the DOFs being used by the InverseKinematics module, but this might not - /// always be possible, so this function ensures that solutions can be - /// interpreted correctly. - virtual const std::vector& getDofs() const = 0; - - /// Set the configuration of the DOFs. The components of _config must - /// correspond to the DOFs provided by getDofs(). - void setPositions(const Eigen::VectorXd& _config); - - /// Get the configuration of the DOFs. The components of this vector will - /// correspond to the DOFs provided by getDofs(). - Eigen::VectorXd getPositions() const; - - /// Set how you want extra DOFs to be utilized by the IK module - void setExtraDofUtilization(ExtraDofUtilization_t _utilization); - - /// Get how extra DOFs are being utilized by the IK module - ExtraDofUtilization_t getExtraDofUtilization() const; - - /// Set how much to clamp the error vector that gets applied to extra DOFs - void setExtraErrorLengthClamp(double _clamp); - - /// Get how much we will clamp the error vector that gets applied to extra - /// DOFs - double getExtraErrorLengthClamp() const; - - /// Set the function that will be used to compare the qualities of two - /// solutions. This function should return true if the first argument is a - /// better solution than the second argument. - /// - /// By default, it will prefer the solution which has the smallest size for - /// its largest change in joint angle. In other words, for each - /// configuration that it is given, it will compare the largest change in - /// joint angle for each configuration and pick the one that is smallest. - /// - /// Note that outside of this comparison function, the Solutions will be - /// split between which are valid, which are out-of-reach, and which are - /// in violation of joint limits. Valid solutions will always be ranked - /// above invalid solutions, and joint limit violations will always be - /// ranked last. - void setQualityComparisonFunction(const QualityComparison& _func); - - /// Reset the quality comparison function to the default method. - void resetQualityComparisonFunction(); - - /// Get the Properties for this Analytical class - Properties getAnalyticalProperties() const; - - /// Construct a mapping from the DOFs of getDofs() to their indices within - /// the Node's list of dependent DOFs. This will be called immediately after - /// the Analytical is constructed; this one call is sufficient as long as - /// the DOFs of Analytical::getDofs() is not changed. However, if your - /// Analytical is able to change the DOFs that it operates on, then you will - /// need to call this function each time the DOFs have changed. - void constructDofMap(); - - protected: - - /// This function will compute a gradient which utilizes the extra DOFs - /// that go unused by the Analytical solution and then it will add the - /// components of that gradient to the output parameter: grad. - /// - /// You can override this function to customize how the extra DOFs are used. - /// The default behavior is to use a simple Jacobian Transpose method. - /// - /// The utilization flag will be PRE_ANALYTICAL if the function is being - /// called before the Analytical solution is computed; it will be - /// POST_ANALYTICAL if the function is being called after the Analytical - /// solution is computed. - virtual void addExtraDofGradient( - Eigen::VectorXd& grad, - const Eigen::Vector6d& error, - ExtraDofUtilization_t utilization); - - /// Go through the mSolutions vector and tag entries with LIMIT_VIOLATED if - /// any components of their configuration are outside of their position - /// limits. This will NOT clear the LIMIT_VIOLATED flag from entries of - /// mSolutions which are already tagged with it, even if they do not violate - /// any limits. - void checkSolutionJointLimits(); - - /// Vector of solutions - std::vector mSolutions; - - /// Properties for this Analytical IK solver - UniqueProperties mAnalyticalP; - - private: - - /// This maps the DOFs provided by getDofs() to their index in the Node's - /// list of dependent DOFs. This map is constructed by constructDofMap(). - std::vector mDofMap; - - /// List of extra DOFs in the module which are not covered by the Analytical - /// IK implementation. The index of each DOF is its dependency index in the - /// JacobianNode (i.e. the column it applies to in the Node's Jacobian). - std::vector mExtraDofs; - - /// A cache for the valid solutions. The valid and invalid solution caches - /// are kept separate so that they can each be sorted by quality - /// individually. Valid solutions will always be at the top of mFinalResults - /// even if their quality is scored below the invalid solutions. - std::vector mValidSolutionsCache; - - /// A cache for the out of reach solutions. - std::vector mOutOfReachCache; - - /// A cache for solutions that violate joint limits - std::vector mLimitViolationCache; - - /// A cache for storing the current configuration - Eigen::VectorXd mConfigCache; - - /// A cache for storing the current configuration so that it can be restored - /// later. This is different from mConfigCache which will not be used to - /// restore the configuration. - Eigen::VectorXd mRestoreConfigCache; - - /// A cache for storing the gradient for the extra DOFs - Eigen::VectorXd mExtraDofGradCache; - }; + // Methods for computing IK gradient + class GradientMethod; + class JacobianDLS; + class JacobianTranspose; + class Analytical; /// If this IK module is set to active, then it will be utilized by any /// HierarchicalIK that has it in its list. If it is set to inactive, then it @@ -1116,84 +376,10 @@ class InverseKinematics : public common::Subject protected: - /// The InverseKinematics::Objective Function is simply used to merge the - /// objective and null space objective functions that are being held by an - /// InverseKinematics module. This class is not meant to be extended or - /// instantiated by a user. Call InverseKinematics::resetProblem() to set - /// the objective of the module's Problem to an InverseKinematics::Objective. - class Objective final : public Function, public optimizer::Function - { - public: - - /// Constructor - Objective(InverseKinematics* _ik); - - /// Virtual destructor - virtual ~Objective() = default; - - // Documentation inherited - optimizer::FunctionPtr clone(InverseKinematics* _newIK) const override; - - // Documentation inherited - double eval(const Eigen::VectorXd& _x) override; - - // Documentation inherited - void evalGradient(const Eigen::VectorXd& _x, - Eigen::Map _grad) override; - - protected: - - /// Pointer to this Objective's IK module - sub_ptr mIK; - - /// Cache for the gradient of the Objective - Eigen::VectorXd mGradCache; - - /// Cache for the null space SVD - Eigen::JacobiSVD mSVDCache; - - /// Cache for the null space - Eigen::MatrixXd mNullSpaceCache; - - public: - // To get byte-aligned Eigen vectors - EIGEN_MAKE_ALIGNED_OPERATOR_NEW - }; - + // For the definitions of these functions, see the bottom of this header + class Objective; friend class Objective; - - /// The InverseKinematics::Constraint Function is simply meant to be used to - /// merge the ErrorMethod and GradientMethod that are being held by an - /// InverseKinematics module. This class is not meant to be extended or - /// instantiated by a user. Call InverseKinematics::resetProblem() to set the - /// first equality constraint of the module's Problem to an - /// InverseKinematics::Constraint. - class Constraint final : public Function, public optimizer::Function - { - public: - - /// Constructor - Constraint(InverseKinematics* _ik); - - /// Virtual constructor - virtual ~Constraint() = default; - - // Documentation inherited - optimizer::FunctionPtr clone(InverseKinematics* _newIK) const override; - - // Documentation inherited - double eval(const Eigen::VectorXd& _x) override; - - // Documentation inherited - void evalGradient(const Eigen::VectorXd& _x, - Eigen::Map _grad) override; - - protected: - - /// Pointer to this Constraint's IK module - sub_ptr mIK; - }; - + class Constraint; friend class Constraint; /// Constructor that accepts a JacobianNode @@ -1270,6 +456,846 @@ class InverseKinematics : public common::Subject typedef InverseKinematics IK; +//============================================================================== +/// This class should be inherited by optimizer::Function classes that have a +/// dependency on the InverseKinematics module that they belong to. If you +/// pass an InverseKinematics::Function into the Problem of an +/// InverseKinematics module, then it will be properly cloned whenever the +/// InverseKinematics module that it belongs to gets cloned. Any Function +/// classes in the Problem that do not inherit InverseKinematics::Function +/// will just be copied over by reference. +class InverseKinematics::Function +{ +public: + + /// Enable this function to be cloned to a new IK module. + virtual optimizer::FunctionPtr clone(InverseKinematics* _newIK) const = 0; + + /// Virtual destructor + virtual ~Function() = default; +}; + +//============================================================================== +/// ErrorMethod is a base class for different ways of computing the error of +/// an InverseKinematics module. +class InverseKinematics::ErrorMethod : public common::Subject +{ +public: + + typedef std::pair Bounds; + + /// The Properties struct contains settings that are commonly used by + /// methods that compute error for inverse kinematics. + struct Properties + { + /// Default constructor + Properties(const Bounds& _bounds = + Bounds(Eigen::Vector6d::Constant(-DefaultIKTolerance), + Eigen::Vector6d::Constant( DefaultIKTolerance)), + + double _errorClamp = DefaultIKErrorClamp, + + const Eigen::Vector6d& _errorWeights = Eigen::compose( + Eigen::Vector3d::Constant(DefaultIKAngularWeight), + Eigen::Vector3d::Constant(DefaultIKLinearWeight))); + + /// Bounds that define the acceptable range of the Node's transform + /// relative to its target frame. + std::pair mBounds; + + /// The error vector will be clamped to this length with each iteration. + /// This is used to enforce sane behavior, even when there are extremely + /// large error vectors. + double mErrorLengthClamp; + + /// These weights will be applied to the error vector component-wise. This + /// allows you to set some components of error as more important than + /// others, or to scale their coordinate spaces. For example, you will + /// often want the first three components (orientation error) to have + /// smaller weights than the last three components (translation error). + Eigen::Vector6d mErrorWeights; + + // To get byte-aligned Eigen vectors + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + }; + + /// Constructor + ErrorMethod(InverseKinematics* _ik, + const std::string& _methodName, + const Properties& _properties = Properties()); + + /// Virtual destructor + virtual ~ErrorMethod() = default; + + /// Enable this ErrorMethod to be cloned to a new IK module. + virtual std::unique_ptr clone( + InverseKinematics* _newIK) const = 0; + + /// Override this function with your implementation of the error vector + /// computation. The expectation is that the first three components of the + /// vector will correspond to orientation error (in an angle-axis format) + /// while the last three components correspond to translational error. + /// + /// When implementing this function, you should assume that the Skeleton's + /// current joint positions corresponds to the positions that you + /// must use to compute the error. This function will only get called when + /// an update is needed. + virtual Eigen::Vector6d computeError() = 0; + + /// Override this function with your implementation of computing the desired + /// given the current transform and error vector. If you want the desired + /// transform to always be equal to the Target's transform, you can simply + /// call ErrorMethod::computeDesiredTransform to implement this function. + virtual Eigen::Isometry3d computeDesiredTransform( + const Eigen::Isometry3d& _currentTf, const Eigen::Vector6d& _error) = 0; + + /// This function is used to handle caching the error vector. + const Eigen::Vector6d& evalError(const Eigen::VectorXd& _q); + + /// Get the name of this ErrorMethod. + const std::string& getMethodName() const; + + /// Set all the error bounds. + void setBounds( + const Eigen::Vector6d& _lower = + Eigen::Vector6d::Constant(-DefaultIKTolerance), + const Eigen::Vector6d& _upper = + Eigen::Vector6d::Constant( DefaultIKTolerance)); + + /// Set all the error bounds. + void setBounds(const std::pair& _bounds); + + /// Get all the error bounds. + const std::pair& getBounds() const; + + /// Set the error bounds for orientation. + void setAngularBounds( + const Eigen::Vector3d& _lower = + Eigen::Vector3d::Constant(-DefaultIKTolerance), + const Eigen::Vector3d& _upper = + Eigen::Vector3d::Constant( DefaultIKTolerance)); + + /// Set the error bounds for orientation. + void setAngularBounds( + const std::pair& _bounds); + + /// Get the error bounds for orientation. + std::pair getAngularBounds() const; + + /// Set the error bounds for translation. + void setLinearBounds( + const Eigen::Vector3d& _lower = + Eigen::Vector3d::Constant(-DefaultIKTolerance), + const Eigen::Vector3d& _upper = + Eigen::Vector3d::Constant( DefaultIKTolerance)); + + /// Set the error bounds for translation. + void setLinearBounds( + const std::pair& _bounds); + + /// Get the error bounds for translation. + std::pair getLinearBounds() const; + + /// Set the clamp that will be applied to the length of the error vector + /// each iteration. + void setErrorLengthClamp(double _clampSize = DefaultIKErrorClamp); + + /// Set the clamp that will be applied to the length of the error vector + /// each iteration. + double getErrorLengthClamp() const; + + /// Set the weights that will be applied to each component of the error + /// vector. + void setErrorWeights(const Eigen::Vector6d& _weights); + + /// Get the weights that will be applied to each component of the error + /// vector. + const Eigen::Vector6d& getErrorWeights() const; + + /// Set the weights that will be applied to each angular component of the + /// error vector. + void setAngularErrorWeights( + const Eigen::Vector3d& _weights = + Eigen::Vector3d::Constant(DefaultIKAngularWeight)); + + /// Get the weights that will be applied to each angular component of the + /// error vector. + Eigen::Vector3d getAngularErrorWeights() const; + + /// Set the weights that will be applied to each linear component of the + /// error vector. + void setLinearErrorWeights( + const Eigen::Vector3d& _weights = + Eigen::Vector3d::Constant(DefaultIKLinearWeight)); + + /// Get the weights that will be applied to each linear component of the + /// error vector. + Eigen::Vector3d getLinearErrorWeights() const; + + /// Get the Properties of this ErrorMethod + Properties getErrorMethodProperties() const; + + /// Clear the cache to force the error to be recomputed. It should generally + /// not be necessary to call this function. + void clearCache(); + +protected: + + /// Pointer to the IK module of this ErrorMethod + common::sub_ptr mIK; + + /// Name of this error method + std::string mMethodName; + + /// The last joint positions passed into this ErrorMethod + Eigen::VectorXd mLastPositions; + + /// The last error vector computed by this ErrorMethod + Eigen::Vector6d mLastError; + + /// The properties of this ErrorMethod + Properties mErrorP; + +public: + // To get byte-aligned Eigen vectors + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + +}; + +//============================================================================== +/// The TaskSpaceRegion is a nicely generalized method for computing the error +/// of an IK Problem. +class InverseKinematics::TaskSpaceRegion : public ErrorMethod +{ +public: + + struct UniqueProperties + { + /// Setting this to true (which is default) will tell it to compute the + /// error based on the center of the Task Space Region instead of the edge + /// of the Task Space Region. This often results in faster convergence, as + /// the Node will enter the Task Space Region more aggressively. + /// + /// Once the Node is inside the Task Space Region, the error vector will + /// drop to zero, regardless of whether this flag is true or false. + bool mComputeErrorFromCenter; + + /// Default constructor + UniqueProperties(bool computeErrorFromCenter = true); + }; + + struct Properties : ErrorMethod::Properties, UniqueProperties + { + /// Default constructor + Properties( + const ErrorMethod::Properties& errorProperties = + ErrorMethod::Properties(), + const UniqueProperties& taskSpaceProperties = UniqueProperties()); + }; + + /// Default Constructor + explicit TaskSpaceRegion(InverseKinematics* _ik, + const Properties& _properties = Properties()); + + /// Virtual destructor + virtual ~TaskSpaceRegion() = default; + + // Documentation inherited + std::unique_ptr clone(InverseKinematics* _newIK) const override; + + // Documentation inherited + Eigen::Isometry3d computeDesiredTransform( + const Eigen::Isometry3d& _currentTf, + const Eigen::Vector6d& _error) override; + + // Documentation inherited + Eigen::Vector6d computeError() override; + + /// Set whether this TaskSpaceRegion should compute its error vector from + /// the center of the region. + void setComputeFromCenter(bool computeFromCenter); + + /// Get whether this TaskSpaceRegion is set to compute its error vector from + /// the center of the region. + bool isComputingFromCenter() const; + + /// Get the Properties of this TaskSpaceRegion + Properties getTaskSpaceRegionProperties() const; + +protected: + + /// Properties of this TaskSpaceRegion + UniqueProperties mTaskSpaceP; + +}; + +//============================================================================== +/// GradientMethod is a base class for different ways of computing the +/// gradient of an InverseKinematics module. +class InverseKinematics::GradientMethod : public common::Subject +{ +public: + + struct Properties + { + /// The component-wise clamp for this GradientMethod + double mComponentWiseClamp; + + /// The weights for this GradientMethod + Eigen::VectorXd mComponentWeights; + + /// Default constructor + Properties(double clamp = DefaultIKGradientComponentClamp, + const Eigen::VectorXd& weights = Eigen::VectorXd()); + }; + + /// Constructor + GradientMethod(InverseKinematics* _ik, + const std::string& _methodName, + const Properties& _properties); + + /// Virtual destructor + virtual ~GradientMethod() = default; + + /// Enable this GradientMethod to be cloned to a new IK module + virtual std::unique_ptr clone( + InverseKinematics* _newIK) const = 0; + + /// Override this function with your implementation of the gradient + /// computation. The direction that this gradient points in should make the + /// error **worse** if applied to the joint positions, because the + /// Problem is configured as a gradient **descent** error minimization + /// Problem. + /// + /// The error vector that is passed in will be determined by the IK module's + /// ErrorMethod. The expectation is that the first three components of the + /// vector correspond to orientation error (in an angle-axis format) while + /// the last three components correspond to translational error. + /// + /// When implementing this function, you should assume that the Skeleton's + /// current joint positions corresponds to the positions that you + /// must use to compute the error. This function will only get called when + /// an update is needed. + virtual void computeGradient(const Eigen::Vector6d& _error, + Eigen::VectorXd& _grad) = 0; + + /// This function is used to handle caching the gradient vector and + /// interfacing with the solver. + void evalGradient(const Eigen::VectorXd& _q, + Eigen::Map _grad); + + /// Get the name of this GradientMethod. + const std::string& getMethodName() const; + + /// Clamp the gradient based on the clamp settings of this GradientMethod. + void clampGradient(Eigen::VectorXd& _grad) const; + + /// Set the component-wise clamp for this GradientMethod. Each component + /// of the gradient will be individually clamped to this size. + void setComponentWiseClamp(double _clamp = DefaultIKGradientComponentClamp); + + /// Get the component-wise clamp for this GradientMethod. + double getComponentWiseClamp() const; + + /// Apply weights to the gradient based on the weight settings of this + /// GradientMethod. + void applyWeights(Eigen::VectorXd& _grad) const; + + /// Set the weights that will be applied to each component of the gradient. + /// If the number of components in _weights is smaller than the number of + /// components in the gradient, then a weight of 1.0 will be applied to all + /// components that are out of the range of _weights. Passing in an empty + /// vector for _weights will effectively make all the gradient components + /// unweighted. + void setComponentWeights(const Eigen::VectorXd& _weights); + + /// Get the weights of this GradientMethod. + const Eigen::VectorXd& getComponentWeights() const; + + /// Convert the gradient that gets generated by Jacobian methods into a + /// gradient that can be used by a GradientDescentSolver. + /// + /// Not all Joint types can be integrated using standard addition (e.g. + /// FreeJoint and BallJoint), therefore Jacobian-based differential methods + /// will tend to generate gradients that cannot be correctly used by a + /// simple addition-based GradientDescentSolver. Running your gradient + /// through this function before returning it will make the gradient + /// suitable for a standard solver. + void convertJacobianMethodOutputToGradient( + Eigen::VectorXd& grad, const std::vector& dofs); + + /// Get the Properties of this GradientMethod + Properties getGradientMethodProperties() const; + + /// Clear the cache to force the gradient to be recomputed. It should + /// generally not be necessary to call this function. + void clearCache(); + +protected: + + /// The IK module that this GradientMethod belongs to. + common::sub_ptr mIK; + + /// The name of this method + std::string mMethodName; + + /// The last positions that was passed to this GradientMethod + Eigen::VectorXd mLastPositions; + + /// The last gradient that was computed by this GradientMethod + Eigen::VectorXd mLastGradient; + + /// Properties for this GradientMethod + Properties mGradientP; + +private: + + /// Cache used by convertJacobianMethodOutputToGradient to avoid + /// reallocating this vector on each iteration. + Eigen::VectorXd mInitialPositionsCache; +}; + +//============================================================================== +/// JacobianDLS refers to the Damped Least Squares Jacobian Pseudoinverse +/// (specifically, Moore-Penrose Pseudoinverse). This is a very precise method +/// for computing the gradient and is especially suitable for performing IK on +/// industrial manipulators that need to follow very exact workspace paths. +/// However, it is vulnerable to be jittery around singularities (though the +/// damping helps with this), and each cycle might take more time to compute +/// than the JacobianTranspose method (although the JacobianDLS method will +/// usually converge in fewer cycles than JacobianTranspose). +class InverseKinematics::JacobianDLS : public GradientMethod +{ +public: + + struct UniqueProperties + { + /// Damping coefficient + double mDamping; + + /// Default constructor + UniqueProperties(double damping = DefaultIKDLSCoefficient); + }; + + struct Properties : GradientMethod::Properties, UniqueProperties + { + /// Default constructor + Properties( + const GradientMethod::Properties& gradientProperties = + GradientMethod::Properties(), + const UniqueProperties& dlsProperties = UniqueProperties()); + }; + + /// Constructor + explicit JacobianDLS(InverseKinematics* _ik, + const Properties& properties = Properties()); + + /// Virtual destructor + virtual ~JacobianDLS() = default; + + // Documentation inherited + std::unique_ptr clone( + InverseKinematics* _newIK) const override; + + // Documentation inherited + void computeGradient(const Eigen::Vector6d& _error, + Eigen::VectorXd& _grad) override; + + /// Set the damping coefficient. A higher damping coefficient will smooth + /// out behavior around singularities but will also result in less precision + /// in general. The default value is appropriate for most use cases. + void setDampingCoefficient(double _damping = DefaultIKDLSCoefficient); + + /// Get the damping coefficient. + double getDampingCoefficient() const; + + /// Get the Properties of this JacobianDLS + Properties getJacobianDLSProperties() const; + +protected: + + /// Properties of this Damped Least Squares method + UniqueProperties mDLSProperties; + +}; + +//============================================================================== +/// JacobianTranspose will simply apply the transpose of the Jacobian to the +/// error vector in order to compute the gradient. This method tends to be +/// very smooth but imprecise, requiring more iterations before converging +/// and being less precise in general. This method is suitable for animations +/// where smoothness is prefered over precision. +class InverseKinematics::JacobianTranspose : public GradientMethod +{ +public: + + /// Constructor + explicit JacobianTranspose(InverseKinematics* _ik, + const Properties& properties = Properties()); + + /// Virtual destructor + virtual ~JacobianTranspose() = default; + + // Documentation inherited + std::unique_ptr clone( + InverseKinematics* _newIK) const override; + + // Documentation inherited + void computeGradient(const Eigen::Vector6d& _error, + Eigen::VectorXd& _grad) override; +}; + +//============================================================================== +/// Analytical is a base class that should be inherited by methods that are +/// made to solve the IK analytically instead of iteratively. This provides an +/// extended API that is relevant to Analytical solvers but not iterative +/// solvers. +/// +/// Creating an Analytical solver will have the side effect of removing the +/// error clamp and error weights from your ErrorMethod. If you still want +/// your error computations to be clamped and weighted, you should set it +/// again after creating the Analytical solver. Clamping and weighting the +/// error vector often helps iterative methods to converge smoothly, but it is +/// counter-productive for analytical methods which do not typically rely on +/// convergence; analytical methods can usually solve the entire error vector +/// directly. +class InverseKinematics::Analytical : public GradientMethod +{ +public: + + /// Bitwise enumerations that are used to describe some properties of each + /// solution produced by the analytical IK. + enum Validity_t + { + VALID = 0, ///< The solution is completely valid and reaches the target + OUT_OF_REACH = 1 << 0, ///< The solution does not reach the target + LIMIT_VIOLATED = 1 << 1 ///< The solution has one or more joint positions that violate the joint limits + }; + + /// If there are extra DOFs in the IK module which your Analytical solver + /// implementation does not make use of, those DOFs can be used to + /// supplement the analytical solver using Jacobian transpose iteration. + /// This enumeration is used to indicate whether you want those DOFs to be + /// used before applying the analytical solution, after applying the + /// analytical solution, or not be used at all. + /// + /// Jacobian transpose is used for the extra DOFs because it is inexpensive + /// and robust to degenerate Jacobians which are common in low dimensional + /// joint spaces. The primary advantage of pseudoinverse methods over + /// Jacobian transpose methods is their precision, but analytical methods + /// are even more precise than pseudoinverse methods, so that precision is + /// not needed in this case. + /// + /// If you want the extra DOFs to use a different method than Jacobian + /// transpose, you can create two seperate IK modules (one which is + /// analytical and one with the iterative method of your choice) and combine + /// them in a HierarchicalIK. + enum ExtraDofUtilization + { + UNUSED = 0, + PRE_ANALYTICAL, + POST_ANALYTICAL, + PRE_AND_POST_ANALYTICAL + }; + + struct Solution + { + /// Default constructor + Solution(const Eigen::VectorXd& _config = Eigen::VectorXd(), + int _validity = VALID); + + /// Configuration computed by the Analytical solver + Eigen::VectorXd mConfig; + + /// Bitmap for whether this configuration is valid. Bitwise-compare it to + /// the enumerations in Validity_t to whether this configuration is valid. + int mValidity; + }; + + // std::function template for comparing the quality of configurations + typedef std::function QualityComparison; + + struct UniqueProperties + { + /// Flag for how to use the extra DOFs in the IK module. + ExtraDofUtilization mExtraDofUtilization; + + /// How much to clamp the extra error that gets applied to DOFs + double mExtraErrorLengthClamp; + + /// Function for comparing the qualities of solutions + QualityComparison mQualityComparator; + + /// Default constructor. Uses a default quality comparison function. + UniqueProperties(ExtraDofUtilization extraDofUtilization = UNUSED, + double extraErrorLengthClamp = DefaultIKErrorClamp); + + /// Constructor that allows you to set the quality comparison function. + UniqueProperties(ExtraDofUtilization extraDofUtilization, + double extraErrorLengthClamp, + QualityComparison qualityComparator); + + /// Reset the quality comparison function to its default behavior. + void resetQualityComparisonFunction(); + }; + + struct Properties : GradientMethod::Properties, UniqueProperties + { + // Default constructor + Properties( + const GradientMethod::Properties& gradientProperties = + GradientMethod::Properties(), + const UniqueProperties& analyticalProperties = UniqueProperties()); + + // Construct Properties by specifying the UniqueProperties. The + // GradientMethod::Properties components will be set to defaults. + Properties(const UniqueProperties& analyticalProperties); + }; + + /// Constructor + Analytical(InverseKinematics* _ik, const std::string& _methodName, + const Properties& _properties); + + /// Virtual destructor + virtual ~Analytical() = default; + + /// Get the solutions for this IK module, along with a tag indicating + /// whether each solution is valid. This function will assume that you want + /// to use the desired transform given by the IK module's current + /// ErrorMethod. + const std::vector& getSolutions(); + + /// Get the solutions for this IK module, along with a tag indicating + /// whether each solution is valid. This function will compute the + /// configurations using the given desired transform instead of using the + /// IK module's current ErrorMethod. + const std::vector& getSolutions( + const Eigen::Isometry3d& _desiredTf); + + /// You should not need to override this function. Instead, you should + /// override computeSolutions. + void computeGradient(const Eigen::Vector6d& _error, + Eigen::VectorXd& _grad) override; + + /// Use this function to fill the entries of the mSolutions variable. Be + /// sure to clear the mSolutions vector at the start, and to also return the + /// mSolutions vector at the end. Note that you are not expected to evaluate + /// any of the solutions for their quality. However, you should set the + /// Solution::mValidity flag to OUT_OF_REACH for each solution that does not + /// actually reach the desired transform, and you should call + /// checkSolutionJointLimits() and the end of the function, which will set + /// the LIMIT_VIOLATED flags of any configurations that are outside of the + /// position limits. + virtual const std::vector& computeSolutions( + const Eigen::Isometry3d& _desiredBodyTf) = 0; + + /// Get a list of the DOFs that will be included in the entries of the + /// solutions returned by getSolutions(). Ideally, this should match up with + /// the DOFs being used by the InverseKinematics module, but this might not + /// always be possible, so this function ensures that solutions can be + /// interpreted correctly. + virtual const std::vector& getDofs() const = 0; + + /// Set the configuration of the DOFs. The components of _config must + /// correspond to the DOFs provided by getDofs(). + void setPositions(const Eigen::VectorXd& _config); + + /// Get the configuration of the DOFs. The components of this vector will + /// correspond to the DOFs provided by getDofs(). + Eigen::VectorXd getPositions() const; + + /// Set how you want extra DOFs to be utilized by the IK module + void setExtraDofUtilization(ExtraDofUtilization _utilization); + + /// Get how extra DOFs are being utilized by the IK module + ExtraDofUtilization getExtraDofUtilization() const; + + /// Set how much to clamp the error vector that gets applied to extra DOFs + void setExtraErrorLengthClamp(double _clamp); + + /// Get how much we will clamp the error vector that gets applied to extra + /// DOFs + double getExtraErrorLengthClamp() const; + + /// Set the function that will be used to compare the qualities of two + /// solutions. This function should return true if the first argument is a + /// better solution than the second argument. + /// + /// By default, it will prefer the solution which has the smallest size for + /// its largest change in joint angle. In other words, for each + /// configuration that it is given, it will compare the largest change in + /// joint angle for each configuration and pick the one that is smallest. + /// + /// Note that outside of this comparison function, the Solutions will be + /// split between which are valid, which are out-of-reach, and which are + /// in violation of joint limits. Valid solutions will always be ranked + /// above invalid solutions, and joint limit violations will always be + /// ranked last. + void setQualityComparisonFunction(const QualityComparison& _func); + + /// Reset the quality comparison function to the default method. + void resetQualityComparisonFunction(); + + /// Get the Properties for this Analytical class + Properties getAnalyticalProperties() const; + + /// Construct a mapping from the DOFs of getDofs() to their indices within + /// the Node's list of dependent DOFs. This will be called immediately after + /// the Analytical is constructed; this one call is sufficient as long as + /// the DOFs of Analytical::getDofs() is not changed. However, if your + /// Analytical is able to change the DOFs that it operates on, then you will + /// need to call this function each time the DOFs have changed. + void constructDofMap(); + +protected: + + /// This function will compute a gradient which utilizes the extra DOFs + /// that go unused by the Analytical solution and then it will add the + /// components of that gradient to the output parameter: grad. + /// + /// You can override this function to customize how the extra DOFs are used. + /// The default behavior is to use a simple Jacobian Transpose method. + /// + /// The utilization flag will be PRE_ANALYTICAL if the function is being + /// called before the Analytical solution is computed; it will be + /// POST_ANALYTICAL if the function is being called after the Analytical + /// solution is computed. + virtual void addExtraDofGradient( + Eigen::VectorXd& grad, + const Eigen::Vector6d& error, + ExtraDofUtilization utilization); + + /// Go through the mSolutions vector and tag entries with LIMIT_VIOLATED if + /// any components of their configuration are outside of their position + /// limits. This will NOT clear the LIMIT_VIOLATED flag from entries of + /// mSolutions which are already tagged with it, even if they do not violate + /// any limits. + void checkSolutionJointLimits(); + + /// Vector of solutions + std::vector mSolutions; + + /// Properties for this Analytical IK solver + UniqueProperties mAnalyticalP; + +private: + + /// This maps the DOFs provided by getDofs() to their index in the Node's + /// list of dependent DOFs. This map is constructed by constructDofMap(). + std::vector mDofMap; + + /// List of extra DOFs in the module which are not covered by the Analytical + /// IK implementation. The index of each DOF is its dependency index in the + /// JacobianNode (i.e. the column it applies to in the Node's Jacobian). + std::vector mExtraDofs; + + /// A cache for the valid solutions. The valid and invalid solution caches + /// are kept separate so that they can each be sorted by quality + /// individually. Valid solutions will always be at the top of mFinalResults + /// even if their quality is scored below the invalid solutions. + std::vector mValidSolutionsCache; + + /// A cache for the out of reach solutions. + std::vector mOutOfReachCache; + + /// A cache for solutions that violate joint limits + std::vector mLimitViolationCache; + + /// A cache for storing the current configuration + Eigen::VectorXd mConfigCache; + + /// A cache for storing the current configuration so that it can be restored + /// later. This is different from mConfigCache which will not be used to + /// restore the configuration. + Eigen::VectorXd mRestoreConfigCache; + + /// A cache for storing the gradient for the extra DOFs + Eigen::VectorXd mExtraDofGradCache; +}; + +//============================================================================== +/// The InverseKinematics::Objective Function is simply used to merge the +/// objective and null space objective functions that are being held by an +/// InverseKinematics module. This class is not meant to be extended or +/// instantiated by a user. Call InverseKinematics::resetProblem() to set +/// the objective of the module's Problem to an InverseKinematics::Objective. +class InverseKinematics::Objective final : + public Function, public optimizer::Function +{ +public: + + /// Constructor + Objective(InverseKinematics* _ik); + + /// Virtual destructor + virtual ~Objective() = default; + + // Documentation inherited + optimizer::FunctionPtr clone(InverseKinematics* _newIK) const override; + + // Documentation inherited + double eval(const Eigen::VectorXd& _x) override; + + // Documentation inherited + void evalGradient(const Eigen::VectorXd& _x, + Eigen::Map _grad) override; + +protected: + + /// Pointer to this Objective's IK module + sub_ptr mIK; + + /// Cache for the gradient of the Objective + Eigen::VectorXd mGradCache; + + /// Cache for the null space SVD + Eigen::JacobiSVD mSVDCache; + + /// Cache for the null space + Eigen::MatrixXd mNullSpaceCache; + +public: + // To get byte-aligned Eigen vectors + EIGEN_MAKE_ALIGNED_OPERATOR_NEW +}; + +//============================================================================== +/// The InverseKinematics::Constraint Function is simply meant to be used to +/// merge the ErrorMethod and GradientMethod that are being held by an +/// InverseKinematics module. This class is not meant to be extended or +/// instantiated by a user. Call InverseKinematics::resetProblem() to set the +/// first equality constraint of the module's Problem to an +/// InverseKinematics::Constraint. +class InverseKinematics::Constraint final : + public Function, public optimizer::Function +{ +public: + + /// Constructor + Constraint(InverseKinematics* _ik); + + /// Virtual constructor + virtual ~Constraint() = default; + + // Documentation inherited + optimizer::FunctionPtr clone(InverseKinematics* _newIK) const override; + + // Documentation inherited + double eval(const Eigen::VectorXd& _x) override; + + // Documentation inherited + void evalGradient(const Eigen::VectorXd& _x, + Eigen::Map _grad) override; + +protected: + + /// Pointer to this Constraint's IK module + sub_ptr mIK; +}; + } // namespace dynamics } // namespace dart diff --git a/unittests/testForwardKinematics.cpp b/unittests/testForwardKinematics.cpp index 32029b7f26259..4a1be2687ebe5 100644 --- a/unittests/testForwardKinematics.cpp +++ b/unittests/testForwardKinematics.cpp @@ -219,7 +219,7 @@ TEST(FORWARD_KINEMATICS, JACOBIAN_PARTIAL_CHANGE) //============================================================================== TEST(FORWARD_KINEMATICS, JACOBIAN_END_EFFECTOR_CHANGE) { - // This is a regression test for issue #499 + // This is a regression test for pull request #683 const double tolerance = 1e-8; dart::utils::DartLoader loader;