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;
}
void FunctionApproximatorIRFRLS::proj(const MatrixXd& vecs, const MatrixXd& periods, const VectorXd& phases, Eigen::MatrixXd& projected)
{
projected = vecs * periods.transpose();
projected.rowwise() += phases.transpose();
projected = projected.unaryExpr(ptr_fun(double_cosine));
}