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