diff --git a/CMakeLists.txt b/CMakeLists.txt index b83dfcf4ebf2a..6236a9cffa756 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -45,7 +45,7 @@ project(dart) set(DART_MAJOR_VERSION "5") set(DART_MINOR_VERSION "1") -set(DART_PATCH_VERSION "1") +set(DART_PATCH_VERSION "2") set(DART_VERSION "${DART_MAJOR_VERSION}.${DART_MINOR_VERSION}.${DART_PATCH_VERSION}") set(DART_PKG_DESC "Dynamic Animation and Robotics Toolkit.") set(DART_PKG_EXTERNAL_DEPS "flann, ccd, fcl") diff --git a/dart/constraint/BalanceConstraint.cpp b/dart/constraint/BalanceConstraint.cpp index 8b80326748887..3aec3b5d1b3a0 100644 --- a/dart/constraint/BalanceConstraint.cpp +++ b/dart/constraint/BalanceConstraint.cpp @@ -38,6 +38,7 @@ #include "dart/dynamics/Skeleton.h" #include "dart/dynamics/EndEffector.h" +#include "dart/dynamics/Joint.h" namespace dart { namespace constraint { @@ -180,6 +181,27 @@ static void addDampedPseudoInverseToGradient( } } +//============================================================================== +static void convertJacobianMethodOutputToGradient( + Eigen::Map& grad, + const std::weak_ptr& mIK) +{ + const dart::dynamics::SkeletonPtr& skel = mIK.lock()->getSkeleton(); + skel->setVelocities(grad); + + const Eigen::VectorXd 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; +} + //============================================================================== void BalanceConstraint::evalGradient(const Eigen::VectorXd& _x, Eigen::Map _grad) @@ -255,7 +277,7 @@ void BalanceConstraint::evalGradient(const Eigen::VectorXd& _x, } else { - mNullSpaceCache.setZero(); + mNullSpaceCache.setZero(nDofs, nDofs); } size_t numEE = skel->getNumEndEffectors(); @@ -292,7 +314,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 +343,8 @@ void BalanceConstraint::evalGradient(const Eigen::VectorXd& _x, } } } + + convertJacobianMethodOutputToGradient(_grad, mIK); } //============================================================================== diff --git a/dart/dynamics/BodyNode.cpp b/dart/dynamics/BodyNode.cpp index fc23985a8cbfb..402e3d6e0d7a2 100644 --- a/dart/dynamics/BodyNode.cpp +++ b/dart/dynamics/BodyNode.cpp @@ -1183,6 +1183,22 @@ 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 these two pipelines that are + // currently independent of each other. if(JacobianNode* node = dynamic_cast(_newChildEntity)) mChildJacobianNodes.insert(node); @@ -1218,13 +1234,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/EndEffector.cpp b/dart/dynamics/EndEffector.cpp index 371dc44fc69f0..ad22fe18f0bd9 100644 --- a/dart/dynamics/EndEffector.cpp +++ b/dart/dynamics/EndEffector.cpp @@ -111,6 +111,11 @@ EndEffector::~EndEffector() assert(mBodyNode->mEndEffectors[index] == this); mBodyNode->mEndEffectors.erase(mBodyNode->mEndEffectors.begin() + index); + // We sneak this in here to patch the issue where child JacobianNodes were + // going unrecognized. This only works for EndEffector, but EndEffector is + // the only JacobianNode type besides BodyNode in v5.1. + mBodyNode->mChildJacobianNodes.erase(this); + for(size_t i=index; imEndEffectors.size(); ++i) { EndEffector* ee = mBodyNode->mEndEffectors[i]; diff --git a/dart/dynamics/InverseKinematics.cpp b/dart/dynamics/InverseKinematics.cpp index 3ac3404b77a46..1c581d8cec01d 100644 --- a/dart/dynamics/InverseKinematics.cpp +++ b/dart/dynamics/InverseKinematics.cpp @@ -38,10 +38,38 @@ #include "dart/dynamics/BodyNode.h" #include "dart/dynamics/DegreeOfFreedom.h" #include "dart/dynamics/SimpleFrame.h" +#include "dart/dynamics/Joint.h" namespace dart { namespace dynamics { +//============================================================================== +static void convertJacobianMethodOutputToGradient( + Eigen::VectorXd& grad, const std::vector& dofs, + InverseKinematics* mIK) +{ + const SkeletonPtr& skel = mIK->getNode()->getSkeleton(); + const Eigen::VectorXd 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; +} + //============================================================================== InverseKinematicsPtr InverseKinematics::create(JacobianNode* _node) { @@ -92,16 +120,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; } @@ -650,6 +687,7 @@ void InverseKinematics::JacobianDLS::computeGradient( J.transpose()*J).inverse() * J.transpose() * _error; } + convertJacobianMethodOutputToGradient(_grad, mIK->getDofs(), mIK); applyWeights(_grad); clampGradient(_grad); } @@ -690,6 +728,7 @@ void InverseKinematics::JacobianTranspose::computeGradient( const math::Jacobian& J = mIK->computeJacobian(); _grad = J.transpose() * _error; + convertJacobianMethodOutputToGradient(_grad, mIK->getDofs(), mIK); applyWeights(_grad); clampGradient(_grad); } diff --git a/dart/dynamics/Skeleton.cpp b/dart/dynamics/Skeleton.cpp index a60d4bf4dc390..c00ebaa6bf6d3 100644 --- a/dart/dynamics/Skeleton.cpp +++ b/dart/dynamics/Skeleton.cpp @@ -1540,6 +1540,11 @@ void Skeleton::registerEndEffector(EndEffector* _newEndEffector) mEndEffectors.push_back(_newEndEffector); _newEndEffector->mIndexInSkeleton = mEndEffectors.size()-1; + // We sneak this in here to patch the issue where child JacobianNodes were + // going unrecognized. This only works for EndEffector, but EndEffector is + // the only JacobianNode type besides BodyNode in v5.1. + _newEndEffector->mBodyNode->mChildJacobianNodes.insert(_newEndEffector); + // The EndEffector name gets added when the EndEffector is constructed, so we // don't need to add it here. } diff --git a/package.xml b/package.xml index d96121eb79ff9..2460ade673b88 100644 --- a/package.xml +++ b/package.xml @@ -4,7 +4,7 @@ a Catkin workspace. Catkin is not required to build DART. For more information, see: http://ros.org/reps/rep-0136.html --> dart - 5.1.1 + 5.1.2 DART (Dynamic Animation and Robotics Toolkit) is a collaborative, cross-platform, open source library created by the Georgia Tech Graphics diff --git a/unittests/testForwardKinematics.cpp b/unittests/testForwardKinematics.cpp index 33982f03f805d..f0ee5b2a51fd2 100644 --- a/unittests/testForwardKinematics.cpp +++ b/unittests/testForwardKinematics.cpp @@ -60,8 +60,8 @@ TEST(FORWARD_KINEMATICS, YAW_ROLL) // positions const size_t numTests = 2; double temp = sqrt(0.5*l2*l2); - Vector2d joints [numTests] = { Vector2d( DART_PI/4.0, DART_PI/2.0), - Vector2d(-DART_PI/4.0, -DART_PI/4.0) }; + Vector2d joints [numTests] = { Vector2d( M_PI/4.0, M_PI/2.0), + Vector2d(-M_PI/4.0, -M_PI/4.0) }; Vector3d expectedPos [numTests] = { Vector3d(temp, -temp, l1), Vector3d(temp / sqrt(2.0), temp / sqrt(2.0), l1+temp) }; @@ -71,8 +71,8 @@ TEST(FORWARD_KINEMATICS, YAW_ROLL) for (size_t i = 0; i < numTests; i++) { robot->setPositions(Eigen::VectorXd(joints[i])); - Vector3d actual - = robot->getBodyNode("ee")->getTransform().translation(); + BodyNode* bn = robot->getBodyNode("ee"); + Vector3d actual = bn->getTransform().translation(); bool equality = equals(actual, expectedPos[i], 1e-3); EXPECT_TRUE(equality); if(!equality) @@ -102,9 +102,9 @@ TEST(FORWARD_KINEMATICS, TWO_ROLLS) // Set the test cases with the joint values and the expected end-effector // positions const size_t numTests = 2; - Vector2d joints [numTests] = { Vector2d(0.0, DART_PI/2.0), - Vector2d(3*DART_PI/4.0, - -DART_PI/4.0)}; + Vector2d joints [numTests] = { Vector2d(0.0, M_PI/2.0), + Vector2d(3*M_PI/4.0, + -M_PI/4.0)}; Vector3d expectedPos [numTests] = { Vector3d(0.0, -1.0, 1.5), Vector3d(0.0, -2.06, -1.06) }; @@ -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); }