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