00001 #include <kdl/kinfam/serialchain.hpp>
00002 #include <kdl/frames.hpp>
00003 #include <kdl/framevel.hpp>
00004 #include <kdl/frames_io.hpp>
00005 #include <kdl/kinfam/crs450.hpp>
00006 #include <kdl/kinfam/kuka160.hpp>
00007 #include <kdl/kinfam/serialchaincartpos2jnt.hpp>
00008 #include <kdl/kinfam/lineartransmission.hpp>
00009
00010 using namespace KDL;
00011
00012
00013
00014 void CompareFamilies(KinematicFamily* KF1,KinematicFamily* KF2) {
00015 Jnt2CartPos* jnt2cartpos1 = KF1->createJnt2CartPos();
00016 Jnt2CartPos* jnt2cartpos2 = KF2->createJnt2CartPos();
00017 JointVector q(6);
00018 q[0] = 0*KDL::deg2rad;
00019 q[1] = 10*KDL::deg2rad;
00020 q[2] = 20*KDL::deg2rad;
00021 q[3] = 30*KDL::deg2rad;
00022 q[4] = 40*KDL::deg2rad;
00023 q[5] = 50*KDL::deg2rad;
00024 Frame F1,F2;
00025 jnt2cartpos1->evaluate(q);
00026 jnt2cartpos1->getFrame(F1);
00027 jnt2cartpos2->evaluate(q);
00028 jnt2cartpos2->getFrame(F2);
00029 if (!Equal(F1,F2,1E-7)) {
00030 std::cout << "the two kinematic families do not give the same result." << std::endl;
00031 std::cout << "Result of first kinematic family " << std::endl;
00032 std::cout << F1 << std::endl;
00033 std::cout << "Result of second kinematic family " << std::endl;
00034 std::cout << F2 << std::endl;
00035 exit(1);
00036 }
00037 delete jnt2cartpos1;
00038 delete jnt2cartpos2;
00039 }
00040
00041
00042
00043
00044 class TestForwardAndInverse {
00045 KinematicFamily* family;
00046 Jnt2CartPos* jnt2cartpos;
00047 Frame F_base_ee;
00048 Frame F_base_ee2;
00049 JointVector q_solved;
00050 JointVector q_initial;
00051 public:
00052 CartPos2Jnt* cartpos2jnt;
00053 public:
00054 static void TestFamily(KinematicFamily* _family) {
00055 JointVector q_initial(6);
00056 q_initial[0] = 0*KDL::deg2rad;
00057 q_initial[1] = 0*KDL::deg2rad;
00058 q_initial[2] = 90*KDL::deg2rad;
00059 q_initial[3] = 0*KDL::deg2rad;
00060 q_initial[4] = 90*KDL::deg2rad;
00061 q_initial[5] = 0*KDL::deg2rad;
00062 TestForwardAndInverse testobj(_family,q_initial);
00063 JointVector q(6);
00064 q[0] = 0*KDL::deg2rad;
00065 q[1] = 10*KDL::deg2rad;
00066 q[2] = 20*KDL::deg2rad;
00067 q[3] = 30*KDL::deg2rad;
00068 q[4] = 40*KDL::deg2rad;
00069 q[5] = 50*KDL::deg2rad;
00070 testobj.test(q);
00071
00072 q[0] = -10*KDL::deg2rad;
00073 q[1] = -10*KDL::deg2rad;
00074 q[2] = 40*KDL::deg2rad;
00075 q[3] = -30*KDL::deg2rad;
00076 q[4] = 20*KDL::deg2rad;
00077 q[5] = 60*KDL::deg2rad;
00078 testobj.test(q);
00079
00080 }
00081
00082
00083
00084
00085
00086
00087 class TestForwardPosAndJac {
00088 KinematicFamily* family;
00089 Jnt2CartPos* jnt2cartpos;
00090 Jnt2Jac* jnt2jac;
00091 Jacobian<Frame> FJ_base_ee;
00092 Frame F_base_ee1;
00093 Frame F_base_ee2;
00094 public:
00095 static void TestFamily(KinematicFamily* _family) {
00096 TestForwardPosAndJac testobj(_family);
00097 JointVector q(6);
00098 q[0] = 0*KDL::deg2rad;
00099 q[1] = 10*KDL::deg2rad;
00100 q[2] = 20*KDL::deg2rad;
00101 q[3] = 30*KDL::deg2rad;
00102 q[4] = 40*KDL::deg2rad;
00103 q[5] = 50*KDL::deg2rad;
00104 testobj.test(q);
00105 q[0] = -50*KDL::deg2rad;
00106 q[1] = -10*KDL::deg2rad;
00107 q[2] = 20*KDL::deg2rad;
00108 q[3] = -30*KDL::deg2rad;
00109 q[4] = 20*KDL::deg2rad;
00110 q[5] = 110*KDL::deg2rad;
00111 testobj.test(q);
00112 }
00113
00114 TestForwardPosAndJac(KinematicFamily* _family) :
00115 family(_family),
00116 jnt2cartpos(_family->createJnt2CartPos()),
00117 jnt2jac(_family->createJnt2Jac()),
00118 FJ_base_ee(_family->nrOfJoints())
00119 {
00120
00121 assert( jnt2jac != 0);
00122 assert( jnt2cartpos != 0);
00123 }
00124
00125 int test(JointVector& q) {
00126 double deltaq = 1E-4;
00127 double epsJ = 1E-4;
00128 if (jnt2jac->evaluate(q)!=0) return 1;
00129 jnt2jac->getJacobian(FJ_base_ee);
00130 for (int i=0; i< q.size() ;i++) {
00131
00132 double oldqi = q[i];
00133 q[i] = oldqi+deltaq;
00134 if (jnt2cartpos->evaluate(q)!=0) return 1;
00135 jnt2cartpos->getFrame(F_base_ee2);
00136 q[i] = oldqi-deltaq;
00137 if (jnt2cartpos->evaluate(q)!=0) return 1;
00138 jnt2cartpos->getFrame(F_base_ee1);
00139 q[i] = oldqi;
00140
00141 Twist Jcol = diff(F_base_ee1,F_base_ee2,2*deltaq);
00142 if (!Equal(Jcol,FJ_base_ee.deriv(i),epsJ)) {
00143 std::cout << "Difference between symbolic and numeric calculation of Jacobian for column "
00144 << i << std::endl;
00145 std::cout << "Numeric " << Jcol << std::endl;
00146 std::cout << "Symbolic " << FJ_base_ee.deriv(i) << std::endl;
00147 exit(1);
00148 }
00149 }
00150 }
00151
00152 ~TestForwardPosAndJac() {
00153 delete jnt2cartpos;
00154 delete jnt2jac;
00155 }
00156 };
00157
00158
00159
00160
00161
00162
00163 class TestCartVelAndJac {
00164 KinematicFamily* family;
00165 Jnt2CartVel* jnt2cartvel;
00166 Jnt2Jac* jnt2jac;
00167 Jacobian<Frame> FJ_base_ee;
00168 FrameVel F_base_ee;
00169 public:
00170 static void TestFamily(KinematicFamily* _family) {
00171 TestCartVelAndJac testobj(_family);
00172 JointVector qdot(6);
00173 qdot[0] = 0.1;
00174 qdot[1] = 0.2;
00175 qdot[2] = -0.3;
00176 qdot[3] = 0.4;
00177 qdot[4] = -0.5;
00178 qdot[5] = 0.6;
00179 JointVector q(6);
00180 q[0] = 0*KDL::deg2rad;
00181 q[1] = 10*KDL::deg2rad;
00182 q[2] = 20*KDL::deg2rad;
00183 q[3] = 30*KDL::deg2rad;
00184 q[4] = 40*KDL::deg2rad;
00185 q[5] = 50*KDL::deg2rad;
00186 std::cout << "q[1] = " << q[1] << std::endl;
00187 testobj.test(q,qdot);
00188 q[0] = -50*KDL::deg2rad;
00189 q[1] = -10*KDL::deg2rad;
00190 q[2] = 20*KDL::deg2rad;
00191 q[3] = -30*KDL::deg2rad;
00192 q[4] = 20*KDL::deg2rad;
00193 q[5] = 110*KDL::deg2rad;
00194 testobj.test(q,qdot);
00195 }
00196
00197 TestCartVelAndJac(KinematicFamily* _family) :
00198 family(_family),
00199 jnt2cartvel(_family->createJnt2CartVel()),
00200 jnt2jac(_family->createJnt2Jac()),
00201 FJ_base_ee(_family->nrOfJoints())
00202 {
00203
00204 assert( jnt2jac != 0);
00205 assert( jnt2cartvel != 0);
00206 }
00207
00208 int test(JointVector& q,JointVector& qdot) {
00209 std::cout << "Testing wether Jnt2CartVel and Jnt2Jac are consistent " << std::endl;
00210 std::cout << "q[1] = " << q[1] << std::endl;
00211 double deltaq = 1E-4;
00212 double epsJ = 1E-4;
00213 int result;
00214 result = jnt2jac->evaluate(q);
00215 assert(result==0);
00216 jnt2jac->getJacobian(FJ_base_ee);
00217 result = jnt2cartvel->evaluate(q,qdot);
00218 jnt2cartvel->getFrameVel(F_base_ee);
00219 assert(result==0);
00220 Twist t = Twist::Zero();
00221 for (int i=0; i< q.size() ;i++) {
00222 t += FJ_base_ee.deriv(i)*qdot[i];
00223 }
00224 if (!Equal(t,F_base_ee.GetTwist(),1E-6)) {
00225 std::cout << "Difference between the resuls"<< std::endl;
00226 std::cout << "via the Jacobian " << t << std::endl;
00227 std::cout << "via the jnt2cartvel transformation " << F_base_ee.GetTwist() << std::endl;
00228 exit(1);
00229 }
00230 }
00231
00232 ~TestCartVelAndJac() {
00233 delete jnt2cartvel;
00234 delete jnt2jac;
00235 }
00236 };
00237
00238
00239
00240
00241
00242
00243
00244 class TestCartVelAndInverse {
00245 KinematicFamily* family;
00246 Jnt2CartVel* jnt2cartvel;
00247 CartVel2Jnt* cartvel2jnt;
00248 FrameVel F_base_ee;
00249 JointVector qdot2;
00250 public:
00251 static void TestFamily(KinematicFamily* _family) {
00252 TestCartVelAndInverse testobj(_family);
00253 JointVector qdot(6);
00254 qdot[0] = 0.1;
00255 qdot[1] = 0.2;
00256 qdot[2] = -0.3;
00257 qdot[3] = 0.4;
00258 qdot[4] = -0.5;
00259 qdot[5] = 0.6;
00260 JointVector q(6);
00261 q[0] = 0*KDL::deg2rad;
00262 q[1] = 10*KDL::deg2rad;
00263 q[2] = 20*KDL::deg2rad;
00264 q[3] = 30*KDL::deg2rad;
00265 q[4] = 40*KDL::deg2rad;
00266 q[5] = 50*KDL::deg2rad;
00267 testobj.test(q,qdot);
00268 q[0] = -50*KDL::deg2rad;
00269 q[1] = -10*KDL::deg2rad;
00270 q[2] = 20*KDL::deg2rad;
00271 q[3] = -30*KDL::deg2rad;
00272 q[4] = 20*KDL::deg2rad;
00273 q[5] = 110*KDL::deg2rad;
00274 testobj.test(q,qdot);
00275 }
00276
00277 TestCartVelAndInverse(KinematicFamily* _family) :
00278 family(_family),
00279 jnt2cartvel(_family->createJnt2CartVel()),
00280 cartvel2jnt(_family->createCartVel2Jnt()),
00281 qdot2(_family->nrOfJoints())
00282 {
00283
00284 assert( cartvel2jnt != 0);
00285 assert( jnt2cartvel != 0);
00286 }
00287
00288 int test(const JointVector& q,const JointVector& qdot) {
00289 std::cout << "Testing wether Jnt2CartVel and CartVel2Jnt are consistent " << std::endl;
00290 double epsJ = 1E-7;
00291 int result;
00292
00293 result = jnt2cartvel->evaluate(q,qdot);
00294 assert(result==0);
00295 jnt2cartvel->getFrameVel(F_base_ee);
00296
00297 cartvel2jnt->setTwist(F_base_ee.GetTwist());
00298 result = cartvel2jnt->evaluate(q, qdot2);
00299 assert(result==0);
00300
00301 for (int i=0;i<qdot.size();++i) {
00302 if (fabs(qdot[i]-qdot2[i])>epsJ) {
00303 std::cout << " original joint velocities and calculated joint velocities do not match" << std::endl;
00304
00305 for (int j=0;j<qdot.size();j++) {
00306 std::cout << "qdot["<<j<<"]="<<qdot[j]<<" and qdot2["<<j<<"]="<<qdot2[j] << std::endl;
00307 }
00308 std::cout << "Frame : " << F_base_ee.GetFrame() << std::endl;
00309 std::cout << "Twist : " << F_base_ee.GetTwist() << std::endl;
00310 exit(1);
00311 }
00312 }
00313 }
00314
00315 ~TestCartVelAndInverse() {
00316 delete jnt2cartvel;
00317 delete cartvel2jnt;
00318 }
00319 };
00320
00321
00322
00323
00324
00325
00326
00327
00331 class CRS450_exp: public SerialChain {
00332 public:
00333 explicit CRS450_exp(int jointoffset=0) :
00334 SerialChain("CRS450", 6, jointoffset,new LinearTransmission(6) )
00335 {
00336 double L1 = 0.33;
00337 double L2 = 0.305;
00338 double L3 = 0.33;
00339 double L4 = 0.176;
00340 LinearTransmission* tr = (LinearTransmission*)transmission;
00341 tr->setTransmission(2,2.0,1.0);
00342 tr->setTransmission(3,3.0,2.0);
00343 addJoint(new JointRotZ(Frame::Identity()));
00344 addJoint(new JointRotY(Frame(Rotation::Identity(),Vector(0,0,L1))));
00345 addJoint(new JointRotY(Frame(Rotation::Identity(),Vector(0,0,L2))));
00346 addJoint(new JointRotZ(Frame(Rotation::Identity(),Vector(0,0,0))));
00347 addJoint(new JointRotY(Frame(Rotation::Identity(),Vector(0,0,L3))));
00348 addJoint(new JointRotZ(Frame(Rotation::Identity(),Vector(0,0,0))));
00349 setLastJointToEE(Frame(Rotation::Identity(),Vector(0,0,L4)));
00350 };
00351 };
00352
00353
00354 int main(int argc,char* argv[]) {
00355 KinematicFamily* family1,*family2,*family3,*family4;
00356 std::cout << std::endl << "Tests on CRS450 " << std::endl;
00357 family1 = new CRS450();
00358 TestForwardAndInverse::TestFamily(family1);
00359 TestForwardPosAndJac::TestFamily(family1);
00360 TestCartVelAndJac::TestFamily(family1);
00361 TestCartVelAndInverse::TestFamily(family1);
00362 std::cout <<std::endl << "Tests on CRS450_exp " << std::endl;
00363 family2 = new CRS450_exp();
00364 TestForwardAndInverse::TestFamily(family2);
00365 TestForwardPosAndJac::TestFamily(family2);
00366 TestCartVelAndJac::TestFamily(family2);
00367 TestCartVelAndInverse::TestFamily(family2);
00368 std::cout << std::endl << "Tests on CRS450Feath " << std::endl;
00369 family3 = new CRS450Feath();
00370 TestForwardAndInverse::TestFamily(family3);
00371 TestForwardPosAndJac::TestFamily(family3);
00372 TestCartVelAndJac::TestFamily(family3);
00373 TestCartVelAndInverse::TestFamily(family3);
00374 std::cout << std::endl << "Comparing the kinematic families "<< std::endl;
00375 CompareFamilies(family1,family3);
00376 std::cout << std::endl << "Tests on CRS450Feath->createSerialChain " << std::endl;
00377 family4 = ((ZXXZXZ*)family3)->createSerialChain();
00378 TestForwardAndInverse::TestFamily(family4);
00379 TestForwardPosAndJac::TestFamily(family4);
00380 TestCartVelAndJac::TestFamily(family4);
00381 TestCartVelAndInverse::TestFamily(family4);
00382
00383
00384
00385
00386 KinematicFamily* family1b,*family2b,*family3b,*family4b;
00387 family1b = family1->clone();
00388 family2b = family2->clone();
00389 family3b = family3->clone();
00390 family4b = family4->clone();
00391
00392 delete family4;
00393 delete family3;
00394 delete family2;
00395 delete family1;
00396
00397 delete family4b;
00398 delete family3b;
00399 delete family2b;
00400 delete family1b;
00401
00402 return 0;
00403 }
00404
