00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021 #include "KNI_InvKin/KatanaKinematics6M90G.h"
00022 #include "common/MathHelperFunctions.h"
00023 #include <algorithm>
00024
00025
00026 using namespace std;
00027
00028 namespace KNI {
00029
00030 const double KatanaKinematics6M90G::_tolerance = 0.001;
00031 const int KatanaKinematics6M90G::_nrOfPossibleSolutions = 8;
00032
00033 void
00034 KatanaKinematics6M90G::DK(coordinates& solution, encoders const& current_encoders) const {
00035 using namespace KNI_MHF;
00036
00037
00038 double x0, x1, x2, x3;
00039 double y0, y1, y2, y3;
00040 double z0, z1, z2, z3;
00041 double R13, R23, R33, R31, R32;
00042
00043 angles current_angles(6);
00044 for(int z = 0; z < 6; ++z) {
00045 current_angles[z] = enc2rad(current_encoders[z], _parameters[z].angleOffset, _parameters[z].epc, _parameters[z].encOffset, _parameters[z].rotDir);
00046
00047
00048
00049
00050
00051
00052 }
00053
00054
00055 current_angles[1] = current_angles[1] - M_PI/2.0;
00056 current_angles[2] = current_angles[2] - M_PI;
00057 current_angles[3] = M_PI - current_angles[3];
00058
00059 coordinates pose(6);
00060
00061 angles cx(current_angles.size()), sx(current_angles.size());
00062 angles::iterator cx_iter, sx_iter;
00063
00064 angles angle = current_angles;
00065
00066 angle[2] = angle[1]+angle[2];
00067 angle[3] = angle[2]+angle[3];
00068
00069 cx_iter = cx.begin();
00070 sx_iter = sx.begin();
00071 std::transform(angle.begin(), angle.end(), sx_iter, unary_precalc_sin<double>() );
00072 std::transform(angle.begin(), angle.end(), cx_iter, unary_precalc_cos<double>() );
00073
00074 R13 = (-1*cx[0]*cx[3]*cx[4])-(sx[0]*sx[4]);
00075 R23 = (-1*sx[0]*cx[3]*cx[4])+(cx[0]*sx[4]);
00076 R33 = sx[3]*cx[4];
00077 R31 = cx[3];
00078 R32 =(-1)*sx[3]*sx[4];
00079
00080
00081 x0 = cx[0]*sx[1];
00082 x1 = cx[0]*sx[2];
00083 x2 = cx[0]*sx[3];
00084 x3 = -cx[0]*cx[3]*cx[4]-sx[0]*sx[4];
00085 pose[0] = x0*_length[0]+x1*_length[1]+x2*_length[2]+x3*_length[3];
00086
00087
00088 y0 = sx[0]*sx[1];
00089 y1 = sx[0]*sx[2];
00090 y2 = sx[0]*sx[3];
00091 y3 = -sx[0]*cx[3]*cx[4]+cx[0]*sx[4];
00092 pose[1] = y0*_length[0]+y1*_length[1]+y2*_length[2]+y3*_length[3];
00093
00094
00095 z0 = cx[1];
00096 z1 = cx[2];
00097 z2 = cx[3];
00098 z3 = cx[4]*sx[3];
00099 pose[2] = z0*_length[0]+z1*_length[1]+z2*_length[2]+z3*_length[3];
00100
00101
00102 pose[4] = acos(R33);
00103 if(pose[4]==0) {
00104 pose[3] = atan2(pose[1],pose[0]);
00105 pose[5] = 0;
00106 } else if(pose[4]==M_PI) {
00107 pose[3] = atan2(pose[1],pose[0])+M_PI/2;
00108 pose[5] = M_PI/2;
00109 } else {
00110 pose[3] = atan2(R13,-R23);
00111 pose[5] = atan2(R31,R32);
00112 }
00113
00114
00115 std::swap(solution, pose);
00116 }
00117
00118 void
00119 KatanaKinematics6M90G::init( metrics const& length, parameter_container const& parameters ) {
00120 assert( (length.size() == 4) && "You have to provide the metrics for exactly 4 links" );
00121 assert( (parameters.size() == 6) && "You have to provide exactly 5 motor parameters" );
00122 _setLength( length );
00123 _setParameters ( parameters );
00124 }
00125
00126
00127 void
00128 KatanaKinematics6M90G::IK_theta234theta5(angles_calc& angle, const position &p_gr) const {
00129 using namespace KNI_MHF;
00130
00131 angle.theta234 = -acotan( ( (p_gr.x * p_gr.z * cos(angle.theta1) ) -
00132 sqrt( ( -pow2(p_gr.z) ) *
00133 ( -pow2(_length[3]) + pow2(p_gr.x) + pow2(p_gr.z) ) * pow2(sin(angle.theta1))
00134 )
00135 ) / pow2(p_gr.z)
00136 );
00137
00138 angle.theta5 = acos( p_gr.z/(_length[3]*sin(angle.theta234)) );
00139
00140 if(p_gr.z==0) {
00141 angle.theta234=0;
00142 angle.theta5=angle.theta1-atan1(-p_gr.x,-p_gr.y);
00143 }
00144
00145 bool griptest;
00146 griptest = GripperTest(p_gr, angle);
00147 if(!griptest) {
00148 angle.theta5=-angle.theta5;
00149 griptest=GripperTest(p_gr, angle);
00150 if(!griptest) {
00151 angle.theta234 = -acotan( ( ( p_gr.x * p_gr.z * cos(angle.theta1) ) +
00152 sqrt( ( -pow2(p_gr.z) ) *
00153 ( -pow2(_length[3]) + pow2(p_gr.x) + pow2(p_gr.z) ) * pow2(sin(angle.theta1))
00154 )
00155 ) / pow2(p_gr.z)
00156 );
00157 angle.theta5 = acos( p_gr.z / (_length[3]*sin(angle.theta234)) );
00158 if(p_gr.z==0) {
00159 angle.theta234=-M_PI;
00160 angle.theta5=atan1(p_gr.x,p_gr.y) - angle.theta1;
00161 }
00162
00163 griptest=GripperTest(p_gr, angle);
00164 if(!griptest) {
00165 angle.theta5=-angle.theta5;
00166 }
00167 }
00168 }
00169
00170 }
00171
00172 bool
00173 KatanaKinematics6M90G::GripperTest(const position &p_gr, const angles_calc &angle) const {
00174 using namespace KNI_MHF;
00175 double xgr2, ygr2, zgr2;
00176
00177 xgr2 = -_length[3]*(cos(angle.theta1)*cos(angle.theta234)*cos(angle.theta5)+sin(angle.theta1)*sin(angle.theta5));
00178 ygr2 = -_length[3]*(sin(angle.theta1)*cos(angle.theta234)*cos(angle.theta5)-cos(angle.theta1)*sin(angle.theta5));
00179 zgr2 = _length[3]*sin(angle.theta234)*cos(angle.theta5);
00180
00181 if((pow2(p_gr.x-xgr2)+pow2(p_gr.y-ygr2)+pow2(p_gr.z-zgr2))>=_tolerance)
00182 return false;
00183
00184 return true;
00185 }
00186
00187 void
00188 KatanaKinematics6M90G::IK_b1b2costh3_6MS(angles_calc &angle, const position &p) const {
00189 using namespace KNI_MHF;
00190
00191 double xg, yg, zg;
00192 double d5 = _length[2] + _length[3];
00193 xg = p.x + ( _length[3] * cos(angle.theta1) * sin(angle.theta234) );
00194 yg = p.y + ( _length[3] * sin(angle.theta1) * sin(angle.theta234) );
00195 zg = p.z + ( _length[3] * cos(angle.theta234) );
00196
00197
00198 angle.b1 = xg*cos(angle.theta1) + yg*sin(angle.theta1) - d5*sin(angle.theta234);
00199 angle.b2 = zg - d5*cos(angle.theta234);
00200 angle.costh3 = -( pow2(angle.b1) + pow2(angle.b2) - pow2(_length[0]) - pow2(_length[1]) ) / ( 2.0*_length[0]*_length[1] );
00201
00202 }
00203
00204 void
00205 KatanaKinematics6M90G::thetacomp(angles_calc &angle, const position &p_m) const {
00206 using namespace KNI_MHF;
00207
00208 angle.theta2 = -M_PI/2.0 - ( atan0(angle.b1,angle.b2)+atan0(_length[0]+_length[1]*cos(angle.theta3),_length[1]*sin(angle.theta3)) );
00209 angle.theta4 = angle.theta234-angle.theta2-angle.theta3;
00210
00211 if(!PositionTest6MS(angle,p_m)) {
00212 angle.theta2 = angle.theta2+M_PI;
00213 angle.theta4 = angle.theta234-angle.theta2-angle.theta3;
00214 }
00215
00216 }
00217
00218 bool
00219 KatanaKinematics6M90G::PositionTest6MS(const angles_calc &a, const position &p) const {
00220 using namespace KNI_MHF;
00221 double temp, xm2, ym2, zm2;
00222
00223 temp = _length[0]*sin(a.theta2)+_length[1]*sin(a.theta2+a.theta3)+_length[2]*sin(a.theta234);
00224 xm2 = cos(a.theta1)*temp;
00225 ym2 = sin(a.theta1)*temp;
00226 zm2 = _length[0]*cos(a.theta2)+_length[1]*cos(a.theta2+a.theta3)+_length[2]*cos(a.theta234);
00227
00228 if((pow2(p.x-xm2)+pow2(p.y-ym2)+pow2(p.z-zm2))>=_tolerance)
00229 return false;
00230
00231 return true;
00232 }
00233
00234 bool
00235 KatanaKinematics6M90G::angledef(angles_calc &a) const {
00236 using namespace KNI_MHF;
00237
00238
00239 a.theta2=anglereduce(a.theta2+M_PI/2.0);
00240 a.theta3=anglereduce(a.theta3+M_PI);
00241 a.theta4=anglereduce(M_PI-a.theta4);
00242 a.theta5=anglereduce(a.theta5);
00243
00244 if(a.theta1>_parameters[0].angleStop) {
00245 a.theta1=a.theta1-2.0*M_PI;
00246 }
00247 if(a.theta2>M_PI) {
00248 a.theta2=a.theta2-2.0*M_PI;
00249 }
00250 if(a.theta5<_parameters[4].angleOffset) {
00251 a.theta5=a.theta5+2.0*M_PI;
00252 }
00253
00254 return AnglePositionTest(a);
00255
00256 }
00257
00258 bool
00259 KatanaKinematics6M90G::AnglePositionTest(const angles_calc &a) const {
00260
00261 if( (a.theta1+0.0087<_parameters[0].angleOffset)||(a.theta1>_parameters[0].angleStop) )
00262 return false;
00263
00264 if( (a.theta2-0.0087>_parameters[1].angleOffset)||(a.theta2<_parameters[1].angleStop) )
00265 return false;
00266
00267 if( (a.theta3<_parameters[2].angleOffset)||(a.theta3>_parameters[2].angleStop) )
00268 return false;
00269
00270 if( (a.theta4<_parameters[3].angleOffset)||(a.theta4>_parameters[3].angleStop) )
00271 return false;
00272
00273 if( (a.theta5<_parameters[4].angleOffset)||(a.theta5>_parameters[4].angleStop) )
00274 return false;
00275
00276 return true;
00277 }
00278
00279
00280
00281
00282 void
00283 KatanaKinematics6M90G::IK(encoders::iterator solution, coordinates const& pose, encoders const& current_encoders) const {
00284 using namespace KNI_MHF;
00285
00286
00287
00288
00289
00290
00291
00292 position p_gr;
00293 position p_m;
00294 angles_container angle(_nrOfPossibleSolutions);
00295
00296
00297 p_gr.x = _length[3]*sin(pose[4])*sin(pose[3]);
00298 p_gr.y = -_length[3]*sin(pose[4])*cos(pose[3]);
00299 p_gr.z = _length[3]*cos(pose[4]);
00300
00301 p_m.x = pose[0]-p_gr.x;
00302 p_m.y = pose[1]-p_gr.y;
00303 p_m.z = pose[2]-p_gr.z;
00304
00305
00306 angle[0].theta1 = atan1(p_m.x,p_m.y);
00307 angle[4].theta1 = angle[0].theta1+M_PI;
00308
00309
00310
00311 if(angle[0].theta1>_parameters[0].angleStop)
00312 angle[0].theta1=angle[0].theta1-2.0*M_PI;
00313
00314 if(angle[0].theta1<_parameters[0].angleOffset)
00315 angle[0].theta1=angle[0].theta1+2.0*M_PI;
00316
00317 if(angle[4].theta1>_parameters[0].angleStop)
00318 angle[4].theta1=angle[4].theta1-2.0*M_PI;
00319
00320 if(angle[4].theta1<_parameters[0].angleOffset)
00321 angle[4].theta1=angle[4].theta1+2.0*M_PI;
00322
00323
00324
00325
00326 IK_theta234theta5(angle[0], p_gr);
00327 IK_b1b2costh3_6MS(angle[0], p_m);
00328
00329 angle[1]=angle[0];
00330 angle[0].theta3 = acos(angle[0].costh3)-M_PI;
00331 thetacomp(angle[0], p_m);
00332 angle[1].theta3 = -acos(angle[1].costh3)+M_PI;
00333 thetacomp(angle[1], p_m);
00334
00335
00336 angle[2].theta1=angle[0].theta1;
00337 angle[2].theta234=angle[0].theta234-M_PI;
00338 angle[2].theta5=M_PI-angle[0].theta5;
00339
00340 IK_b1b2costh3_6MS(angle[2], p_m);
00341 angle[3]=angle[2];
00342 angle[2].theta3 = acos(angle[2].costh3)-M_PI;
00343 thetacomp(angle[2], p_m);
00344 angle[3].theta3 = -acos(angle[3].costh3)+M_PI;
00345 thetacomp(angle[3], p_m);
00346
00347
00348
00349
00350 IK_theta234theta5(angle[4], p_gr);
00351 IK_b1b2costh3_6MS(angle[4], p_m);
00352
00353 angle[5]=angle[4];
00354 angle[4].theta3 = acos(angle[4].costh3)-M_PI;
00355 thetacomp(angle[4], p_m);
00356 angle[5].theta3 = -acos(angle[5].costh3)+M_PI;
00357 thetacomp(angle[5], p_m);
00358
00359
00360 angle[6].theta1=angle[4].theta1;
00361 angle[6].theta234=angle[4].theta234-M_PI;
00362 angle[6].theta5=M_PI-angle[4].theta5;
00363 IK_b1b2costh3_6MS(angle[6], p_m);
00364 angle[7]=angle[6];
00365 angle[6].theta3 = acos(angle[6].costh3)-M_PI;
00366 thetacomp(angle[6], p_m);
00367 angle[7].theta3 = -acos(angle[7].costh3)+M_PI;
00368 thetacomp(angle[7], p_m);
00369
00370 for( ::std::vector<angles_calc>::iterator iter = angle.begin(); iter != angle.end(); ) {
00371 if( pow2(iter->costh3) <= 1.0) {
00372 if(!angledef(*iter))
00373 iter = angle.erase(iter);
00374 else
00375 ++iter;
00376 continue;
00377 }
00378 iter = angle.erase(iter);
00379 }
00380
00381
00382 if(angle.size() == 0) {
00383 throw NoSolutionException();
00384 }
00385
00386 ::std::vector< ::std::vector<int> > PossibleTargetsInEncoders;
00387 for( ::std::vector<angles_calc>::iterator i = angle.begin(); i != angle.end(); ++i ) {
00388 ::std::vector<int> solution(5);
00389
00390 solution[0] = rad2enc(i->theta1, _parameters[0].angleOffset, _parameters[0].epc, _parameters[0].encOffset, _parameters[0].rotDir);
00391 solution[1] = rad2enc(i->theta2, _parameters[1].angleOffset, _parameters[1].epc, _parameters[1].encOffset, _parameters[1].rotDir);
00392 solution[2] = rad2enc(i->theta3, _parameters[2].angleOffset, _parameters[2].epc, _parameters[2].encOffset, _parameters[2].rotDir);
00393 solution[3] = rad2enc(i->theta4, _parameters[3].angleOffset, _parameters[3].epc, _parameters[3].encOffset, _parameters[3].rotDir);
00394 solution[4] = rad2enc(i->theta5, _parameters[4].angleOffset, _parameters[4].epc, _parameters[4].encOffset, _parameters[4].rotDir);
00395
00396 PossibleTargetsInEncoders.push_back(solution);
00397 }
00398
00399
00400 ::std::vector< ::std::vector<int> >::const_iterator sol = KinematicsDefaultEncMinAlgorithm()(PossibleTargetsInEncoders.begin(), PossibleTargetsInEncoders.end(), current_encoders.begin(), current_encoders.end());
00401
00402 assert( sol != PossibleTargetsInEncoders.end() && "All solutions are out of range");
00403
00404
00405 encoders::iterator gripper_encoder_iter = std::copy( (*sol).begin(), (*sol).end(), solution );
00406 *gripper_encoder_iter = current_encoders[5];
00407
00408 }
00409
00410
00411
00412 }
00413