12 static const double L0 = 1.0;
13 static const double L1 = 0.5;
14 static const double L2 = 0.4;
15 static const double L3 = 0;
16 static const double L4 = 0;
17 static const double L5 = 0;
52 Vector(0.0,-0.3120511,-0.0038871),
59 Vector(0.0,-0.0015515,0.0),
95 switch(representation)
116 for(
unsigned int l=0;l<6;l++)
117 for(
unsigned int c=0;c<J_q.
columns();c++)
118 Jdot(l,c) = (J_qdt(l,c) - J_q(l,c))/dt;
126 Jdot(0,0) = -
L1 * (qdot(0) + qdot(1))*
cos(q(0)+q(1))-
L0*
cos(q(0))*qdot(0);
127 Jdot(0,1) = -
L1 * (qdot(0) + qdot(1))*
cos(q(0)+q(1));
128 Jdot(1,0) = -
L1 * (qdot(0) + qdot(1))*
sin(q(0)+q(1))-
L0*
sin(q(0))*qdot(0);
129 Jdot(1,1) = -
L1 * (qdot(0) + qdot(1))*
sin(q(0)+q(1));
139 J(0,1) = -
L1 *
sin(q(0)+q(1));
141 J(1,1) =
L1 *
cos(q(0)+q(1));
149 for(
unsigned int i=0; i<q.
rows(); i++)
150 q_qdqt(i) += dt*qdot(i);
156 for(
unsigned int i=0; i<q.
rows(); i++)
170 q_dqdt =
diff(q,qdot,dt);
176 Frame F_bs_ee_q,F_bs_ee_q_dqdt;
182 j_solver.
JntToJac(q_dqdt,jac_q_dqdt);
185 fk_solver.
JntToCart(q_dqdt,F_bs_ee_q_dqdt);
190 Jdot_diff(jac_q,jac_q_dqdt,dt,jdot_by_diff);
196 Twist jdot_qdot_by_solver;
199 Twist jdot_qdot_by_diff;
203 std::cout <<
"Jdot diff:\n" << jdot_by_diff <<
"\n"
204 <<
"Jdot solver:\n" << jdot_by_solver <<
"\n"
206 << jdot_qdot_by_diff-jdot_qdot_by_solver <<
"\n"
210 double err = jdot_qdot_by_diff.
vel.
Norm() - jdot_qdot_by_solver.
vel.
Norm()
231 Twist jdot_qdot_by_solver;
238 std::cout <<
"Jdot symbolic:\n" << jdot_sym <<
"\n"
239 <<
"Jdot solver:\n" << jdot_by_solver <<
"\n"
240 <<
"Error:\n" << jdot_qdot_sym-jdot_qdot_by_solver <<
"\n"
244 double err = jdot_qdot_sym.
vel.
Norm() - jdot_qdot_by_solver.
vel.
Norm()
252 bool verbose =
false;
254 bool print_err =
false;
256 for(
double dt=1e-6;dt<0.1;dt*=10)
258 double eps_diff_vs_solver = 4.0*dt;
260 for(
int i=0; i<100 ; ++i)
264 success &= err<=eps_diff_vs_solver;
266 if(!success || print_err){
268 <<
"err: "<< err <<
"\n"
269 <<
"eps_diff_vs_solver: " << eps_diff_vs_solver <<
std::endl;
311 bool verbose =
false;
313 bool print_err =
false;
315 double eps_sym_vs_solver = 1e-10;
317 for(
int i=0; i<100; ++i)
321 success &= err_d2_sym <= eps_sym_vs_solver;
323 if(!success || print_err){
324 std::cout <<
"err_d2_sym: " << err_d2_sym <<
"\n"
325 <<
"eps_sym_vs_solver: "<< eps_sym_vs_solver <<
std::endl;
332 CPPUNIT_ASSERT(success);