void check_values(const std::vector<SimpleFrame*>& targets,
const std::vector<SimpleFrame*>& followers,
const Frame* relativeTo,
const Frame* inCoordinatesOf,
double tolerance)
{
for(std::size_t i=0; i<targets.size(); ++i)
{
Frame* T = targets[i];
Frame* F = followers[i];
const Eigen::Isometry3d& tf_error =
T->getTransform(relativeTo)*F->getTransform(relativeTo).inverse();
Eigen::Vector6d error;
Eigen::AngleAxisd rot_error(tf_error.rotation());
error.block<3,1>(0,0) = rot_error.angle()*rot_error.axis();
error.block<3,1>(3,0) = tf_error.translation();
EXPECT_TRUE( error.norm() < tolerance );
EXPECT_TRUE( equals(T->getSpatialVelocity(relativeTo, inCoordinatesOf),
F->getSpatialVelocity(relativeTo, inCoordinatesOf),
tolerance) );
EXPECT_TRUE( equals(T->getLinearVelocity(relativeTo, inCoordinatesOf),
F->getLinearVelocity(relativeTo, inCoordinatesOf),
tolerance) );
EXPECT_TRUE( equals(T->getAngularVelocity(relativeTo, inCoordinatesOf),
F->getAngularVelocity(relativeTo, inCoordinatesOf),
tolerance) );
EXPECT_TRUE( equals(T->getSpatialAcceleration(relativeTo, inCoordinatesOf),
F->getSpatialAcceleration(relativeTo, inCoordinatesOf),
tolerance) );
EXPECT_TRUE( equals(T->getLinearAcceleration(relativeTo, inCoordinatesOf),
F->getLinearAcceleration(relativeTo, inCoordinatesOf),
tolerance) );
EXPECT_TRUE( equals(T->getAngularAcceleration(relativeTo, inCoordinatesOf),
F->getAngularAcceleration(relativeTo, inCoordinatesOf),
tolerance) );
}
}
bool InverseKinematics::inverseKinematics(const MultiBody& mb, MultiBodyConfig& mbc,
const sva::PTransformd& ef_target)
{
int iter = 0;
bool converged = false;
int dof = 0;
rbd::forwardKinematics(mb, mbc);
Eigen::MatrixXd jacMat;
Eigen::Vector6d v = Eigen::Vector6d::Ones();
Eigen::Vector3d rotErr;
Eigen::VectorXd res = Eigen::VectorXd::Zero(3);
while( ! converged && iter < max_iterations_)
{
jacMat = jac_.jacobian(mb, mbc);
//non-strict zeros in jacobian can be a problem...
jacMat = jacMat.unaryExpr(CwiseRoundOp(-almost_zero_, almost_zero_));
svd_.compute(jacMat, Eigen::ComputeThinU | Eigen::ComputeThinV);
rotErr = sva::rotationError(mbc.bodyPosW[ef_index_].rotation(),
ef_target.rotation());
v << rotErr, ef_target.translation() - mbc.bodyPosW[ef_index_].translation();
converged = v.norm() < threshold_;
res = svd_.solve(v);
dof = 0;
for(auto index : jac_.jointsPath())
{
std::vector<double>& qi = mbc.q[index];
for(auto &qv : qi)
{
qv += lambda_*res[dof];
++dof;
}
}
rbd::forwardKinematics(mb, mbc);
rbd::forwardVelocity(mb, mbc);
iter++;
}
return converged;
}