urdf::Pose transformToPose(const sva::PTransformd& X)
{
urdf::Pose pose;
pose.position = fromEigen(X.translation());
pose.rotation = fromEigen(Eigen::Matrix3d(X.rotation().inverse()));
return pose;
}
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;
}