47 const std::vector<kdl_kinematics_plugin::JointMimic>& mimic_joints,
48 bool position_ik,
double threshold)
49 : mimic_joints_(mimic_joints)
50 , num_mimic_joints_(countMimicJoints(mimic_joints))
53 , position_ik_(position_ik)
54 , threshold_(threshold)
55 , jac_(chain_.getNrOfJoints())
56 , jac_reduced_(chain_.getNrOfJoints() - num_mimic_joints_)
58 assert(mimic_joints_.size() == chain.getNrOfJoints());
60 for (
const auto& item : mimic_joints)
61 assert(item.map_index < chain_.getNrOfJoints());
73bool ChainIkSolverVelMimicSVD::jacToJacReduced(
const Jacobian& jac, Jacobian& jac_reduced)
88 const Eigen::VectorXd& joint_weights,
89 const Eigen::Matrix<double, 6, 1>& cartesian_weights)
92 if (num_mimic_joints_ > 0)
94 jnt2jac_.JntToJac(q_in, jac_);
96 jacToJacReduced(jac_, jac_reduced_);
100 jnt2jac_.JntToJac(q_in, jac_reduced_);
104 auto& jac = jac_reduced_.data;
105 const Eigen::Index rows = position_ik_ ? 3 : 6;
106 jac.topRows(rows) *= joint_weights.asDiagonal();
107 jac.topRows(rows).transpose() *= cartesian_weights.topRows(rows).asDiagonal();
110 Eigen::Matrix<double, 6, 1> vin;
111 vin.topRows<3>() = Eigen::Map<const Eigen::Array3d>(v_in.vel.data, 3) * cartesian_weights.topRows<3>().array();
112 vin.bottomRows<3>() = Eigen::Map<const Eigen::Array3d>(v_in.rot.data, 3) * cartesian_weights.bottomRows<3>().array();
114 const auto jacobian = jac.topRows(rows);
115 const Eigen::VectorXd x_sol =
118 if (num_mimic_joints_ > 0)
120 qdot_out_reduced_.noalias() = x_sol;
121 qdot_out_reduced_.array() *= joint_weights.array();
122 for (
unsigned int i = 0; i < chain_.getNrOfJoints(); ++i)
123 qdot_out(i) = qdot_out_reduced_[mimic_joints_[i].map_index] * mimic_joints_[i].multiplier;
127 qdot_out.data.noalias() = x_sol;
128 qdot_out.data.array() *= joint_weights.array();