Skip to content

Commit

Permalink
Merge pull request #683 from dartsim/grey/fix_ik/aspects
Browse files Browse the repository at this point in the history 
Fixing Inverse Kinematics for 6.0
  • Loading branch information
@jslee02
jslee02 committed Apr 25, 2016
2 parents ba3502e + e6b8ff9 commit a3be5a9
Show file tree
Hide file tree
Showing 9 changed files with 1,054 additions and 853 deletions.
26 changes: 24 additions & 2 deletions dart/constraint/BalanceConstraint.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -255,7 +255,7 @@ void BalanceConstraint::evalGradient(const Eigen::VectorXd& _x,
}
else
{
mNullSpaceCache.setZero();
mNullSpaceCache.setZero(nDofs, nDofs);
}

std::size_t numEE = skel->getNumEndEffectors();
Expand Down Expand Up @@ -292,7 +292,7 @@ void BalanceConstraint::evalGradient(const Eigen::VectorXd& _x,
}
else
{
mNullSpaceCache.setZero();
mNullSpaceCache.setZero(nDofs, nDofs);
}

for(std::size_t i=0; i<2; ++i)
Expand Down Expand Up @@ -321,6 +321,8 @@ void BalanceConstraint::evalGradient(const Eigen::VectorXd& _x,
}
}
}

convertJacobianMethodOutputToGradient(_grad);
}

//==============================================================================
Expand Down Expand Up @@ -400,5 +402,25 @@ void BalanceConstraint::clearCaches()
mLastCOM = Eigen::Vector3d::Constant(std::nan(""));
}

//==============================================================================
void BalanceConstraint::convertJacobianMethodOutputToGradient(
Eigen::Map<Eigen::VectorXd>& grad)
{
const dart::dynamics::SkeletonPtr& skel = mIK.lock()->getSkeleton();
skel->setVelocities(grad);

mInitialPositionsCache = skel->getPositions();

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(std::size_t i=0; i < skel->getNumDofs(); ++i)
skel->setVelocity(i, 0.0);

grad = skel->getPositions();
grad -= mInitialPositionsCache;
}

} // namespace constraint
} // namespace dart
9 changes: 9 additions & 0 deletions dart/constraint/BalanceConstraint.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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<Eigen::VectorXd>& grad);

/// Pointer to the hierarchical IK that owns this Function. Note that this
/// Function does not work correctly without a HierarchicalIK.
std::weak_ptr<dynamics::HierarchicalIK> mIK;
Expand Down Expand Up @@ -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
Expand Down
38 changes: 24 additions & 14 deletions dart/dynamics/BodyNode.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -1416,6 +1416,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 All @@ -1424,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
Expand All @@ -1444,23 +1460,17 @@ void BodyNode::processNewEntity(Entity* _newChildEntity)
//==============================================================================
void BodyNode::processRemovedEntity(Entity* _oldChildEntity)
{
std::vector<BodyNode*>::iterator it = find(mChildBodyNodes.begin(),
mChildBodyNodes.end(),
_oldChildEntity);
std::vector<BodyNode*>::iterator it = std::find(mChildBodyNodes.begin(),
mChildBodyNodes.end(),
_oldChildEntity);
if(it != mChildBodyNodes.end())
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())
if(std::find(mNonBodyNodeEntities.begin(), mNonBodyNodeEntities.end(),
_oldChildEntity) != mNonBodyNodeEntities.end())
mNonBodyNodeEntities.erase(_oldChildEntity);
}

Expand Down
101 changes: 73 additions & 28 deletions dart/dynamics/InverseKinematics.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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. 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);

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 @@ -698,6 +707,32 @@ InverseKinematics::GradientMethod::getComponentWeights() const
return mGradientP.mComponentWeights;
}

//==============================================================================
void InverseKinematics::GradientMethod::convertJacobianMethodOutputToGradient(
Eigen::VectorXd& grad, const std::vector<std::size_t>& dofs)
{
const SkeletonPtr& skel = mIK->getNode()->getSkeleton();
mInitialPositionsCache = skel->getPositions(dofs);

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(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 std::size_t numJointDofs = joint->getNumDofs();
for(std::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
Expand Down Expand Up @@ -767,6 +802,7 @@ void InverseKinematics::JacobianDLS::computeGradient(
J.transpose()*J).inverse() * J.transpose() * _error;
}

convertJacobianMethodOutputToGradient(_grad, mIK->getDofs());
applyWeights(_grad);
clampGradient(_grad);
}
Expand Down Expand Up @@ -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);
}
Expand All @@ -829,7 +866,7 @@ InverseKinematics::Analytical::Solution::Solution(

//==============================================================================
InverseKinematics::Analytical::UniqueProperties::UniqueProperties(
ExtraDofUtilization_t extraDofUtilization, double extraErrorLengthClamp)
ExtraDofUtilization extraDofUtilization, double extraErrorLengthClamp)
: mExtraDofUtilization(extraDofUtilization),
mExtraErrorLengthClamp(extraErrorLengthClamp)
{
Expand All @@ -838,7 +875,7 @@ InverseKinematics::Analytical::UniqueProperties::UniqueProperties(

//==============================================================================
InverseKinematics::Analytical::UniqueProperties::UniqueProperties(
ExtraDofUtilization_t extraDofUtilization,
ExtraDofUtilization extraDofUtilization,
double extraErrorLengthClamp,
QualityComparison qualityComparator)
: mExtraDofUtilization(extraDofUtilization),
Expand Down Expand Up @@ -991,30 +1028,44 @@ const std::vector<IK::Analytical::Solution>& IK::Analytical::getSolutions(
}

//==============================================================================
static void applyExtraDofGradient(Eigen::VectorXd& grad,
const Eigen::Vector6d& error,
const InverseKinematics* ik,
const std::vector<std::size_t>& extraDofs,
const Eigen::VectorXd& compWeights,
double compClamp)
void InverseKinematics::Analytical::addExtraDofGradient(
Eigen::VectorXd& grad,
const Eigen::Vector6d& error,
ExtraDofUtilization /*utilization*/)
{
const math::Jacobian& J = ik->computeJacobian();
const std::vector<int>& gradMap = ik->getDofMap();
mExtraDofGradCache.resize(mExtraDofs.size());
const math::Jacobian& J = mIK->computeJacobian();
const std::vector<int>& gradMap = mIK->getDofMap();

for(std::size_t i=0; i < extraDofs.size(); ++i)
for(std::size_t i=0; i < mExtraDofs.size(); ++i)
{
std::size_t depIndex = extraDofs[i];
std::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(std::size_t i=0; i < mExtraDofs.size(); ++i)
{
std::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;
}
Expand All @@ -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<int>& gradMap = mIK->getDofMap();
for(std::size_t i=0; i < mExtraDofs.size(); ++i)
Expand All @@ -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<int>& analyticalToDependent = mDofMap;
Expand All @@ -1079,8 +1127,7 @@ void InverseKinematics::Analytical::computeGradient(
}

if(POST_ANALYTICAL == mAnalyticalP.mExtraDofUtilization
&& mExtraDofs.size() > 0
&& (bestValidity != VALID) )
&& mExtraDofs.size() > 0 )
{
setPositions(bestSolution);

Expand All @@ -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);
}
}

Expand All @@ -1115,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;
Expand Down
Loading

0 comments on commit a3be5a9

Please sign in to comment.