00001 #include "jacobiandottest.hpp"
00002
00003 CPPUNIT_TEST_SUITE_REGISTRATION(JacobianDotTest);
00004
00005 using namespace KDL;
00006
00007 void JacobianDotTest::setUp(){}
00008 void JacobianDotTest::tearDown(){}
00009
00010
00011 namespace KDL{
00012 static const double L0 = 1.0;
00013 static const double L1 = 0.5;
00014 static const double L2 = 0.4;
00015 static const double L3 = 0;
00016 static const double L4 = 0;
00017 static const double L5 = 0;
00018 Chain d2(){
00019 Chain d2;
00020 d2.addSegment(Segment(Joint(Joint::RotZ),Frame(Vector(L0,0,0))));
00021 d2.addSegment(Segment(Joint(Joint::RotZ),Frame(Vector(L1,0,0))));
00022 return d2;
00023 }
00024 Chain d6(){
00025 Chain d6;
00026 d6.addSegment(Segment(Joint(Joint::RotZ),Frame(Vector(L0,0,0))));
00027 d6.addSegment(Segment(Joint(Joint::RotX),Frame(Vector(L1,0,0))));
00028 d6.addSegment(Segment(Joint(Joint::RotX),Frame(Vector(L2,0,0))));
00029 d6.addSegment(Segment(Joint(Joint::RotZ),Frame(Vector(L3,0,0))));
00030 d6.addSegment(Segment(Joint(Joint::RotX),Frame(Vector(L4,0,0))));
00031 d6.addSegment(Segment(Joint(Joint::RotZ),Frame(Vector(L5,0,0))));
00032 return d6;
00033 }
00034 Chain KukaLWR_DHnew(){
00035 Chain kukaLWR_DHnew;
00036
00037
00038 kukaLWR_DHnew.addSegment(Segment(Joint(Joint::None),
00039 Frame::DH_Craig1989(0.0, 0.0, 0.31, 0.0)
00040 ));
00041
00042 kukaLWR_DHnew.addSegment(Segment(Joint(Joint::RotZ),
00043 Frame::DH_Craig1989(0.0, 1.5707963, 0.0, 0.0),
00044 Frame::DH_Craig1989(0.0, 1.5707963, 0.0, 0.0).Inverse()*RigidBodyInertia(2,
00045 Vector::Zero(),
00046 RotationalInertia(0.0,0.0,0.0115343,0.0,0.0,0.0))));
00047
00048
00049 kukaLWR_DHnew.addSegment(Segment(Joint(Joint::RotZ),
00050 Frame::DH_Craig1989(0.0, -1.5707963, 0.4, 0.0),
00051 Frame::DH_Craig1989(0.0, -1.5707963, 0.4, 0.0).Inverse()*RigidBodyInertia(2,
00052 Vector(0.0,-0.3120511,-0.0038871),
00053 RotationalInertia(-0.5471572,-0.0000302,-0.5423253,0.0,0.0,0.0018828))));
00054
00055
00056 kukaLWR_DHnew.addSegment(Segment(Joint(Joint::RotZ),
00057 Frame::DH_Craig1989(0.0, -1.5707963, 0.0, 0.0),
00058 Frame::DH_Craig1989(0.0, -1.5707963, 0.0, 0.0).Inverse()*RigidBodyInertia(2,
00059 Vector(0.0,-0.0015515,0.0),
00060 RotationalInertia(0.0063507,0.0,0.0107804,0.0,0.0,-0.0005147))));
00061
00062
00063 kukaLWR_DHnew.addSegment(Segment(Joint(Joint::RotZ),
00064 Frame::DH_Craig1989(0.0, 1.5707963, 0.39, 0.0),
00065 Frame::DH_Craig1989(0.0, 1.5707963, 0.39, 0.0).Inverse()*RigidBodyInertia(2,
00066 Vector(0.0,0.5216809,0.0),
00067 RotationalInertia(-1.0436952,0.0,-1.0392780,0.0,0.0,0.0005324))));
00068
00069
00070 kukaLWR_DHnew.addSegment(Segment(Joint(Joint::RotZ),
00071 Frame::DH_Craig1989(0.0, 1.5707963, 0.0, 0.0),
00072 Frame::DH_Craig1989(0.0, 1.5707963, 0.0, 0.0).Inverse()*RigidBodyInertia(2,
00073 Vector(0.0,0.0119891,0.0),
00074 RotationalInertia(0.0036654,0.0,0.0060429,0.0,0.0,0.0004226))));
00075
00076
00077 kukaLWR_DHnew.addSegment(Segment(Joint(Joint::RotZ),
00078 Frame::DH_Craig1989(0.0, -1.5707963, 0.0, 0.0),
00079 Frame::DH_Craig1989(0.0, -1.5707963, 0.0, 0.0).Inverse()*RigidBodyInertia(2,
00080 Vector(0.0,0.0080787,0.0),
00081 RotationalInertia(0.0010431,0.0,0.0036376,0.0,0.0,0.0000101))));
00082
00083 kukaLWR_DHnew.addSegment(Segment(Joint(Joint::RotZ),
00084 Frame::Identity(),
00085 RigidBodyInertia(2,
00086 Vector::Zero(),
00087 RotationalInertia(0.000001,0.0,0.0001203,0.0,0.0,0.0))));
00088 return kukaLWR_DHnew;
00089 }
00090 }
00091
00092
00093 void changeRepresentation(Jacobian& J,const Frame& F_bs_ee,const int& representation)
00094 {
00095 switch(representation)
00096 {
00097 case ChainJntToJacDotSolver::HYBRID:
00098 break;
00099 case ChainJntToJacDotSolver::BODYFIXED:
00100
00101 J.changeBase(F_bs_ee.M.Inverse());
00102 break;
00103 case ChainJntToJacDotSolver::INTERTIAL:
00104
00105 J.changeRefPoint(-F_bs_ee.p);
00106 break;
00107 }
00108 }
00109 void Jdot_diff(const Jacobian& J_q,
00110 const Jacobian& J_qdt,
00111 const double& dt,
00112 Jacobian& Jdot)
00113 {
00114 assert(J_q.columns() == J_qdt.columns());
00115 assert(J_q.columns() == Jdot.columns());
00116 for(int l=0;l<6;l++)
00117 for(int c=0;c<J_q.columns();c++)
00118 Jdot(l,c) = (J_qdt(l,c) - J_q(l,c))/dt;
00119 }
00120
00121 Jacobian Jdot_d2_symbolic(const JntArray& q,const JntArray& qdot)
00122 {
00123
00124 Jacobian Jdot(q.rows());
00125 SetToZero(Jdot);
00126 Jdot(0,0) = -L1 * (qdot(0) + qdot(1))*cos(q(0)+q(1))-L0*cos(q(0))*qdot(0);
00127 Jdot(0,1) = -L1 * (qdot(0) + qdot(1))*cos(q(0)+q(1));
00128 Jdot(1,0) = -L1 * (qdot(0) + qdot(1))*sin(q(0)+q(1))-L0*sin(q(0))*qdot(0);
00129 Jdot(1,1) = -L1 * (qdot(0) + qdot(1))*sin(q(0)+q(1));
00130 return Jdot;
00131 }
00132
00133 Jacobian J_d2_symbolic(const JntArray& q,const JntArray& qdot)
00134 {
00135
00136 Jacobian J(q.rows());
00137 SetToZero(J);
00138 J(0,0) = -L1 * sin(q(0)+q(1))-L0*sin(q(0));
00139 J(0,1) = -L1 * sin(q(0)+q(1));
00140 J(1,0) = L1 * cos(q(0)+q(1))+L0*cos(q(0));
00141 J(1,1) = L1 * cos(q(0)+q(1));
00142 J(5,0) = J(5,1) = 1;
00143 return J;
00144 }
00145
00146 JntArray diff(const JntArray& q,const JntArray& qdot,const double& dt)
00147 {
00148 JntArray q_qdqt(q);
00149 for(int i=0; i<q.rows(); i++)
00150 q_qdqt(i) += dt*qdot(i);
00151 return q_qdqt;
00152 }
00153
00154 void random(JntArray& q)
00155 {
00156 for(int i=0; i<q.rows(); i++)
00157 random(q(i));
00158 }
00159
00160 double compare_Jdot_Diff_vs_Solver(const Chain& chain,const double& dt,const int& representation,bool verbose)
00161 {
00162
00163 JntArray q(chain.getNrOfJoints());
00164 JntArray qdot(chain.getNrOfJoints());
00165 JntArray q_dqdt(chain.getNrOfJoints());
00166
00167
00168 random(q);
00169 random(qdot);
00170 q_dqdt = diff(q,qdot,dt);
00171
00172 ChainJntToJacDotSolver jdot_solver(chain);
00173 ChainJntToJacSolver j_solver(chain);
00174 ChainFkSolverPos_recursive fk_solver(chain);
00175
00176 Frame F_bs_ee_q,F_bs_ee_q_dqdt;
00177 Jacobian jac_q(chain.getNrOfJoints()),
00178 jac_q_dqdt(chain.getNrOfJoints()),
00179 jdot_by_diff(chain.getNrOfJoints());
00180
00181 j_solver.JntToJac(q,jac_q);
00182 j_solver.JntToJac(q_dqdt,jac_q_dqdt);
00183
00184 fk_solver.JntToCart(q,F_bs_ee_q);
00185 fk_solver.JntToCart(q_dqdt,F_bs_ee_q_dqdt);
00186
00187 changeRepresentation(jac_q,F_bs_ee_q,representation);
00188 changeRepresentation(jac_q_dqdt,F_bs_ee_q_dqdt,representation);
00189
00190 Jdot_diff(jac_q,jac_q_dqdt,dt,jdot_by_diff);
00191
00192 Jacobian jdot_by_solver(chain.getNrOfJoints());
00193 jdot_solver.setRepresentation(representation);
00194 jdot_solver.JntToJacDot(JntArrayVel(q_dqdt,qdot),jdot_by_solver);
00195
00196 Twist jdot_qdot_by_solver;
00197 MultiplyJacobian(jdot_by_solver,qdot,jdot_qdot_by_solver);
00198
00199 Twist jdot_qdot_by_diff;
00200 MultiplyJacobian(jdot_by_diff,qdot,jdot_qdot_by_diff);
00201
00202 if(verbose){
00203 std::cout << "Jdot diff : \n" << jdot_by_diff<<std::endl;
00204 std::cout << "Jdot solver:\n"<<jdot_by_solver<<std::endl;
00205
00206 std::cout << "Error : " <<jdot_qdot_by_diff-jdot_qdot_by_solver<<q<<qdot<<std::endl;
00207 }
00208 double err = jdot_qdot_by_diff.vel.Norm() - jdot_qdot_by_solver.vel.Norm()
00209 + jdot_qdot_by_diff.rot.Norm() - jdot_qdot_by_solver.rot.Norm();
00210 return std::abs(err);
00211 }
00212
00213 double compare_d2_Jdot_Symbolic_vs_Solver(bool verbose)
00214 {
00215 Chain chain=d2();
00216 JntArray q(chain.getNrOfJoints());
00217 JntArray qdot(chain.getNrOfJoints());
00218
00219 random(q);
00220 random(qdot);
00221
00222 ChainJntToJacDotSolver jdot_solver(chain);
00223
00224 Jacobian jdot_sym = Jdot_d2_symbolic(q,qdot);
00225
00226 Jacobian jdot_by_solver(chain.getNrOfJoints());
00227 jdot_solver.JntToJacDot(JntArrayVel(q,qdot),jdot_by_solver);
00228
00229 Twist jdot_qdot_by_solver;
00230 MultiplyJacobian(jdot_by_solver,qdot,jdot_qdot_by_solver);
00231
00232 Twist jdot_qdot_sym;
00233 MultiplyJacobian(jdot_sym,qdot,jdot_qdot_sym);
00234
00235 if(verbose){
00236 std::cout << "Jdot symbolic : \n" << jdot_sym<<std::endl;
00237 std::cout << "Jdot solver:\n"<<jdot_by_solver<<std::endl;
00238 std::cout << "Error : " <<jdot_qdot_sym-jdot_qdot_by_solver<<q<<qdot<<std::endl;
00239 }
00240 double err = jdot_qdot_sym.vel.Norm() - jdot_qdot_by_solver.vel.Norm()
00241 + jdot_qdot_sym.rot.Norm() - jdot_qdot_by_solver.rot.Norm();
00242 return std::abs(err);
00243 }
00244
00245 bool runTest(const Chain& chain,const int& representation)
00246 {
00247 bool success=true;
00248 bool verbose = false;
00249 double err;
00250 bool print_err = false;
00251
00252 for(double dt=1e-6;dt<0.1;dt*=10)
00253 {
00254 double eps_diff_vs_solver = 3.0*dt;
00255
00256 for(int i=0;i<100;i++)
00257 {
00258 err = compare_Jdot_Diff_vs_Solver(chain,dt,representation,verbose);
00259
00260 success &= err<=eps_diff_vs_solver;
00261
00262 if(!success || print_err){
00263 std::cout<<" dt:"<< dt<<" err:"<<err
00264 <<" eps_diff_vs_solver:"<<eps_diff_vs_solver
00265 <<std::endl;
00266 if(!success)
00267 break;
00268 }
00269 }
00270 }
00271
00272 return success;
00273 }
00274
00275 void JacobianDotTest::testD2DiffHybrid(){
00276 CPPUNIT_ASSERT(runTest(d2(),0));
00277 }
00278 void JacobianDotTest::testD6DiffHybrid(){
00279 CPPUNIT_ASSERT(runTest(d6(),0));
00280 }
00281 void JacobianDotTest::testKukaDiffHybrid(){
00282 CPPUNIT_ASSERT(runTest(KukaLWR_DHnew(),0));
00283 }
00284
00285 void JacobianDotTest::testD2DiffInertial(){
00286 CPPUNIT_ASSERT(runTest(d2(),2));
00287 }
00288 void JacobianDotTest::testD6DiffInertial(){
00289 CPPUNIT_ASSERT(runTest(d6(),2));
00290 }
00291 void JacobianDotTest::testKukaDiffInertial(){
00292 CPPUNIT_ASSERT(runTest(KukaLWR_DHnew(),2));
00293 }
00294 void JacobianDotTest::testD2DiffBodyFixed(){
00295 CPPUNIT_ASSERT(runTest(d2(),1));
00296 }
00297 void JacobianDotTest::testD6DiffBodyFixed(){
00298 CPPUNIT_ASSERT(runTest(d6(),1));
00299 }
00300 void JacobianDotTest::testKukaDiffBodyFixed(){
00301 CPPUNIT_ASSERT(runTest(KukaLWR_DHnew(),1));
00302 }
00303
00304 void JacobianDotTest::testD2Symbolic(){
00305
00306 bool success=true;
00307 bool verbose = false;
00308 double err_d2_sym;
00309 bool print_err = false;
00310
00311 double eps_sym_vs_solver = 1e-10;
00312
00313 for(int i=0;i<100;i++)
00314 {
00315 err_d2_sym = compare_d2_Jdot_Symbolic_vs_Solver(verbose);
00316
00317 success &= err_d2_sym<=eps_sym_vs_solver;
00318
00319 if(!success || print_err){
00320 std::cout <<" err_d2_sym:"<<err_d2_sym
00321 <<" eps_sym_vs_solver:"<<eps_sym_vs_solver<<std::endl;
00322 if(!success)
00323 break;
00324 }
00325
00326 }
00327
00328 CPPUNIT_ASSERT(success);
00329 }