Skip to content

Commit

Permalink
Merge pull request #684 from dartsim/grey/fix_ik/5.1
Browse files Browse the repository at this point in the history
Fixing Inverse Kinematics for 5.1
  • Loading branch information
jslee02 committed Apr 22, 2016
2 parents 388b847 + d42b13a commit 8134697
Show file tree
Hide file tree
Showing 8 changed files with 162 additions and 31 deletions.
2 changes: 1 addition & 1 deletion CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -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")
Expand Down
28 changes: 26 additions & 2 deletions dart/constraint/BalanceConstraint.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -38,6 +38,7 @@

#include "dart/dynamics/Skeleton.h"
#include "dart/dynamics/EndEffector.h"
#include "dart/dynamics/Joint.h"

namespace dart {
namespace constraint {
Expand Down Expand Up @@ -180,6 +181,27 @@ static void addDampedPseudoInverseToGradient(
}
}

//==============================================================================
static void convertJacobianMethodOutputToGradient(
Eigen::Map<Eigen::VectorXd>& grad,
const std::weak_ptr<dynamics::HierarchicalIK>& 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<Eigen::VectorXd> _grad)
Expand Down Expand Up @@ -255,7 +277,7 @@ void BalanceConstraint::evalGradient(const Eigen::VectorXd& _x,
}
else
{
mNullSpaceCache.setZero();
mNullSpaceCache.setZero(nDofs, nDofs);
}

size_t numEE = skel->getNumEndEffectors();
Expand Down Expand Up @@ -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)
Expand Down Expand Up @@ -321,6 +343,8 @@ void BalanceConstraint::evalGradient(const Eigen::VectorXd& _x,
}
}
}

convertJacobianMethodOutputToGradient(_grad, mIK);
}

//==============================================================================
Expand Down
24 changes: 17 additions & 7 deletions dart/dynamics/BodyNode.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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<JacobianNode*>(_newChildEntity))
mChildJacobianNodes.insert(node);

Expand Down Expand Up @@ -1218,13 +1234,7 @@ void BodyNode::processRemovedEntity(Entity* _oldChildEntity)
mChildBodyNodes.erase(it);

if(JacobianNode* node = dynamic_cast<JacobianNode*>(_oldChildEntity))
{
std::unordered_set<JacobianNode*>::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())
Expand Down
5 changes: 5 additions & 0 deletions dart/dynamics/EndEffector.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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; i<mBodyNode->mEndEffectors.size(); ++i)
{
EndEffector* ee = mBodyNode->mEndEffectors[i];
Expand Down
39 changes: 39 additions & 0 deletions dart/dynamics/InverseKinematics.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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<size_t>& 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)
{
Expand Down Expand Up @@ -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;
}

Expand Down Expand Up @@ -650,6 +687,7 @@ void InverseKinematics::JacobianDLS::computeGradient(
J.transpose()*J).inverse() * J.transpose() * _error;
}

convertJacobianMethodOutputToGradient(_grad, mIK->getDofs(), mIK);
applyWeights(_grad);
clampGradient(_grad);
}
Expand Down Expand Up @@ -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);
}
Expand Down
5 changes: 5 additions & 0 deletions dart/dynamics/Skeleton.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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.
}
Expand Down
2 changes: 1 addition & 1 deletion package.xml
Original file line number Diff line number Diff line change
Expand Up @@ -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 -->
<name>dart</name>
<version>5.1.1</version>
<version>5.1.2</version>
<description>
DART (Dynamic Animation and Robotics Toolkit) is a collaborative,
cross-platform, open source library created by the Georgia Tech Graphics
Expand Down
88 changes: 68 additions & 20 deletions unittests/testForwardKinematics.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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) };
Expand All @@ -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)
Expand Down Expand Up @@ -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) };

Expand All @@ -130,7 +130,8 @@ TEST(FORWARD_KINEMATICS, TWO_ROLLS)
Eigen::MatrixXd finiteDifferenceJacobian(
const SkeletonPtr& skeleton,
const Eigen::VectorXd& q,
const std::vector<size_t>& active_indices)
const std::vector<size_t>& active_indices,
JacobianNode* node)
{
Eigen::MatrixXd J(3, q.size());
for(int i=0; i < q.size(); ++i)
Expand All @@ -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;
Expand All @@ -162,12 +161,12 @@ Eigen::MatrixXd finiteDifferenceJacobian(
Eigen::MatrixXd standardJacobian(
const SkeletonPtr& skeleton,
const Eigen::VectorXd& q,
const std::vector<size_t>& active_indices)
const std::vector<size_t>& 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)
Expand All @@ -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<size_t> 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);
}
Expand Down

0 comments on commit 8134697

Please sign in to comment.