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; }
CylindricalPositionConstr::CylindricalPositionConstr( PGData* pgdata, int bodyId, const sva::PTransformd& targetFrame, const sva::PTransformd& surfaceFrame) : roboptim::DifferentiableSparseFunction(pgdata->pbSize(), 3, "FreeGripperPositionContact") , pgdata_(pgdata) , bodyIndex_(pgdata->multibody().bodyIndexById(bodyId)) , targetFrame_(targetFrame) , surfaceFrame_(surfaceFrame) , jac_(pgdata->mb(), bodyId, surfaceFrame.translation()) , jacMat_(3, jac_.dof()) {}
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; }