00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026 #include <moveit/kdl_kinematics_plugin/chainiksolver_vel_pinv_mimic.hpp>
00027 #include <ros/console.h>
00028
00029 namespace KDL
00030 {
00031 ChainIkSolverVel_pinv_mimic::ChainIkSolverVel_pinv_mimic(const Chain& _chain, int _num_mimic_joints,
00032 int _num_redundant_joints, bool _position_ik, double _eps,
00033 int _maxiter)
00034 : chain(_chain)
00035 , jnt2jac(chain)
00036 , jac(chain.getNrOfJoints())
00037 , U(6, JntArray(chain.getNrOfJoints() - _num_mimic_joints))
00038 , S(chain.getNrOfJoints() - _num_mimic_joints)
00039 , V(chain.getNrOfJoints() - _num_mimic_joints, JntArray(chain.getNrOfJoints() - _num_mimic_joints))
00040 , tmp(chain.getNrOfJoints() - _num_mimic_joints)
00041 , jac_reduced(chain.getNrOfJoints() - _num_mimic_joints)
00042 , qdot_out_reduced(chain.getNrOfJoints() - _num_mimic_joints)
00043 , U_translate(MatrixXd::Zero(3, chain.getNrOfJoints() - _num_mimic_joints))
00044 , S_translate(VectorXd::Zero(chain.getNrOfJoints() - _num_mimic_joints))
00045 , V_translate(MatrixXd::Zero(chain.getNrOfJoints() - _num_mimic_joints, chain.getNrOfJoints() - _num_mimic_joints))
00046 , tmp_translate(VectorXd::Zero(chain.getNrOfJoints() - _num_mimic_joints))
00047 , jac_locked(chain.getNrOfJoints() - _num_redundant_joints - _num_mimic_joints)
00048 , qdot_out_reduced_locked(chain.getNrOfJoints() - _num_mimic_joints - _num_redundant_joints)
00049 , qdot_out_locked(chain.getNrOfJoints() - _num_redundant_joints)
00050 , svd(jac_reduced)
00051 , eps(_eps)
00052 , maxiter(_maxiter)
00053 , num_mimic_joints(_num_mimic_joints)
00054 , position_ik(_position_ik)
00055 , U_locked(MatrixXd::Zero(6, chain.getNrOfJoints() - _num_mimic_joints - _num_redundant_joints))
00056 , S_locked(VectorXd::Zero(chain.getNrOfJoints() - _num_mimic_joints - _num_redundant_joints))
00057 , V_locked(MatrixXd::Zero(chain.getNrOfJoints() - _num_mimic_joints - _num_redundant_joints,
00058 chain.getNrOfJoints() - _num_mimic_joints - _num_redundant_joints))
00059 , tmp_locked(VectorXd::Zero(chain.getNrOfJoints() - _num_mimic_joints - _num_redundant_joints))
00060 , U_translate_locked(MatrixXd::Zero(3, chain.getNrOfJoints() - _num_mimic_joints - _num_redundant_joints))
00061 , S_translate_locked(VectorXd::Zero(chain.getNrOfJoints() - _num_mimic_joints - _num_redundant_joints))
00062 , V_translate_locked(MatrixXd::Zero(chain.getNrOfJoints() - _num_mimic_joints - _num_redundant_joints,
00063 chain.getNrOfJoints() - _num_mimic_joints - _num_redundant_joints))
00064 , tmp_translate_locked(VectorXd::Zero(chain.getNrOfJoints() - _num_mimic_joints - _num_redundant_joints))
00065 , num_redundant_joints(_num_redundant_joints)
00066 , redundant_joints_locked(false)
00067 {
00068 mimic_joints_.resize(chain.getNrOfJoints());
00069 for (std::size_t i = 0; i < mimic_joints_.size(); ++i)
00070 mimic_joints_[i].reset(i);
00071 }
00072
00073 ChainIkSolverVel_pinv_mimic::~ChainIkSolverVel_pinv_mimic()
00074 {
00075 }
00076
00077 bool ChainIkSolverVel_pinv_mimic::setMimicJoints(const std::vector<kdl_kinematics_plugin::JointMimic>& mimic_joints)
00078 {
00079 if (mimic_joints.size() != chain.getNrOfJoints())
00080 return false;
00081
00082 for (std::size_t i = 0; i < mimic_joints.size(); ++i)
00083 {
00084 if (mimic_joints[i].map_index >= chain.getNrOfJoints())
00085 return false;
00086 }
00087 mimic_joints_ = mimic_joints;
00088 return true;
00089 }
00090
00091 bool ChainIkSolverVel_pinv_mimic::setRedundantJointsMapIndex(
00092 const std::vector<unsigned int>& redundant_joints_map_index)
00093 {
00094 if (redundant_joints_map_index.size() != chain.getNrOfJoints() - num_mimic_joints - num_redundant_joints)
00095 {
00096 ROS_ERROR("Map index size: %d does not match expected size. No. of joints: %d, num_mimic_joints: %d, "
00097 "num_redundant_joints: %d",
00098 (int)redundant_joints_map_index.size(), (int)chain.getNrOfJoints(), (int)num_mimic_joints,
00099 (int)num_redundant_joints);
00100 return false;
00101 }
00102
00103 for (std::size_t i = 0; i < redundant_joints_map_index.size(); ++i)
00104 {
00105 if (redundant_joints_map_index[i] >= chain.getNrOfJoints() - num_mimic_joints)
00106 return false;
00107 }
00108 locked_joints_map_index = redundant_joints_map_index;
00109 return true;
00110 }
00111
00112 bool ChainIkSolverVel_pinv_mimic::jacToJacReduced(const Jacobian& jac, Jacobian& jac_reduced_l)
00113 {
00114 jac_reduced_l.data.setZero();
00115 for (std::size_t i = 0; i < chain.getNrOfJoints(); ++i)
00116 {
00117 Twist vel1 = jac_reduced_l.getColumn(mimic_joints_[i].map_index);
00118 Twist vel2 = jac.getColumn(i);
00119 Twist result = vel1 + (mimic_joints_[i].multiplier * vel2);
00120 jac_reduced_l.setColumn(mimic_joints_[i].map_index, result);
00121 }
00122 return true;
00123 }
00124
00125 bool ChainIkSolverVel_pinv_mimic::jacToJacLocked(const Jacobian& jac, Jacobian& jac_locked)
00126 {
00127 jac_locked.data.setZero();
00128 for (std::size_t i = 0; i < chain.getNrOfJoints() - num_mimic_joints - num_redundant_joints; ++i)
00129 {
00130 jac_locked.setColumn(i, jac.getColumn(locked_joints_map_index[i]));
00131 }
00132 return true;
00133 }
00134
00135 int ChainIkSolverVel_pinv_mimic::CartToJntRedundant(const JntArray& q_in, const Twist& v_in, JntArray& qdot_out)
00136 {
00137 qdot_out.data.setZero();
00138
00139
00140 if (num_mimic_joints > 0)
00141 {
00142 jnt2jac.JntToJac(q_in, jac);
00143
00144 jacToJacReduced(jac, jac_reduced);
00145 }
00146 else
00147 jnt2jac.JntToJac(q_in, jac_reduced);
00148
00149
00150 jacToJacLocked(jac_reduced, jac_locked);
00151
00152
00153
00154
00155
00156 int ret;
00157 if (!position_ik)
00158 ret = svd_eigen_HH(jac_locked.data, U_locked, S_locked, V_locked, tmp_locked, maxiter);
00159 else
00160 ret =
00161 svd_eigen_HH(jac_locked.data.topLeftCorner(3, chain.getNrOfJoints() - num_mimic_joints - num_redundant_joints),
00162 U_translate_locked, S_translate_locked, V_translate_locked, tmp_translate_locked, maxiter);
00163
00164 double sum;
00165 unsigned int i, j;
00166
00167
00168
00169
00170
00171 unsigned int rows;
00172 if (!position_ik)
00173 rows = jac_locked.rows();
00174 else
00175 rows = 3;
00176
00177
00178 for (i = 0; i < jac_locked.columns(); i++)
00179 {
00180 sum = 0.0;
00181 for (j = 0; j < rows; j++)
00182 {
00183 if (!position_ik)
00184 sum += U_locked(j, i) * v_in(j);
00185 else
00186 sum += U_translate_locked(j, i) * v_in(j);
00187 }
00188
00189
00190 if (!position_ik)
00191 tmp(i) = sum * (fabs(S_locked(i)) < eps ? 0.0 : 1.0 / S_locked(i));
00192 else
00193 tmp(i) = sum * (fabs(S_translate_locked(i)) < eps ? 0.0 : 1.0 / S_translate_locked(i));
00194 }
00195
00196
00197 for (i = 0; i < jac_locked.columns(); i++)
00198 {
00199 sum = 0.0;
00200 for (j = 0; j < jac_locked.columns(); j++)
00201 {
00202 if (!position_ik)
00203 sum += V_locked(i, j) * tmp(j);
00204 else
00205 sum += V_translate_locked(i, j) * tmp(j);
00206 }
00207
00208 if (num_mimic_joints > 0)
00209 qdot_out_reduced_locked(i) = sum;
00210 else
00211 qdot_out_locked(i) = sum;
00212 }
00213 ROS_DEBUG_STREAM_NAMED("kdl", "Solution:");
00214
00215 if (num_mimic_joints > 0)
00216 {
00217 for (i = 0; i < chain.getNrOfJoints() - num_mimic_joints - num_redundant_joints; ++i)
00218 {
00219 qdot_out_reduced(locked_joints_map_index[i]) = qdot_out_reduced_locked(i);
00220 }
00221 for (i = 0; i < chain.getNrOfJoints(); ++i)
00222 {
00223 qdot_out(i) = qdot_out_reduced(mimic_joints_[i].map_index) * mimic_joints_[i].multiplier;
00224 }
00225 }
00226 else
00227 {
00228 for (i = 0; i < chain.getNrOfJoints() - num_redundant_joints; ++i)
00229 {
00230 qdot_out(locked_joints_map_index[i]) = qdot_out_locked(i);
00231 }
00232 }
00233
00234
00235
00236 return ret;
00237 }
00238
00239 int ChainIkSolverVel_pinv_mimic::CartToJnt(const JntArray& q_in, const Twist& v_in, JntArray& qdot_out)
00240 {
00241 if (redundant_joints_locked)
00242 return CartToJntRedundant(q_in, v_in, qdot_out);
00243
00244
00245
00246 if (num_mimic_joints > 0)
00247 {
00248 jnt2jac.JntToJac(q_in, jac);
00249
00250 jacToJacReduced(jac, jac_reduced);
00251 }
00252 else
00253 jnt2jac.JntToJac(q_in, jac_reduced);
00254
00255
00256
00257
00258
00259 int ret;
00260 if (!position_ik)
00261 ret = svd.calculate(jac_reduced, U, S, V, maxiter);
00262 else
00263 ret = svd_eigen_HH(jac_reduced.data.topLeftCorner(3, chain.getNrOfJoints() - num_mimic_joints), U_translate,
00264 S_translate, V_translate, tmp_translate, maxiter);
00265
00266 double sum;
00267 unsigned int i, j;
00268
00269
00270
00271
00272
00273 unsigned int rows;
00274 if (!position_ik)
00275 rows = jac_reduced.rows();
00276 else
00277 rows = 3;
00278
00279
00280 for (i = 0; i < jac_reduced.columns(); i++)
00281 {
00282 sum = 0.0;
00283 for (j = 0; j < rows; j++)
00284 {
00285 if (!position_ik)
00286 sum += U[j](i) * v_in(j);
00287 else
00288 sum += U_translate(j, i) * v_in(j);
00289 }
00290
00291
00292 if (!position_ik)
00293 tmp(i) = sum * (fabs(S(i)) < eps ? 0.0 : 1.0 / S(i));
00294 else
00295 tmp(i) = sum * (fabs(S_translate(i)) < eps ? 0.0 : 1.0 / S_translate(i));
00296 }
00297
00298
00299 for (i = 0; i < jac_reduced.columns(); i++)
00300 {
00301 sum = 0.0;
00302 for (j = 0; j < jac_reduced.columns(); j++)
00303 {
00304 if (!position_ik)
00305 sum += V[i](j) * tmp(j);
00306 else
00307 sum += V_translate(i, j) * tmp(j);
00308 }
00309
00310 if (num_mimic_joints > 0)
00311 qdot_out_reduced(i) = sum;
00312 else
00313 qdot_out(i) = sum;
00314 }
00315 ROS_DEBUG_STREAM_NAMED("kdl", "Solution:");
00316 if (num_mimic_joints > 0)
00317 {
00318 for (i = 0; i < chain.getNrOfJoints(); ++i)
00319 {
00320 qdot_out(i) = qdot_out_reduced(mimic_joints_[i].map_index) * mimic_joints_[i].multiplier;
00321 }
00322 }
00323
00324 return ret;
00325 }
00326 }