Lines 90-98
void test_inverseposkin(KDL::Chain& chain) {
Link Here
|
90 |
KDL::JntArray q_sol(n); |
90 |
KDL::JntArray q_sol(n); |
91 |
for (int trial=0;trial<num_of_trials;++trial) { |
91 |
for (int trial=0;trial<num_of_trials;++trial) { |
92 |
q.data.setRandom(); |
92 |
q.data.setRandom(); |
93 |
q.data *= PI; |
93 |
q.data *= KDL::PI; |
94 |
q_init.data.setRandom(); |
94 |
q_init.data.setRandom(); |
95 |
q_init.data *= PI; |
95 |
q_init.data *= KDL::PI; |
96 |
KDL::Frame pos_goal,pos_reached; |
96 |
KDL::Frame pos_goal,pos_reached; |
97 |
fwdkin.JntToCart(q,pos_goal); |
97 |
fwdkin.JntToCart(q,pos_goal); |
98 |
//solver.compute_fwdpos(q.data); |
98 |
//solver.compute_fwdpos(q.data); |