solvertest.cpp
Go to the documentation of this file.
1 #include "solvertest.hpp"
2 #include <frames_io.hpp>
3 #include <framevel_io.hpp>
4 #include <kinfam_io.hpp>
5 #include <time.h>
6 #include <utilities/utility.h>
7 
8 CPPUNIT_TEST_SUITE_REGISTRATION( SolverTest );
9 
10 using namespace KDL;
11 
12 void SolverTest::setUp()
13 {
14  srand( (unsigned)time( NULL ));
15 
16  chain1.addSegment(Segment("Segment 1", Joint("Joint 1", Joint::RotZ),
17  Frame(Vector(0.0,0.0,0.0))));
18  chain1.addSegment(Segment("Segment 2", Joint("Joint 2", Joint::RotX),
19  Frame(Vector(0.0,0.0,0.9))));
20  chain1.addSegment(Segment("Segment 3", Joint("Joint 3", Joint::None),
21  Frame(Vector(-0.4,0.0,0.0))));
22  chain1.addSegment(Segment("Segment 4", Joint("Joint 4", Joint::RotX),
23  Frame(Vector(0.0,0.0,1.2))));
24  chain1.addSegment(Segment("Segment 5", Joint("Joint 5", Joint::None),
25  Frame(Vector(0.4,0.0,0.0))));
26  chain1.addSegment(Segment("Segment 6", Joint("Joint 6", Joint::RotZ),
27  Frame(Vector(0.0,0.0,1.4))));
28  chain1.addSegment(Segment("Segment 7", Joint("Joint 7", Joint::RotX),
29  Frame(Vector(0.0,0.0,0.0))));
30  chain1.addSegment(Segment("Segment 8", Joint("Joint 8", Joint::RotZ),
31  Frame(Vector(0.0,0.0,0.4))));
32  chain1.addSegment(Segment("Segment 9", Joint("Joint 9", Joint::None),
33  Frame(Vector(0.0,0.0,0.0))));
34 
35  chain2.addSegment(Segment("Segment 1", Joint("Joint 1", Joint::RotZ),
36  Frame(Vector(0.0,0.0,0.5))));
37  chain2.addSegment(Segment("Segment 2", Joint("Joint 2", Joint::RotX),
38  Frame(Vector(0.0,0.0,0.4))));
39  chain2.addSegment(Segment("Segment 3", Joint("Joint 3", Joint::RotX),
40  Frame(Vector(0.0,0.0,0.3))));
41  chain2.addSegment(Segment("Segment 4", Joint("Joint 4", Joint::RotX),
42  Frame(Vector(0.0,0.0,0.2))));
43  chain2.addSegment(Segment("Segment 5", Joint("Joint 5", Joint::RotZ),
44  Frame(Vector(0.0,0.0,0.1))));
45 
46 
47  chain3.addSegment(Segment("Segment 1", Joint("Joint 1", Joint::RotZ),
48  Frame(Vector(0.0,0.0,0.0))));
49  chain3.addSegment(Segment("Segment 2", Joint("Joint 2", Joint::RotX),
50  Frame(Vector(0.0,0.0,0.9))));
51  chain3.addSegment(Segment("Segment 3", Joint("Joint 3", Joint::RotZ),
52  Frame(Vector(-0.4,0.0,0.0))));
53  chain3.addSegment(Segment("Segment 4", Joint("Joint 4", Joint::RotX),
54  Frame(Vector(0.0,0.0,1.2))));
55  chain3.addSegment(Segment("Segment 5", Joint("Joint 5", Joint::None),
56  Frame(Vector(0.4,0.0,0.0))));
57  chain3.addSegment(Segment("Segment 6", Joint("Joint 6", Joint::RotZ),
58  Frame(Vector(0.0,0.0,1.4))));
59  chain3.addSegment(Segment("Segment 7", Joint("Joint 7", Joint::RotX),
60  Frame(Vector(0.0,0.0,0.0))));
61  chain3.addSegment(Segment("Segment 8", Joint("Joint 8", Joint::RotZ),
62  Frame(Vector(0.0,0.0,0.4))));
63  chain3.addSegment(Segment("Segment 9", Joint("Joint 9", Joint::RotY),
64  Frame(Vector(0.0,0.0,0.0))));
65 
66 
67  chain4.addSegment(Segment("Segment 1", Joint("Joint 1", Vector(10,0,0), Vector(1,0,1),Joint::RotAxis),
68  Frame(Vector(0.0,0.0,0.5))));
69  chain4.addSegment(Segment("Segment 2", Joint("Joint 2", Vector(0,5,0), Vector(1,0,0),Joint::RotAxis),
70  Frame(Vector(0.0,0.0,0.4))));
71  chain4.addSegment(Segment("Segment 3", Joint("Joint 3", Vector(0,0,5), Vector(1,0,4),Joint::RotAxis),
72  Frame(Vector(0.0,0.0,0.3))));
73  chain4.addSegment(Segment("Segment 4", Joint("Joint 4", Vector(0,0,0), Vector(1,0,0),Joint::RotAxis),
74  Frame(Vector(0.0,0.0,0.2))));
75  chain4.addSegment(Segment("Segment 5", Joint("Joint 5", Vector(0,0,0), Vector(0,0,1),Joint::RotAxis),
76  Frame(Vector(0.0,0.0,0.1))));
77 
78 
79 
80  //chain definition for vereshchagin's dynamic solver
81  Joint rotJoint0 = Joint(Joint::RotZ, 1, 0, 0.01);
82  Joint rotJoint1 = Joint(Joint::RotZ, 1, 0, 0.01);
83 
84  Frame refFrame(Rotation::RPY(0.0, 0.0, 0.0), Vector(0.0, 0.0, 0.0));
85  Frame frame1(Rotation::RPY(0.0, 0.0, 0.0), Vector(0.0, -0.4, 0.0));
86  Frame frame2(Rotation::RPY(0.0, 0.0, 0.0), Vector(0.0, -0.4, 0.0));
87 
88  //chain segments
89  Segment segment1 = Segment(rotJoint0, frame1);
90  Segment segment2 = Segment(rotJoint1, frame2);
91 
92  //rotational inertia around symmetry axis of rotation
93  RotationalInertia rotInerSeg1(0.0, 0.0, 0.0, 0.0, 0.0, 0.0);
94 
95  //spatial inertia
96  RigidBodyInertia inerSegment1(0.3, Vector(0.0, -0.4, 0.0), rotInerSeg1);
97  RigidBodyInertia inerSegment2(0.3, Vector(0.0, -0.4, 0.0), rotInerSeg1);
98  segment1.setInertia(inerSegment1);
99  segment2.setInertia(inerSegment2);
100 
101  //chain
102  chaindyn.addSegment(segment1);
103  chaindyn.addSegment(segment2);
104 
105  // Motoman SIA10 Chain (for IK singular value tests)
106  motomansia10.addSegment(Segment(Joint(Joint::None),
107  Frame::DH_Craig1989(0.0, 0.0, 0.36, 0.0)));
108  motomansia10.addSegment(Segment(Joint(Joint::RotZ),
109  Frame::DH_Craig1989(0.0, PI_2, 0.0, 0.0)));
110  motomansia10.addSegment(Segment(Joint(Joint::RotZ),
111  Frame::DH_Craig1989(0.0, -PI_2, 0.36, 0.0)));
112  motomansia10.addSegment(Segment(Joint(Joint::RotZ),
113  Frame::DH_Craig1989(0.0, PI_2, 0.0, 0.0)));
114  motomansia10.addSegment(Segment(Joint(Joint::RotZ),
115  Frame::DH_Craig1989(0.0, -PI_2, 0.36, 0.0)));
116  motomansia10.addSegment(Segment(Joint(Joint::RotZ),
117  Frame::DH_Craig1989(0.0, PI_2, 0.0, 0.0)));
118  motomansia10.addSegment(Segment(Joint(Joint::RotZ),
119  Frame::DH_Craig1989(0.0, -PI_2, 0.0, 0.0)));
120  motomansia10.addSegment(Segment(Joint(Joint::RotZ),
121  Frame(Rotation::Identity(),Vector(0.0,0.0,0.155))));
122 
123  // Motoman SIA10 Chain with Mass Parameters (for forward dynamics tests)
124 
125  // effective motor inertia is included as joint inertia
126  static const double scale=1;
127  static const double offset=0;
128  static const double inertiamotorA=5.0; // effective motor inertia kg-m^2
129  static const double inertiamotorB=3.0; // effective motor inertia kg-m^2
130  static const double inertiamotorC=1.0; // effective motor inertia kg-m^2
131  static const double damping=0;
132  static const double stiffness=0;
133 
134  // Segment Inertias
135  KDL::RigidBodyInertia inert1(15.0, KDL::Vector(0.0, -0.02, 0.0), // mass, CM
136  KDL::RotationalInertia(0.1, 0.05, 0.1, 0.0, 0.0, 0.0)); // inertia
137  KDL::RigidBodyInertia inert2(5.0, KDL::Vector(0.0, -0.02, -0.1),
138  KDL::RotationalInertia(0.01, 0.1, 0.1, 0.0, 0.0, 0.0));
139  KDL::RigidBodyInertia inert3(5.0, KDL::Vector(0.0, -0.05, 0.02),
140  KDL::RotationalInertia(0.05, 0.01, 0.05, 0.0, 0.0, 0.0));
141  KDL::RigidBodyInertia inert4(3.0, KDL::Vector(0.0, 0.02, -0.15),
142  KDL::RotationalInertia(0.1, 0.1, 0.01, 0.0, 0.0, 0.0));
143  KDL::RigidBodyInertia inert5(3.0, KDL::Vector(0.0, -0.05, 0.01),
144  KDL::RotationalInertia(0.02, 0.01, 0.02, 0.0, 0.0, 0.0));
145  KDL::RigidBodyInertia inert6(3.0, KDL::Vector(0.0, -0.01, -0.1),
146  KDL::RotationalInertia(0.1, 0.1, 0.01, 0.0, 0.0, 0.0));
147  KDL::RigidBodyInertia inert7(1.0, KDL::Vector(0.0, 0.0, 0.05),
148  KDL::RotationalInertia(0.01, 0.01, 0.1, 0.0, 0.0, 0.0));
149 
150  motomansia10dyn.addSegment(Segment(Joint(Joint::RotZ, scale, offset, inertiamotorA, damping, stiffness),
151  Frame::DH(0.0, PI_2, 0.36, 0.0),
152  inert1));
153  motomansia10dyn.addSegment(Segment(Joint(Joint::RotZ, scale, offset, inertiamotorA, damping, stiffness),
154  Frame::DH(0.0, -PI_2, 0.0, 0.0),
155  inert2));
156  motomansia10dyn.addSegment(Segment(Joint(Joint::RotZ, scale, offset, inertiamotorB, damping, stiffness),
157  Frame::DH(0.0, PI_2, 0.36, 0.0),
158  inert3));
159  motomansia10dyn.addSegment(Segment(Joint(Joint::RotZ, scale, offset, inertiamotorB, damping, stiffness),
160  Frame::DH(0.0, -PI_2, 0.0, 0.0),
161  inert4));
162  motomansia10dyn.addSegment(Segment(Joint(Joint::RotZ, scale, offset, inertiamotorC, damping, stiffness),
163  Frame::DH(0.0, PI_2, 0.36, 0.0),
164  inert5));
165  motomansia10dyn.addSegment(Segment(Joint(Joint::RotZ, scale, offset, inertiamotorC, damping, stiffness),
166  Frame::DH(0.0, -PI_2, 0.0, 0.0),
167  inert6));
168  motomansia10dyn.addSegment(Segment(Joint(Joint::RotZ, scale, offset, inertiamotorC, damping, stiffness),
169  Frame::DH(0.0, 0.0, 0.0, 0.0),
170  inert7));
171  motomansia10dyn.addSegment(Segment(Joint(Joint::None),
172  Frame(Rotation::Identity(),Vector(0.0,0.0,0.155))));
173 
183  kukaLWR.addSegment(Segment(Joint(Joint::RotZ, scale, offset, 3.19, damping, stiffness),
184  Frame::DH_Craig1989(0.0, 1.5707963, 0.0, 0.0),
185  Frame::DH_Craig1989(0.0, 1.5707963, 0.0, 0.0).Inverse()*RigidBodyInertia(2,
186  Vector::Zero(),
187  RotationalInertia(0.0,0.0,0.0115343,0.0,0.0,0.0))));
188 
189  kukaLWR.addSegment(Segment(Joint(Joint::RotZ, scale, offset, 3.05, damping, stiffness),
190  Frame::DH_Craig1989(0.0, -1.5707963, 0.4, 0.0),
191  Frame::DH_Craig1989(0.0, -1.5707963, 0.4, 0.0).Inverse()*RigidBodyInertia(2,
192  Vector(0.0,-0.3120511,-0.0038871),
193  RotationalInertia(-0.5471572,-0.0000302,-0.5423253,0.0,0.0,0.0018828))));
194 
195  kukaLWR.addSegment(Segment(Joint(Joint::RotZ, scale, offset, 1.98, damping, stiffness),
196  Frame::DH_Craig1989(0.0, -1.5707963, 0.0, 0.0),
197  Frame::DH_Craig1989(0.0, -1.5707963, 0.0, 0.0).Inverse()*RigidBodyInertia(2,
198  Vector(0.0,-0.0015515,0.0),
199  RotationalInertia(0.0063507,0.0,0.0107804,0.0,0.0,-0.0005147))));
200 
201  kukaLWR.addSegment(Segment(Joint(Joint::RotZ, scale, offset, 2.05, damping, stiffness),
202  Frame::DH_Craig1989(0.0, 1.5707963, 0.39, 0.0),
203  Frame::DH_Craig1989(0.0, 1.5707963, 0.39, 0.0).Inverse()*RigidBodyInertia(2,
204  Vector(0.0,0.5216809,0.0),
205  RotationalInertia(-1.0436952,0.0,-1.0392780,0.0,0.0,0.0005324))));
206 
207  kukaLWR.addSegment(Segment(Joint(Joint::RotZ, scale, offset, 0.787, damping, stiffness),
208  Frame::DH_Craig1989(0.0, 1.5707963, 0.0, 0.0),
209  Frame::DH_Craig1989(0.0, 1.5707963, 0.0, 0.0).Inverse()*RigidBodyInertia(2,
210  Vector(0.0,0.0119891,0.0),
211  RotationalInertia(0.0036654,0.0,0.0060429,0.0,0.0,0.0004226))));
212 
213  kukaLWR.addSegment(Segment(Joint(Joint::RotZ, scale, offset, 0.391, damping, stiffness),
214  Frame::DH_Craig1989(0.0, -1.5707963, 0.0, 0.0),
215  Frame::DH_Craig1989(0.0, -1.5707963, 0.0, 0.0).Inverse()*RigidBodyInertia(2,
216  Vector(0.0,0.0080787,0.0),
217  RotationalInertia(0.0010431,0.0,0.0036376,0.0,0.0,0.0000101))));
218 
219  kukaLWR.addSegment(Segment(Joint(Joint::RotZ, scale, offset, 0.394, damping, stiffness),
220  Frame::Identity(),
221  RigidBodyInertia(2, Vector::Zero(), RotationalInertia(0.000001,0.0,0.0001203,0.0,0.0,0.0))));
222 }
223 
224 void SolverTest::tearDown()
225 {
226 // delete fksolverpos;
227 // delete jacsolver;
228 // delete fksolvervel;
229 // delete iksolvervel;
230 // delete iksolverpos;
231 }
232 
233 void SolverTest::UpdateChainTest()
234 {
235  ChainFkSolverPos_recursive fksolverpos(chain2);
236  ChainFkSolverVel_recursive fksolvervel(chain2);
237  ChainJntToJacSolver jacsolver1(chain2);
238  ChainJntToJacDotSolver jacdotsolver1(chain2);
239  ChainIkSolverVel_pinv iksolver2(chain2);
240  ChainIkSolverVel_pinv_givens iksolver_pinv_givens2(chain2);
241  ChainIkSolverVel_pinv_nso iksolver_pinv_nso(chain2);
242  ChainIkSolverVel_wdls iksolver_wdls(chain2,1e-6,30);
243  ChainIkSolverPos_NR iksolverpos(chain2,fksolverpos,iksolver2);
244  ChainIkSolverPos_NR_JL iksolverpos2(chain2,fksolverpos,iksolver2);
245  ChainIkSolverPos_LMA iksolverpos3(chain2);
246  ChainDynParam dynparam(chain2, Vector::Zero());
247  ChainIdSolver_RNE idsolver1(chain2, Vector::Zero());
248  unsigned int nr_of_constraints = 4;
249  ChainHdSolver_Vereshchagin hdsolver(chain2,Twist::Zero(),4);
250 
251  JntArray q_in(chain2.getNrOfJoints());
252  JntArray q_in2(chain2.getNrOfJoints());
253  JntArrayVel q_in3(chain2.getNrOfJoints());
254  for(unsigned int i=0; i<chain2.getNrOfJoints(); i++)
255  {
256  random(q_in(i));
257  random(q_in2(i));
258  }
259  JntArray q_out(chain2.getNrOfJoints());
260  JntArray q_out2(chain2.getNrOfJoints());
261  JntArray ff_tau(chain2.getNrOfJoints());
262  JntArray constraint_tau(chain2.getNrOfJoints());
263  Jacobian jac(chain2.getNrOfJoints());
264  Frame T;
265  Twist t;
266  FrameVel T2;
267  Wrenches wrenches(chain2.getNrOfSegments());
268  JntSpaceInertiaMatrix m(chain2.getNrOfJoints());
269 
270  Jacobian alpha(nr_of_constraints - 1);
271  JntArray beta(nr_of_constraints - 1);
272  CPPUNIT_ASSERT_EQUAL((int)SolverI::E_OUT_OF_RANGE,fksolverpos.JntToCart(q_in, T, chain2.getNrOfSegments()+1));
273  CPPUNIT_ASSERT_EQUAL((int)SolverI::E_OUT_OF_RANGE,fksolvervel.JntToCart(q_in3, T2, chain2.getNrOfSegments()+1));
274  CPPUNIT_ASSERT_EQUAL((int)SolverI::E_OUT_OF_RANGE, jacsolver1.JntToJac(q_in, jac, chain2.getNrOfSegments()+1));
275  CPPUNIT_ASSERT_EQUAL((int)SolverI::E_OUT_OF_RANGE, jacdotsolver1.JntToJacDot(q_in3, t, chain2.getNrOfSegments()+1));
276  CPPUNIT_ASSERT_EQUAL((int)SolverI::E_OUT_OF_RANGE, jacdotsolver1.JntToJacDot(q_in3, jac, chain2.getNrOfSegments()+1));
277  chain2.addSegment(Segment("Segment 6", Joint("Joint 6", Joint::RotX),
278  Frame(Vector(0.0,0.0,0.1))));
279  CPPUNIT_ASSERT_EQUAL((int)SolverI::E_NOT_UP_TO_DATE, jacsolver1.JntToJac(q_in, jac, chain2.getNrOfSegments()));
280  CPPUNIT_ASSERT_EQUAL((int)SolverI::E_NOT_UP_TO_DATE, jacdotsolver1.JntToJacDot(q_in3, jac, chain2.getNrOfSegments()));
281  CPPUNIT_ASSERT_EQUAL((int)SolverI::E_NOT_UP_TO_DATE, jacdotsolver1.JntToJacDot(q_in3, t, chain2.getNrOfSegments()));
282  CPPUNIT_ASSERT_EQUAL((int)SolverI::E_NOT_UP_TO_DATE, iksolver2.CartToJnt(q_in,t,q_out));
283  CPPUNIT_ASSERT_EQUAL((int)SolverI::E_NOT_UP_TO_DATE, iksolver_pinv_givens2.CartToJnt(q_in,t,q_out));
284  CPPUNIT_ASSERT_EQUAL((int)SolverI::E_NOT_UP_TO_DATE, iksolver_pinv_nso.CartToJnt(q_in,t,q_out));
285  CPPUNIT_ASSERT_EQUAL((int)SolverI::E_NOT_UP_TO_DATE, iksolver_wdls.CartToJnt(q_in,t,q_out));
286  CPPUNIT_ASSERT_EQUAL((int)SolverI::E_NOT_UP_TO_DATE, iksolverpos.CartToJnt(q_in,T,q_out));
287  CPPUNIT_ASSERT_EQUAL((int)SolverI::E_NOT_UP_TO_DATE, iksolverpos2.CartToJnt(q_in,T,q_out));
288  CPPUNIT_ASSERT_EQUAL((int)SolverI::E_NOT_UP_TO_DATE, iksolverpos3.CartToJnt(q_in,T,q_out));
289  CPPUNIT_ASSERT_EQUAL((int)SolverI::E_NOT_UP_TO_DATE, idsolver1.CartToJnt(q_in,q_in2,q_out,wrenches,q_out2));
290  CPPUNIT_ASSERT_EQUAL((int)SolverI::E_NOT_UP_TO_DATE, hdsolver.CartToJnt(q_in,q_in2,q_out,alpha,beta,wrenches, ff_tau, constraint_tau));
291  CPPUNIT_ASSERT_EQUAL((int)SolverI::E_NOT_UP_TO_DATE, dynparam.JntToCoriolis(q_in, q_in2, q_out));
292  CPPUNIT_ASSERT_EQUAL((int)SolverI::E_NOT_UP_TO_DATE, dynparam.JntToGravity(q_in, q_out));
293  CPPUNIT_ASSERT_EQUAL((int)SolverI::E_NOT_UP_TO_DATE, dynparam.JntToMass(q_in, m));
294 
295  jacsolver1.updateInternalDataStructures();
296  jacdotsolver1.updateInternalDataStructures();
297  iksolver2.updateInternalDataStructures();
298  iksolver_pinv_givens2.updateInternalDataStructures();
299  iksolver_pinv_nso.updateInternalDataStructures();
300  iksolver_wdls.updateInternalDataStructures();
301  iksolverpos.updateInternalDataStructures();
302  iksolverpos2.updateInternalDataStructures();
303  iksolverpos3.updateInternalDataStructures();
304  idsolver1.updateInternalDataStructures();
305  hdsolver.updateInternalDataStructures();
306  dynparam.updateInternalDataStructures();
307 
308  CPPUNIT_ASSERT_EQUAL((int)SolverI::E_SIZE_MISMATCH,fksolverpos.JntToCart(q_in, T, chain2.getNrOfSegments()));
309  CPPUNIT_ASSERT_EQUAL((int)SolverI::E_SIZE_MISMATCH,fksolvervel.JntToCart(q_in3, T2, chain2.getNrOfSegments()));
310  CPPUNIT_ASSERT_EQUAL((int)SolverI::E_SIZE_MISMATCH, jacsolver1.JntToJac(q_in, jac, chain2.getNrOfSegments()));
311  CPPUNIT_ASSERT_EQUAL((int)SolverI::E_SIZE_MISMATCH, jacdotsolver1.JntToJacDot(q_in3, t, chain2.getNrOfSegments()));
312  CPPUNIT_ASSERT_EQUAL((int)SolverI::E_SIZE_MISMATCH, jacdotsolver1.JntToJacDot(q_in3, jac, chain2.getNrOfSegments()));
313  CPPUNIT_ASSERT_EQUAL((int)SolverI::E_SIZE_MISMATCH, iksolver2.CartToJnt(q_in,t,q_out));
314  CPPUNIT_ASSERT_EQUAL((int)SolverI::E_SIZE_MISMATCH, iksolver_pinv_givens2.CartToJnt(q_in,t,q_out));
315  CPPUNIT_ASSERT_EQUAL((int)SolverI::E_SIZE_MISMATCH, iksolver_pinv_nso.CartToJnt(q_in,t,q_out));
316  CPPUNIT_ASSERT_EQUAL((int)SolverI::E_SIZE_MISMATCH, iksolver_wdls.CartToJnt(q_in,t,q_out));
317  CPPUNIT_ASSERT_EQUAL((int)SolverI::E_SIZE_MISMATCH, iksolverpos.CartToJnt(q_in,T,q_out));
318  CPPUNIT_ASSERT_EQUAL((int)SolverI::E_SIZE_MISMATCH, iksolverpos2.CartToJnt(q_in,T,q_out));
319  CPPUNIT_ASSERT_EQUAL((int)SolverI::E_SIZE_MISMATCH, iksolverpos3.CartToJnt(q_in,T,q_out));
320  CPPUNIT_ASSERT_EQUAL((int)SolverI::E_SIZE_MISMATCH, idsolver1.CartToJnt(q_in,q_in2,q_out,wrenches,q_out2));
321  CPPUNIT_ASSERT_EQUAL((int)SolverI::E_SIZE_MISMATCH, hdsolver.CartToJnt(q_in,q_in2,q_out,alpha,beta,wrenches, ff_tau, constraint_tau));
322  CPPUNIT_ASSERT_EQUAL((int)SolverI::E_SIZE_MISMATCH, dynparam.JntToCoriolis(q_in, q_in2, q_out));
323  CPPUNIT_ASSERT_EQUAL((int)SolverI::E_SIZE_MISMATCH, dynparam.JntToGravity(q_in, q_out));
324  CPPUNIT_ASSERT_EQUAL((int)SolverI::E_SIZE_MISMATCH, dynparam.JntToMass(q_in, m));
325 
326  q_in.resize(chain2.getNrOfJoints());
327  CPPUNIT_ASSERT_EQUAL((int)SolverI::E_SIZE_MISMATCH, jacsolver1.JntToJac(q_in, jac, chain2.getNrOfSegments()));
328  CPPUNIT_ASSERT_EQUAL((int)SolverI::E_SIZE_MISMATCH, iksolver2.CartToJnt(q_in,t,q_out));
329  CPPUNIT_ASSERT_EQUAL((int)SolverI::E_SIZE_MISMATCH, iksolver_pinv_givens2.CartToJnt(q_in,t,q_out));
330  CPPUNIT_ASSERT_EQUAL((int)SolverI::E_SIZE_MISMATCH, iksolver_pinv_nso.CartToJnt(q_in,t,q_out));
331  CPPUNIT_ASSERT_EQUAL((int)SolverI::E_SIZE_MISMATCH, iksolver_wdls.CartToJnt(q_in,t,q_out));
332  CPPUNIT_ASSERT_EQUAL((int)SolverI::E_SIZE_MISMATCH, iksolverpos.CartToJnt(q_in,T,q_out));
333  CPPUNIT_ASSERT_EQUAL((int)SolverI::E_SIZE_MISMATCH, iksolverpos2.CartToJnt(q_in,T,q_out));
334  CPPUNIT_ASSERT_EQUAL((int)SolverI::E_SIZE_MISMATCH, iksolverpos3.CartToJnt(q_in,T,q_out));
335  CPPUNIT_ASSERT_EQUAL((int)SolverI::E_SIZE_MISMATCH, iksolverpos2.CartToJnt(q_in,T,q_out));
336  CPPUNIT_ASSERT_EQUAL((int)SolverI::E_SIZE_MISMATCH, iksolverpos3.CartToJnt(q_in,T,q_out));
337  CPPUNIT_ASSERT_EQUAL((int)SolverI::E_SIZE_MISMATCH, idsolver1.CartToJnt(q_in,q_in2,q_out,wrenches,q_out2));
338  CPPUNIT_ASSERT_EQUAL((int)SolverI::E_SIZE_MISMATCH, hdsolver.CartToJnt(q_in,q_in2,q_out,alpha,beta,wrenches, ff_tau, constraint_tau));
339  CPPUNIT_ASSERT_EQUAL((int)SolverI::E_SIZE_MISMATCH, dynparam.JntToCoriolis(q_in, q_in2, q_out));
340  CPPUNIT_ASSERT_EQUAL((int)SolverI::E_SIZE_MISMATCH, dynparam.JntToGravity(q_in, q_out));
341  CPPUNIT_ASSERT_EQUAL((int)SolverI::E_SIZE_MISMATCH, dynparam.JntToMass(q_in, m));
342  q_in2.resize(chain2.getNrOfJoints());
343  CPPUNIT_ASSERT_EQUAL((int)SolverI::E_SIZE_MISMATCH, dynparam.JntToCoriolis(q_in, q_in2, q_out));
344  CPPUNIT_ASSERT_EQUAL((int)SolverI::E_SIZE_MISMATCH, idsolver1.CartToJnt(q_in,q_in2,q_out,wrenches,q_out2));
345  CPPUNIT_ASSERT_EQUAL((int)SolverI::E_SIZE_MISMATCH, hdsolver.CartToJnt(q_in,q_in2,q_out,alpha,beta,wrenches, ff_tau, constraint_tau));
346  wrenches.resize(chain2.getNrOfSegments());
347  CPPUNIT_ASSERT_EQUAL((int)SolverI::E_SIZE_MISMATCH, idsolver1.CartToJnt(q_in,q_in2,q_out,wrenches,q_out2));
348  CPPUNIT_ASSERT_EQUAL((int)SolverI::E_SIZE_MISMATCH, hdsolver.CartToJnt(q_in,q_in2,q_out,alpha,beta,wrenches, ff_tau, constraint_tau));
349  q_out2.resize(chain2.getNrOfSegments());
350  ff_tau.resize(chain2.getNrOfSegments());
351  constraint_tau.resize(chain2.getNrOfSegments());
352  CPPUNIT_ASSERT_EQUAL((int)SolverI::E_SIZE_MISMATCH, idsolver1.CartToJnt(q_in,q_in2,q_out,wrenches,q_out2));
353  CPPUNIT_ASSERT_EQUAL((int)SolverI::E_SIZE_MISMATCH, hdsolver.CartToJnt(q_in,q_in2,q_out,alpha,beta,wrenches, ff_tau, constraint_tau));
354  alpha.resize(nr_of_constraints);
355  CPPUNIT_ASSERT_EQUAL((int)SolverI::E_SIZE_MISMATCH, hdsolver.CartToJnt(q_in,q_in2,q_out,alpha,beta,wrenches, ff_tau, constraint_tau));
356  beta.resize(nr_of_constraints);
357  CPPUNIT_ASSERT_EQUAL((int)SolverI::E_SIZE_MISMATCH, hdsolver.CartToJnt(q_in,q_in2,q_out,alpha,beta,wrenches, ff_tau, constraint_tau));
358  jac.resize(chain2.getNrOfJoints());
359  CPPUNIT_ASSERT_EQUAL((int)SolverI::E_SIZE_MISMATCH, jacdotsolver1.JntToJacDot(q_in3, jac, chain2.getNrOfSegments()));
360  q_out.resize(chain2.getNrOfJoints());
361  q_in3.resize(chain2.getNrOfJoints());
362  m.resize(chain2.getNrOfJoints());
363  CPPUNIT_ASSERT_EQUAL((int)SolverI::E_NOERROR,fksolverpos.JntToCart(q_in, T, chain2.getNrOfSegments()));
364  CPPUNIT_ASSERT_EQUAL((int)SolverI::E_NOERROR,fksolvervel.JntToCart(q_in3, T2, chain2.getNrOfSegments()));
365  CPPUNIT_ASSERT_EQUAL((int)SolverI::E_NOERROR, jacsolver1.JntToJac(q_in, jac, chain2.getNrOfSegments()));
366  CPPUNIT_ASSERT_EQUAL((int)SolverI::E_NOERROR, jacdotsolver1.JntToJacDot(q_in3, jac, chain2.getNrOfSegments()));
367  CPPUNIT_ASSERT_EQUAL((int)SolverI::E_NOERROR, jacdotsolver1.JntToJacDot(q_in3, t, chain2.getNrOfSegments()));
368  CPPUNIT_ASSERT((int)SolverI::E_NOERROR <= iksolver2.CartToJnt(q_in,t,q_out));
369  CPPUNIT_ASSERT((int)SolverI::E_NOERROR <= iksolver_pinv_givens2.CartToJnt(q_in,t,q_out));
370  CPPUNIT_ASSERT((int)SolverI::E_NOERROR <= iksolver_pinv_nso.CartToJnt(q_in,t,q_out));
371  CPPUNIT_ASSERT((int)SolverI::E_NOERROR <= iksolver_wdls.CartToJnt(q_in,t,q_out));
372  CPPUNIT_ASSERT((int)SolverI::E_NOERROR <= iksolverpos.CartToJnt(q_in,T,q_out));
373  CPPUNIT_ASSERT((int)SolverI::E_NOERROR <= iksolverpos2.CartToJnt(q_in,T,q_out));
374  CPPUNIT_ASSERT((int)SolverI::E_NOERROR <= iksolverpos3.CartToJnt(q_in,T,q_out));
375  CPPUNIT_ASSERT((int)SolverI::E_NOERROR <= iksolverpos2.CartToJnt(q_in,T,q_out));
376  CPPUNIT_ASSERT((int)SolverI::E_NOERROR <= iksolverpos3.CartToJnt(q_in,T,q_out));
377  CPPUNIT_ASSERT((int)SolverI::E_NOERROR <= idsolver1.CartToJnt(q_in,q_in2,q_out,wrenches,q_out2));
378  CPPUNIT_ASSERT((int)SolverI::E_NOERROR <= hdsolver.CartToJnt(q_in,q_in2,q_out,alpha,beta,wrenches, ff_tau, constraint_tau));
379  CPPUNIT_ASSERT((int)SolverI::E_NOERROR <= dynparam.JntToCoriolis(q_in, q_in2, q_out));
380  CPPUNIT_ASSERT((int)SolverI::E_NOERROR <= dynparam.JntToGravity(q_in, q_out));
381  CPPUNIT_ASSERT((int)SolverI::E_NOERROR <= dynparam.JntToMass(q_in, m));
382 }
383 void SolverTest::FkPosAndJacTest()
384 {
385  ChainFkSolverPos_recursive fksolver1(chain1);
386  ChainJntToJacSolver jacsolver1(chain1);
387  FkPosAndJacLocal(chain1,fksolver1,jacsolver1);
388  ChainFkSolverPos_recursive fksolver2(chain2);
389  ChainJntToJacSolver jacsolver2(chain2);
390  FkPosAndJacLocal(chain2,fksolver2,jacsolver2);
391  ChainFkSolverPos_recursive fksolver3(chain3);
392  ChainJntToJacSolver jacsolver3(chain3);
393  FkPosAndJacLocal(chain3,fksolver3,jacsolver3);
394  ChainFkSolverPos_recursive fksolver4(chain4);
395  ChainJntToJacSolver jacsolver4(chain4);
396  FkPosAndJacLocal(chain4,fksolver4,jacsolver4);
397 }
398 
399 void SolverTest::FkVelAndJacTest()
400 {
401  ChainFkSolverVel_recursive fksolver1(chain1);
402  ChainJntToJacSolver jacsolver1(chain1);
403  FkVelAndJacLocal(chain1,fksolver1,jacsolver1);
404  ChainFkSolverVel_recursive fksolver2(chain2);
405  ChainJntToJacSolver jacsolver2(chain2);
406  FkVelAndJacLocal(chain2,fksolver2,jacsolver2);
407  ChainFkSolverVel_recursive fksolver3(chain3);
408  ChainJntToJacSolver jacsolver3(chain3);
409  FkVelAndJacLocal(chain3,fksolver3,jacsolver3);
410  ChainFkSolverVel_recursive fksolver4(chain4);
411  ChainJntToJacSolver jacsolver4(chain4);
412  FkVelAndJacLocal(chain4,fksolver4,jacsolver4);
413 }
414 
415 void SolverTest::FkVelAndIkVelTest()
416 {
417  //Chain1
418  std::cout<<"square problem"<<std::endl;
419  ChainFkSolverVel_recursive fksolver1(chain1);
420  ChainIkSolverVel_pinv iksolver1(chain1);
421  ChainIkSolverVel_pinv_givens iksolver_pinv_givens1(chain1);
422  std::cout<<"KDL-SVD-HouseHolder"<<std::endl;
423  FkVelAndIkVelLocal(chain1,fksolver1,iksolver1);
424  std::cout<<"KDL-SVD-Givens"<<std::endl;
425  FkVelAndIkVelLocal(chain1,fksolver1,iksolver_pinv_givens1);
426 
427  //Chain2
428  std::cout<<"underdetermined problem"<<std::endl;
429  ChainFkSolverVel_recursive fksolver2(chain2);
430  ChainIkSolverVel_pinv iksolver2(chain2);
431  ChainIkSolverVel_pinv_givens iksolver_pinv_givens2(chain2);
432  std::cout<<"KDL-SVD-HouseHolder"<<std::endl;
433  FkVelAndIkVelLocal(chain2,fksolver2,iksolver2);
434  std::cout<<"KDL-SVD-Givens"<<std::endl;
435  FkVelAndIkVelLocal(chain2,fksolver2,iksolver_pinv_givens2);
436 
437  //Chain3
438  std::cout<<"overdetermined problem"<<std::endl;
439  ChainFkSolverVel_recursive fksolver3(chain3);
440  ChainIkSolverVel_pinv iksolver3(chain3);
441  ChainIkSolverVel_pinv_givens iksolver_pinv_givens3(chain3);
442  std::cout<<"KDL-SVD-HouseHolder"<<std::endl;
443  FkVelAndIkVelLocal(chain3,fksolver3,iksolver3);
444  std::cout<<"KDL-SVD-Givens"<<std::endl;
445  FkVelAndIkVelLocal(chain3,fksolver3,iksolver_pinv_givens3);
446 
447  //Chain4
448  std::cout<<"overdetermined problem"<<std::endl;
449  ChainFkSolverVel_recursive fksolver4(chain4);
450  ChainIkSolverVel_pinv iksolver4(chain4);
451  ChainIkSolverVel_pinv_givens iksolver_pinv_givens4(chain4);
452  std::cout<<"KDL-SVD-HouseHolder"<<std::endl;
453  FkVelAndIkVelLocal(chain4,fksolver4,iksolver4);
454  std::cout<<"KDL-SVD-Givens"<<std::endl;
455  FkVelAndIkVelLocal(chain4,fksolver4,iksolver_pinv_givens4);
456 }
457 
458 void SolverTest::FkPosAndIkPosTest()
459 {
460  std::cout<<"square problem"<<std::endl;
461  ChainFkSolverPos_recursive fksolver1(chain1);
462  ChainIkSolverVel_pinv iksolver1v(chain1);
463  ChainIkSolverVel_pinv_givens iksolverv_pinv_givens1(chain1);
464  ChainIkSolverPos_NR iksolver1(chain1,fksolver1,iksolver1v);
465  ChainIkSolverPos_NR iksolver1_givens(chain1,fksolver1,iksolverv_pinv_givens1,1000);
466 
467  std::cout<<"KDL-SVD-HouseHolder"<<std::endl;
468  FkPosAndIkPosLocal(chain1,fksolver1,iksolver1);
469  std::cout<<"KDL-SVD-Givens"<<std::endl;
470  FkPosAndIkPosLocal(chain1,fksolver1,iksolver1_givens);
471 
472  std::cout<<"underdetermined problem"<<std::endl;
473  ChainFkSolverPos_recursive fksolver2(chain2);
474  ChainIkSolverVel_pinv iksolverv2(chain2);
475  ChainIkSolverVel_pinv_givens iksolverv_pinv_givens2(chain2);
476  ChainIkSolverPos_NR iksolver2(chain2,fksolver2,iksolverv2);
477  ChainIkSolverPos_NR iksolver2_givens(chain2,fksolver2,iksolverv_pinv_givens2,1000);
478 
479  std::cout<<"KDL-SVD-HouseHolder"<<std::endl;
480  FkPosAndIkPosLocal(chain2,fksolver2,iksolver2);
481  std::cout<<"KDL-SVD-Givens"<<std::endl;
482  FkPosAndIkPosLocal(chain2,fksolver2,iksolver2_givens);
483 
484  std::cout<<"overdetermined problem"<<std::endl;
485  ChainFkSolverPos_recursive fksolver3(chain3);
486  ChainIkSolverVel_pinv iksolverv3(chain3);
487  ChainIkSolverVel_pinv_givens iksolverv_pinv_givens3(chain3);
488  ChainIkSolverPos_NR iksolver3(chain3,fksolver3,iksolverv3);
489  ChainIkSolverPos_NR iksolver3_givens(chain3,fksolver3,iksolverv_pinv_givens3,1000);
490 
491  std::cout<<"KDL-SVD-HouseHolder"<<std::endl;
492  FkPosAndIkPosLocal(chain3,fksolver3,iksolver3);
493  std::cout<<"KDL-SVD-Givens"<<std::endl;
494  FkPosAndIkPosLocal(chain3,fksolver3,iksolver3_givens);
495 
496  std::cout<<"underdetermined problem with WGs segment constructor"<<std::endl;
497  ChainFkSolverPos_recursive fksolver4(chain4);
498  ChainIkSolverVel_pinv iksolverv4(chain4);
499  ChainIkSolverVel_pinv_givens iksolverv_pinv_givens4(chain4);
500  ChainIkSolverPos_NR iksolver4(chain4,fksolver4,iksolverv4,1000);
501  ChainIkSolverPos_NR iksolver4_givens(chain4,fksolver4,iksolverv_pinv_givens4,1000);
502 
503  std::cout<<"KDL-SVD-HouseHolder"<<std::endl;
504  FkPosAndIkPosLocal(chain4,fksolver4,iksolver4);
505  std::cout<<"KDL-SVD-Givens"<<std::endl;
506  FkPosAndIkPosLocal(chain4,fksolver4,iksolver4_givens);
507 }
508 
509 void SolverTest::IkSingularValueTest()
510 {
511  unsigned int maxiter = 30;
512  double eps = 1e-6 ;
513  int maxiter_vel = 30;
514  double eps_vel = 0.1 ;
515  Frame F, dF, F_des,F_solved;
516  KDL::Twist F_error ;
517 
518  std::cout<<"KDL-IK Solver Tests for Near Zero SVs"<<std::endl;
519 
520  ChainFkSolverPos_recursive fksolver(motomansia10);
521  ChainIkSolverVel_pinv ikvelsolver1(motomansia10,eps_vel,maxiter_vel);
522  ChainIkSolverPos_NR iksolver1(motomansia10,fksolver,ikvelsolver1,maxiter,eps);
523  unsigned int nj = motomansia10.getNrOfJoints();
524  JntArray q(nj), q_solved(nj) ;
525 
526 
527  std::cout<<"norminal case: convergence"<<std::endl;
528 
529  q(0) = 0. ;
530  q(1) = 0.5 ;
531  q(2) = 0.4 ;
532  q(3) = -PI_2 ;
533  q(4) = 0. ;
534  q(5) = 0. ;
535  q(6) = 0. ;
536 
537  dF.M = KDL::Rotation::RPY(0.1, 0.1, 0.1) ;
538  dF.p = KDL::Vector(0.01,0.01,0.01) ;
539 
540  CPPUNIT_ASSERT_EQUAL(0, fksolver.JntToCart(q,F));
541  F_des = F * dF ;
542 
543  CPPUNIT_ASSERT_EQUAL((int)SolverI::E_NOERROR,
544  iksolver1.CartToJnt(q, F_des, q_solved)); // converges
545  CPPUNIT_ASSERT_EQUAL((int)SolverI::E_NOERROR,
546  ikvelsolver1.getError());
547  CPPUNIT_ASSERT_EQUAL((unsigned int)1,
548  ikvelsolver1.getNrZeroSigmas()) ; // 1 singular value
549 
550  CPPUNIT_ASSERT_EQUAL(0, fksolver.JntToCart(q_solved,F_solved));
551  F_error = KDL::diff(F_solved,F_des);
552  CPPUNIT_ASSERT_EQUAL(F_des,F_solved);
553 
554  std::cout<<"nonconvergence: pseudoinverse singular"<<std::endl;
555 
556  q(0) = 0. ;
557  q(1) = 0.2 ;
558  q(2) = 0.4 ;
559  q(3) = -PI_2 ;
560  q(4) = 0. ;
561  q(5) = 0. ;
562  q(6) = 0. ;
563 
564  dF.M = KDL::Rotation::RPY(0.1, 0.1, 0.1) ;
565  dF.p = KDL::Vector(0.01,0.01,0.01) ;
566 
567  CPPUNIT_ASSERT_EQUAL(0, fksolver.JntToCart(q,F));
568  F_des = F * dF ;
569 
570  CPPUNIT_ASSERT_EQUAL((int)SolverI::E_MAX_ITERATIONS_EXCEEDED,
571  iksolver1.CartToJnt(q,F_des,q_solved)); // no converge
572  CPPUNIT_ASSERT_EQUAL((int)ChainIkSolverVel_pinv::E_CONVERGE_PINV_SINGULAR,
573  ikvelsolver1.getError()); // truncated SV solution
574  CPPUNIT_ASSERT_EQUAL((unsigned int)2,
575  ikvelsolver1.getNrZeroSigmas()) ; // 2 singular values (jac pseudoinverse singular)
576 
577  std::cout<<"nonconvergence: large displacement, low iterations"<<std::endl;
578 
579  q(0) = 0. ;
580  q(1) = 0.5 ;
581  q(2) = 0.4 ;
582  q(3) = -PI_2 ;
583  q(4) = 0. ;
584  q(5) = 0. ;
585  q(6) = 0. ;
586 
587  // big displacement
588  dF.M = KDL::Rotation::RPY(0.2, 0.2, 0.2) ;
589  dF.p = KDL::Vector(-0.2,-0.2, -0.2) ;
590 
591  // low iterations
592  maxiter = 5 ;
593  ChainIkSolverPos_NR iksolver2(motomansia10,fksolver,ikvelsolver1,maxiter,eps);
594 
595  CPPUNIT_ASSERT_EQUAL(0, fksolver.JntToCart(q,F));
596  F_des = F * dF ;
597 
598  CPPUNIT_ASSERT_EQUAL((int)SolverI::E_MAX_ITERATIONS_EXCEEDED,
599  iksolver2.CartToJnt(q,F_des,q_solved)); // does not converge
600  CPPUNIT_ASSERT_EQUAL((int)SolverI::E_NOERROR,
601  ikvelsolver1.getError());
602  CPPUNIT_ASSERT_EQUAL((unsigned int)1,
603  ikvelsolver1.getNrZeroSigmas()) ; // 1 singular value (jac pseudoinverse exists)
604 
605  std::cout<<"nonconvergence: fully singular"<<std::endl;
606 
607  q(0) = 0. ;
608  q(1) = 0. ;
609  q(2) = 0. ;
610  q(3) = 0. ;
611  q(4) = 0. ;
612  q(5) = 0. ;
613  q(6) = 0. ;
614 
615  dF.M = KDL::Rotation::RPY(0.1, 0.1, 0.1) ;
616  dF.p = KDL::Vector(0.01,0.01,0.01) ;
617 
618  CPPUNIT_ASSERT_EQUAL((int)SolverI::E_NOERROR, fksolver.JntToCart(q,F));
619  F_des = F * dF ;
620 
621  CPPUNIT_ASSERT_EQUAL((int)SolverI::E_MAX_ITERATIONS_EXCEEDED,
622  iksolver1.CartToJnt(q,F_des,q_solved)); // no converge
623  CPPUNIT_ASSERT_EQUAL((int)ChainIkSolverVel_pinv::E_CONVERGE_PINV_SINGULAR,
624  ikvelsolver1.getError()); // truncated SV solution
625  CPPUNIT_ASSERT_EQUAL((unsigned int)3,
626  ikvelsolver1.getNrZeroSigmas());
627 }
628 
629 
630 void SolverTest::IkVelSolverWDLSTest()
631 {
632  int maxiter = 30;
633  double eps = 0.1 ;
634  double lambda = 0.1 ;
635 
636  std::cout<<"KDL-IK WDLS Vel Solver Tests for Near Zero SVs"<<std::endl;
637 
638  KDL::ChainIkSolverVel_wdls ikvelsolver(motomansia10,eps,maxiter) ;
639  ikvelsolver.setLambda(lambda) ;
640  unsigned int nj = motomansia10.getNrOfJoints();
641  JntArray q(nj), dq(nj) ;
642 
643  KDL::Vector v05(0.05,0.05,0.05) ;
644  KDL::Twist dx(v05,v05) ;
645 
646  std::cout<<"smallest singular value is above threshold (no WDLS)"<<std::endl;
647 
648  q(0) = 0. ;
649  q(1) = 0.5 ;
650  q(2) = 0.4 ;
651  q(3) = -PI_2 ;
652  q(4) = 0. ;
653  q(5) = 0. ;
654  q(6) = 0. ;
655 
656  CPPUNIT_ASSERT_EQUAL((int)SolverI::E_NOERROR,
657  ikvelsolver.CartToJnt(q, dx, dq)) ; // wdls mode
658  CPPUNIT_ASSERT(1==ikvelsolver.getNrZeroSigmas()) ; // 1 singular value
659 
660 
661  std::cout<<"smallest singular value is below threshold (lambda is scaled)"<<std::endl;
662 
663  q(1) = 0.2 ;
664 
665  CPPUNIT_ASSERT_EQUAL((int)ChainIkSolverVel_wdls::E_CONVERGE_PINV_SINGULAR,
666  ikvelsolver.CartToJnt(q, dx, dq)) ; // wdls mode
667  CPPUNIT_ASSERT_EQUAL((unsigned int)2,ikvelsolver.getNrZeroSigmas()) ; // 2 singular values
668  CPPUNIT_ASSERT_EQUAL(ikvelsolver.getLambdaScaled(),
669  sqrt(1.0-(ikvelsolver.getSigmaMin()/eps)*(ikvelsolver.getSigmaMin()/eps))*lambda) ;
670 
671  std::cout<<"smallest singular value is zero (lambda_scaled=lambda)"<<std::endl;
672 
673  q(1) = 0.0 ;
674 
675  CPPUNIT_ASSERT_EQUAL((int)ChainIkSolverVel_wdls::E_CONVERGE_PINV_SINGULAR,
676  ikvelsolver.CartToJnt(q, dx, dq)) ; // wdls mode
677  CPPUNIT_ASSERT_EQUAL((unsigned int)2,ikvelsolver.getNrZeroSigmas()) ; // 2 singular values
678  CPPUNIT_ASSERT_EQUAL(ikvelsolver.getLambdaScaled(),lambda) ; // full value
679 
680  // fully singular
681  q(2) = 0.0 ;
682  q(3) = 0.0 ;
683 
684  CPPUNIT_ASSERT_EQUAL((int)ChainIkSolverVel_wdls::E_CONVERGE_PINV_SINGULAR,
685  ikvelsolver.CartToJnt(q, dx, dq)) ; // wdls mode
686  CPPUNIT_ASSERT_EQUAL(4,(int)ikvelsolver.getNrZeroSigmas()) ;
687  CPPUNIT_ASSERT_EQUAL(ikvelsolver.getLambdaScaled(),lambda) ; // full value
688 }
689 
690 
691 void SolverTest::FkPosAndJacLocal(Chain& chain,ChainFkSolverPos& fksolverpos,ChainJntToJacSolver& jacsolver)
692 {
693  double deltaq = 1E-4;
694 
695  Frame F1,F2;
696 
697  JntArray q(chain.getNrOfJoints());
698  Jacobian jac(chain.getNrOfJoints());
699 
700  for(unsigned int i=0; i<chain.getNrOfJoints(); i++)
701  {
702  random(q(i));
703  }
704 
705  jacsolver.JntToJac(q,jac);
706 
707  for (unsigned int i=0; i< q.rows() ; i++)
708  {
709  // test the derivative of J towards qi
710  double oldqi = q(i);
711  q(i) = oldqi+deltaq;
712  CPPUNIT_ASSERT_EQUAL((int)SolverI::E_NOERROR, fksolverpos.JntToCart(q,F2));
713  q(i) = oldqi-deltaq;
714  CPPUNIT_ASSERT_EQUAL((int)SolverI::E_NOERROR, fksolverpos.JntToCart(q,F1));
715  q(i) = oldqi;
716  // check Jacobian :
717  Twist Jcol1 = diff(F1,F2,2*deltaq);
718  Twist Jcol2(Vector(jac(0,i),jac(1,i),jac(2,i)),
719  Vector(jac(3,i),jac(4,i),jac(5,i)));
720 
721  //CPPUNIT_ASSERT_EQUAL(true,Equal(Jcol1,Jcol2,epsJ));
722  CPPUNIT_ASSERT_EQUAL(Jcol1,Jcol2);
723  }
724 }
725 
726 void SolverTest::FkVelAndJacLocal(Chain& chain, ChainFkSolverVel& fksolvervel, ChainJntToJacSolver& jacsolver)
727 {
728  JntArray q(chain.getNrOfJoints());
729  JntArray qdot(chain.getNrOfJoints());
730 
731  for(unsigned int i=0; i<chain.getNrOfJoints(); i++)
732  {
733  random(q(i));
734  random(qdot(i));
735  }
736  JntArrayVel qvel(q,qdot);
737  Jacobian jac(chain.getNrOfJoints());
738 
739  FrameVel cart;
740  Twist t;
741 
742  jacsolver.JntToJac(qvel.q,jac);
743  CPPUNIT_ASSERT_EQUAL((int)SolverI::E_NOERROR, fksolvervel.JntToCart(qvel,cart));
744  MultiplyJacobian(jac,qvel.qdot,t);
745  CPPUNIT_ASSERT_EQUAL(cart.deriv(),t);
746 }
747 
748 void SolverTest::FkVelAndIkVelLocal(Chain& chain, ChainFkSolverVel& fksolvervel, ChainIkSolverVel& iksolvervel)
749 {
750 
751  JntArray q(chain.getNrOfJoints());
752  JntArray qdot(chain.getNrOfJoints());
753 
754  for(unsigned int i=0; i<chain.getNrOfJoints(); i++)
755  {
756  random(q(i));
757  random(qdot(i));
758  }
759  JntArrayVel qvel(q,qdot);
760  JntArray qdot_solved(chain.getNrOfJoints());
761 
762  FrameVel cart;
763 
764  CPPUNIT_ASSERT_EQUAL((int)SolverI::E_NOERROR, fksolvervel.JntToCart(qvel,cart));
765 
766  CPPUNIT_ASSERT_EQUAL((int)SolverI::E_NOERROR, iksolvervel.CartToJnt(qvel.q,cart.deriv(),qdot_solved));
767 
768  qvel.deriv()=qdot_solved;
769 
770  if(chain.getNrOfJoints()<=6)
771  CPPUNIT_ASSERT(Equal(qvel.qdot,qdot_solved,1e-5));
772  else
773  {
774  FrameVel cart_solved;
775  CPPUNIT_ASSERT_EQUAL((int)SolverI::E_NOERROR,fksolvervel.JntToCart(qvel,cart_solved));
776  CPPUNIT_ASSERT(Equal(cart.deriv(),cart_solved.deriv(),1e-5));
777  }
778 }
779 
780 
781 void SolverTest::FkPosAndIkPosLocal(Chain& chain,ChainFkSolverPos& fksolverpos, ChainIkSolverPos& iksolverpos)
782 {
783  JntArray q(chain.getNrOfJoints());
784  for(unsigned int i=0; i<chain.getNrOfJoints(); i++)
785  {
786  random(q(i));
787  }
788  JntArray q_init(chain.getNrOfJoints());
789  double tmp;
790  for(unsigned int i=0; i<chain.getNrOfJoints(); i++)
791  {
792  random(tmp);
793  q_init(i)=q(i)+0.1*tmp;
794  }
795  JntArray q_solved(q);
796 
797  Frame F1,F2;
798 
799  CPPUNIT_ASSERT_EQUAL((int)SolverI::E_NOERROR, fksolverpos.JntToCart(q,F1));
800  CPPUNIT_ASSERT_EQUAL((int)SolverI::E_NOERROR, iksolverpos.CartToJnt(q_init,F1,q_solved));
801  CPPUNIT_ASSERT_EQUAL((int)SolverI::E_NOERROR, fksolverpos.JntToCart(q_solved,F2));
802 
803  CPPUNIT_ASSERT_EQUAL(F1,F2);
804  //CPPUNIT_ASSERT_EQUAL(q,q_solved);
805 
806 }
807 
808 
809 void SolverTest::VereshchaginTest()
810 {
811  std::cout << "KDL Vereshchagin Hybrid Dynamics Tests" <<std::endl;
812 
813  // ########################################################################################
814  // Vereshchagin solver test 1
815  // ########################################################################################
833  int solver_return = 0;
834  double eps = 1.e-3;
835 
836  unsigned int nj = kukaLWR.getNrOfJoints();
837  unsigned int ns = kukaLWR.getNrOfSegments();
838 
839  // Necessary test for the used robot model: KDL's implementation of the Vereshchagin solver
840  // can only work with the robot chains that have equal number of joints and segments
841  CPPUNIT_ASSERT(Equal(nj, ns));
842 
843  // Joint position, velocity, acceleration, feed-forward and constraint torques
844  KDL::JntArray q(nj); //input
845  KDL::JntArray qd(nj); //input
846  KDL::JntArray qdd(nj); //output
847  KDL::JntArray ff_tau(nj); //input
848  KDL::JntArray constraint_tau(nj); //output
849 
850  // Random configuration
851  q(0) = 1.6;
852  q(1) = 0.0;
853  q(2) = -1.6;
854  q(3) = -1.57;
855  q(4) = 0.0;
856  q(5) = 1.57;
857  q(6) = -0.8;
858 
859  qd(0) = 1.0;
860  qd(1) = -2.0;
861  qd(2) = 3.0;
862  qd(3) = -4.0;
863  qd(4) = 5.0;
864  qd(5) = -6.0;
865  qd(6) = 7.0;
866 
867  // Random feedforwad torques acting on robot joints
868  ff_tau(0) = 5.0;
869  ff_tau(1) = 0.0;
870  ff_tau(2) = 0.0;
871  ff_tau(3) = 0.0;
872  ff_tau(4) = 0.0;
873  ff_tau(5) = -6.0;
874  ff_tau(6) = 0.0;
875 
876  // External Wrench acting on the end-effector, expressed in base link coordinates
877  // Vereshchagin solver expects that external wrenches are expressed w.r.t. robot's base frame
878  KDL::Vector f(10.0, 15.0, 0.0);
879  KDL::Vector n(0.0, 0.0, 5.0);
880  KDL::Wrench f_tool(f, n);
881  KDL::Wrenches f_ext(ns);
882  f_ext[ns - 1] = f_tool; //input
883 
892  int number_of_constraints = 6;
893 
894  // Constraint Unit forces defined for the end-effector
895  Jacobian alpha_unit_force(number_of_constraints);
896 
897  // Set directions in which the constraint force should work. Alpha in the solver
898  Twist unit_force_x_l(
899  Vector(1.0, 0.0, 0.0),
900  Vector(0.0, 0.0, 0.0));
901  alpha_unit_force.setColumn(0, unit_force_x_l); // constraint active
902 
903  Twist unit_force_y_l(
904  Vector(0.0, 1.0, 0.0),
905  Vector(0.0, 0.0, 0.0));
906  alpha_unit_force.setColumn(1, unit_force_y_l); // constraint active
907 
908  Twist unit_force_z_l(
909  Vector(0.0, 0.0, 1.0),
910  Vector(0.0, 0.0, 0.0));
911  alpha_unit_force.setColumn(2, unit_force_z_l); // constraint active
912 
913  Twist unit_force_x_a(
914  Vector(0.0, 0.0, 0.0),
915  Vector(0.0, 0.0, 0.0));
916  alpha_unit_force.setColumn(3, unit_force_x_a); // constraint diabled... In this direction, end-effector's motion is left to emerge naturally
917 
918  Twist unit_force_y_a(
919  Vector(0.0, 0.0, 0.0),
920  Vector(0.0, 0.0, 0.0));
921  alpha_unit_force.setColumn(4, unit_force_y_a); // constraint diabled... In this direction, end-effector's motion is left to emerge naturally
922 
923  Twist unit_force_z_a(
924  Vector(0.0, 0.0, 0.0),
925  Vector(0.0, 0.0, 1.0));
926  alpha_unit_force.setColumn(5, unit_force_z_a); // constraint active
927 
928  // Acceleration energy for the end-effector.
929  JntArray beta_energy(number_of_constraints);
930  beta_energy(0) = -0.5;
931  beta_energy(1) = -0.5;
932  beta_energy(2) = 0.0;
933  beta_energy(3) = 0.0; // this value has no impact on computations, since its corresponding constraint is disabled
934  beta_energy(4) = 0.0; // this value has no impact on computations, since its corresponding constraint is disabled
935  beta_energy(5) = 0.2;
936 
937  // Arm root acceleration (robot's base mounted on an even surface)
938  // Note: Vereshchagin solver takes root acc. with opposite sign comparead to the KDL's FD and RNE solvers
939  Twist root_Acc(Vector(0.0, 0.0, 9.81), Vector(0.0, 0.0, 0.0));
940 
941  ChainHdSolver_Vereshchagin vereshchaginSolver(kukaLWR, root_Acc, number_of_constraints);
942  solver_return = vereshchaginSolver.CartToJnt(q, qd, qdd, alpha_unit_force, beta_energy, f_ext, ff_tau, constraint_tau);
943  if (solver_return < 0) std::cout << "KDL: Vereshchagin solver ERROR: " << solver_return << std::endl;
944 
945  // ########################################################################################
946  // Final comparison of the _resultant_ end-effector's Cartesian accelerations
947  // and the task-specified acceleration constraints
948 
949  // Number of frames on the robot = ns + 1
950  std::vector<Twist> xDotdot(ns + 1);
951  // This solver's function returns Cartesian accelerations of links in robot base coordinates
952  vereshchaginSolver.getTransformedLinkAcceleration(xDotdot);
953  CPPUNIT_ASSERT(Equal(beta_energy(0), xDotdot[ns].vel(0), eps));
954  CPPUNIT_ASSERT(Equal(beta_energy(1), xDotdot[ns].vel(1), eps));
955  CPPUNIT_ASSERT(Equal(beta_energy(2), xDotdot[ns].vel(2), eps));
956  CPPUNIT_ASSERT(Equal(beta_energy(5), xDotdot[ns].rot(2), eps));
957 
958  // Additional getters for the intermediate solver's outputs: Useful for state- simulation and estimation purposes
959  // Magnitude of the constraint forces acting on the end-effector: Lagrange Multiplier
960  Eigen::VectorXd nu(number_of_constraints);
961  vereshchaginSolver.getContraintForceMagnitude(nu);
962  CPPUNIT_ASSERT(Equal(nu(0), 669693.30355, eps));
963  CPPUNIT_ASSERT(Equal(nu(1), 5930.60826, eps));
964  CPPUNIT_ASSERT(Equal(nu(2), -639.5238, eps));
965  CPPUNIT_ASSERT(Equal(nu(3), 0.000, eps)); // constraint was not active in the task specification
966  CPPUNIT_ASSERT(Equal(nu(4), 0.000, eps)); // constraint was not active in the task specification
967  CPPUNIT_ASSERT(Equal(nu(5), 573.90485, eps));
968 
969  // Total torque acting on each joint
970  JntArray total_tau(nj);
971  vereshchaginSolver.getTotalTorque(total_tau);
972  CPPUNIT_ASSERT(Equal(total_tau(0), 2013.3541, eps));
973  CPPUNIT_ASSERT(Equal(total_tau(1), -6073.4999, eps));
974  CPPUNIT_ASSERT(Equal(total_tau(2), 2227.4487, eps));
975  CPPUNIT_ASSERT(Equal(total_tau(3), 56.87456, eps));
976  CPPUNIT_ASSERT(Equal(total_tau(4), -11.3789, eps));
977  CPPUNIT_ASSERT(Equal(total_tau(5), -6.05957, eps));
978  CPPUNIT_ASSERT(Equal(total_tau(6), 569.0776, eps));
979 
980  // ########################################################################################
981  // Vereshchagin solver test 2
982  // ########################################################################################
983  Vector constrainXLinear(1.0, 0.0, 0.0);
984  Vector constrainXAngular(0.0, 0.0, 0.0);
985  Vector constrainYLinear(0.0, 0.0, 0.0);
986  Vector constrainYAngular(0.0, 0.0, 0.0);
987  // Vector constrainZLinear(0.0, 0.0, 0.0);
988  //Vector constrainZAngular(0.0, 0.0, 0.0);
989  Twist constraintForcesX(constrainXLinear, constrainXAngular);
990  Twist constraintForcesY(constrainYLinear, constrainYAngular);
991  //Twist constraintForcesZ(constrainZLinear, constrainZAngular);
992  Jacobian alpha(1);
993  //alpha.setColumn(0, constraintForcesX);
994  alpha.setColumn(0, constraintForcesX);
995  //alpha.setColumn(0, constraintForcesZ);
996 
997  //Acceleration energy at the end-effector
998  JntArray betha(1); //set to zero
999  betha(0) = 0.0;
1000  //betha(1) = 0.0;
1001  //betha(2) = 0.0;
1002 
1003  //arm root acceleration
1004  Vector linearAcc(0.0, 10, 0.0); //gravitational acceleration along Y
1005  Vector angularAcc(0.0, 0.0, 0.0);
1006  Twist twist1(linearAcc, angularAcc);
1007 
1008  //external forces on the arm
1009  Vector externalForce1(0.0, 0.0, 0.0);
1010  Vector externalTorque1(0.0, 0.0, 0.0);
1011  Vector externalForce2(0.0, 0.0, 0.0);
1012  Vector externalTorque2(0.0, 0.0, 0.0);
1013  Wrench externalNetForce1(externalForce1, externalTorque1);
1014  Wrench externalNetForce2(externalForce2, externalTorque2);
1015  Wrenches externalNetForce;
1016  externalNetForce.push_back(externalNetForce1);
1017  externalNetForce.push_back(externalNetForce2);
1018  //~Definition of constraints and external disturbances
1019  //-------------------------------------------------------------------------------------//
1020 
1021 
1022  //Definition of solver and initial configuration
1023  //-------------------------------------------------------------------------------------//
1024  int numberOfConstraints = 1;
1025  ChainHdSolver_Vereshchagin constraintSolver(chaindyn, twist1, numberOfConstraints);
1026 
1027  //These arrays of joint values contain actual and desired values
1028  //actual is generated by a solver and integrator
1029  //desired is given by an interpolator
1030  //error is the difference between desired-actual
1031  //in this test only the actual values are printed.
1032  const int k = 1;
1033  JntArray jointPoses[k];
1034  JntArray jointRates[k];
1035  JntArray jointAccelerations[k];
1036  JntArray jointFFTorques[k];
1037  JntArray jointConstraintTorques[k];
1038  for (int i = 0; i < k; i++)
1039  {
1040  JntArray jointValues(chaindyn.getNrOfJoints());
1041  jointPoses[i] = jointValues;
1042  jointRates[i] = jointValues;
1043  jointAccelerations[i] = jointValues;
1044  jointFFTorques[i] = jointValues;
1045  jointConstraintTorques[i] = jointValues;
1046  }
1047 
1048  // Initial arm position configuration/constraint
1049  JntArray jointInitialPose(chaindyn.getNrOfJoints());
1050  jointInitialPose(0) = 0.0; // initial joint0 pose
1051  jointInitialPose(1) = PI/6.0; //initial joint1 pose, negative in clockwise
1052  //j0=0.0, j1=pi/6.0 correspond to x = 0.2, y = -0.7464
1053  //j0=2*pi/3.0, j1=pi/4.0 correspond to x = 0.44992, y = 0.58636
1054 
1055  //actual
1056  jointPoses[0](0) = jointInitialPose(0);
1057  jointPoses[0](1) = jointInitialPose(1);
1058 
1059  //~Definition of solver and initial configuration
1060  //-------------------------------------------------------------------------------------//
1061 
1062 
1063  //Definition of process main loop
1064  //-------------------------------------------------------------------------------------//
1065  //Time required to complete the task move(frameinitialPose, framefinalPose)
1066  double taskTimeConstant = 0.1;
1067  double simulationTime = 1*taskTimeConstant;
1068  double timeDelta = 0.01;
1069 
1070  const std::string msg = "Assertion failed, check matrix and array sizes";
1071 
1072  for (double t = 0.0; t <=simulationTime; t = t + timeDelta)
1073  {
1074  CPPUNIT_ASSERT_EQUAL((int)SolverI::E_NOERROR, constraintSolver.CartToJnt(jointPoses[0], jointRates[0], jointAccelerations[0], alpha, betha, externalNetForce, jointFFTorques[0], jointConstraintTorques[0]));
1075 
1076  //Integration(robot joint values for rates and poses; actual) at the given "instanteneous" interval for joint position and velocity.
1077  jointRates[0](0) = jointRates[0](0) + jointAccelerations[0](0) * timeDelta; //Euler Forward
1078  jointPoses[0](0) = jointPoses[0](0) + (jointRates[0](0) - jointAccelerations[0](0) * timeDelta / 2.0) * timeDelta; //Trapezoidal rule
1079  jointRates[0](1) = jointRates[0](1) + jointAccelerations[0](1) * timeDelta; //Euler Forward
1080  jointPoses[0](1) = jointPoses[0](1) + (jointRates[0](1) - jointAccelerations[0](1) * timeDelta / 2.0) * timeDelta;
1081  jointFFTorques[0] = jointConstraintTorques[0];
1082  //printf("time, j0_pose, j1_pose, j0_rate, j1_rate, j0_acc, j1_acc, j0_constraintTau, j1_constraintTau \n");
1083  printf("%f %f %f %f %f %f %f %f %f\n", t, jointPoses[0](0), jointPoses[0](1), jointRates[0](0), jointRates[0](1), jointAccelerations[0](0), jointAccelerations[0](1), jointConstraintTorques[0](0), jointConstraintTorques[0](1));
1084  }
1085 }
1086 
1087 void SolverTest::FkPosVectTest()
1088 {
1089  ChainFkSolverPos_recursive fksolver1(chain1);
1090  std::vector<Frame> v_out(chain1.getNrOfSegments());
1091 
1092  JntArray q(chain1.getNrOfJoints());
1093  JntArray qdot(chain1.getNrOfJoints());
1094 
1095  for(unsigned int i=0; i<chain1.getNrOfJoints(); i++)
1096  {
1097  random(q(i));
1098  random(qdot(i));
1099  }
1100  Frame f_out;
1101  fksolver1.JntToCart(q,v_out);
1102  fksolver1.JntToCart(q,f_out);
1103 
1104  CPPUNIT_ASSERT(Equal(v_out[chain1.getNrOfSegments()-1],f_out,1e-5));
1105 }
1106 
1107 void SolverTest::FkVelVectTest()
1108 {
1109  ChainFkSolverVel_recursive fksolver1(chain1);
1110  std::vector<FrameVel> v_out(chain1.getNrOfSegments());
1111 
1112  JntArray q(chain1.getNrOfJoints());
1113  JntArray qdot(chain1.getNrOfJoints());
1114 
1115  for(unsigned int i=0; i<chain1.getNrOfJoints(); i++)
1116  {
1117  random(q(i));
1118  random(qdot(i));
1119  }
1120  JntArrayVel qvel(q,qdot);
1121  FrameVel f_out;
1122  fksolver1.JntToCart(qvel,v_out);
1123  fksolver1.JntToCart(qvel,f_out);
1124 
1125  CPPUNIT_ASSERT(Equal(v_out[chain1.getNrOfSegments()-1],f_out,1e-5));
1126 }
1127 
1128 void SolverTest::FdSolverDevelopmentTest()
1129 {
1130  int ret;
1131  double eps=1.e-3;
1132 
1133  std::cout<<"KDL FD Solver Development Test for Motoman SIA10"<<std::endl;
1134 
1135  // NOTE: This is prototype code for the KDL forward dynamics solver class
1136  // based on the Recurse Newton Euler Method: ChainFdSolver_RNE
1137 
1138  // Dynamics Solver
1139  Vector gravity(0.0, 0.0, -9.81); // base frame
1140  ChainDynParam DynSolver = KDL::ChainDynParam(motomansia10dyn, gravity);
1141 
1142  unsigned int nj = motomansia10dyn.getNrOfJoints();
1143  unsigned int ns = motomansia10dyn.getNrOfSegments();
1144 
1145  // Joint position, velocity, and acceleration
1146  JntArray q(nj);
1147  JntArray qd(nj);
1148  JntArray qdd(nj);
1149 
1150  // random
1151  q(0) = 0.2;
1152  q(1) = 0.6;
1153  q(2) = 1.;
1154  q(3) = 0.5;
1155  q(4) = -1.4;
1156  q(5) = 0.3;
1157  q(6) = -0.8;
1158 
1159  qd(0) = 1.;
1160  qd(1) = -2.;
1161  qd(2) = 3.;
1162  qd(3) = -4.;
1163  qd(4) = 5.;
1164  qd(5) = -6.;
1165  qd(6) = 7.;
1166 
1167  // Validate FK
1168  ChainFkSolverPos_recursive fksolver(motomansia10dyn);
1169  Frame f_out;
1170  fksolver.JntToCart(q,f_out);
1171  CPPUNIT_ASSERT(Equal(-0.547, f_out.p(0), eps));
1172  CPPUNIT_ASSERT(Equal(-0.301, f_out.p(1), eps));
1173  CPPUNIT_ASSERT(Equal(0.924, f_out.p(2), eps));
1174  CPPUNIT_ASSERT(Equal(0.503, f_out.M(0,0), eps));
1175  CPPUNIT_ASSERT(Equal(0.286, f_out.M(0,1), eps));
1176  CPPUNIT_ASSERT(Equal(-0.816, f_out.M(0,2), eps));
1177  CPPUNIT_ASSERT(Equal(-0.859, f_out.M(1,0), eps));
1178  CPPUNIT_ASSERT(Equal(0.269, f_out.M(1,1), eps));
1179  CPPUNIT_ASSERT(Equal(-0.436, f_out.M(1,2), eps));
1180  CPPUNIT_ASSERT(Equal(0.095, f_out.M(2,0), eps));
1181  CPPUNIT_ASSERT(Equal(0.920, f_out.M(2,1), eps));
1182  CPPUNIT_ASSERT(Equal(0.381, f_out.M(2,2), eps));
1183 
1184  // Validate Jacobian
1185  ChainJntToJacSolver jacsolver(motomansia10dyn);
1186  Jacobian jac(nj);
1187  jacsolver.JntToJac(q, jac);
1188  double Jac[6][7] =
1189  {{0.301,-0.553,0.185,0.019,0.007,-0.086,0.},
1190  {-0.547,-0.112,-0.139,-0.376,-0.037,0.063,0.},
1191  {0.,-0.596,0.105,-0.342,-0.026,-0.113,0.},
1192  {0.,0.199,-0.553,0.788,-0.615,0.162,-0.816},
1193  {0.,-0.980,-0.112,-0.392,-0.536,-0.803,-0.436},
1194  {1.,0.,0.825,0.475,0.578,-0.573,0.381}};
1195  for ( unsigned int i=0; i<6; i++ ) {
1196  for ( unsigned int j=0; j<nj; j++ ) {
1197  CPPUNIT_ASSERT(Equal(jac(i,j), Jac[i][j], eps));
1198  }
1199  }
1200 
1201  // Return values
1202  JntArray taugrav(nj);
1203  JntArray taucor(nj);
1204  JntSpaceInertiaMatrix H(nj), Heff(nj);
1205 
1206  // Compute Dynamics (torque in N-m)
1207  ret = DynSolver.JntToGravity(q, taugrav);
1208  if (ret < 0) std::cout << "KDL: inverse dynamics ERROR: " << ret << std::endl;
1209  CPPUNIT_ASSERT(Equal(0.000, taugrav(0), eps));
1210  CPPUNIT_ASSERT(Equal(-36.672, taugrav(1), eps));
1211  CPPUNIT_ASSERT(Equal(4.315, taugrav(2), eps));
1212  CPPUNIT_ASSERT(Equal(-11.205, taugrav(3), eps));
1213  CPPUNIT_ASSERT(Equal(0.757, taugrav(4), eps));
1214  CPPUNIT_ASSERT(Equal(1.780, taugrav(5), eps));
1215  CPPUNIT_ASSERT(Equal(0.000, taugrav(6), eps));
1216 
1217  ret = DynSolver.JntToCoriolis(q, qd, taucor);
1218  if (ret < 0) std::cout << "KDL: inverse dynamics ERROR: " << ret << std::endl;
1219  CPPUNIT_ASSERT(Equal(-15.523, taucor(0), eps));
1220  CPPUNIT_ASSERT(Equal(24.250, taucor(1), eps));
1221  CPPUNIT_ASSERT(Equal(-6.862, taucor(2), eps));
1222  CPPUNIT_ASSERT(Equal(6.303, taucor(3), eps));
1223  CPPUNIT_ASSERT(Equal(0.110, taucor(4), eps));
1224  CPPUNIT_ASSERT(Equal(-4.898, taucor(5), eps));
1225  CPPUNIT_ASSERT(Equal(-0.249, taucor(6), eps));
1226 
1227  ret = DynSolver.JntToMass(q, H);
1228  if (ret < 0) std::cout << "KDL: inverse dynamics ERROR: " << ret << std::endl;
1229  double Hexp[7][7] =
1230  {{6.8687,-0.4333,0.4599,0.6892,0.0638,-0.0054,0.0381},
1231  {-0.4333,8.8324,-0.5922,0.7905,0.0003,-0.0242,0.0265},
1232  {0.4599,-0.5922,3.3496,-0.0253,0.1150,-0.0243,0.0814},
1233  {0.6892,0.7905,-0.0253,3.9623,-0.0201,0.0087,-0.0291},
1234  {0.0638,0.0003,0.1150,-0.0201,1.1234,0.0029,0.0955},
1235  {-0.0054,-0.0242,-0.0243,0.0087,0.0029,1.1425,0},
1236  {0.0381,0.0265,0.0814,-0.0291,0.0955,0,1.1000}};
1237  for ( unsigned int i=0; i<nj; i++ ) {
1238  for ( unsigned int j=0; j<nj; j++ ) {
1239  CPPUNIT_ASSERT(Equal(H(i,j), Hexp[i][j], eps));
1240  }
1241  }
1242 
1243  // Inverse Dynamics:
1244  // T = H * qdd + Tcor + Tgrav - J^T * Fext
1245  // Forward Dynamics
1246  // 1. Call JntToMass from ChainDynParam to get H
1247  // 2. Call ID with qdd=0 to get T=Tcor+Tgrav+J^T*Fext
1248  // 3. Calculate qdd = H^-1 * T
1249  KDL::ChainIdSolver_RNE IdSolver = KDL::ChainIdSolver_RNE(motomansia10dyn, gravity);
1250 
1251  // In tool coordinates
1252  Vector f(10,-20,30) ;
1253  Vector n(3,-4,5) ;
1254  Wrench f_tool(f,n);
1255  // In local link coordinates
1256  Wrenches f_ext(ns);
1257  for(unsigned int i=0;i<ns;i++){
1258  SetToZero(f_ext[i]);
1259  }
1260  f_ext[ns-1]=f_tool;
1261 
1262  JntArray Tnoninertial(nj);
1263  JntArray jntarraynull(nj);
1264  SetToZero(jntarraynull);
1265  IdSolver.CartToJnt(q, qd, jntarraynull, f_ext, Tnoninertial);
1266  CPPUNIT_ASSERT(Equal(-21.252, Tnoninertial(0), eps));
1267  CPPUNIT_ASSERT(Equal(-37.933, Tnoninertial(1), eps));
1268  CPPUNIT_ASSERT(Equal(-2.497, Tnoninertial(2), eps));
1269  CPPUNIT_ASSERT(Equal(-15.289, Tnoninertial(3), eps));
1270  CPPUNIT_ASSERT(Equal(-4.646, Tnoninertial(4), eps));
1271  CPPUNIT_ASSERT(Equal(-9.201, Tnoninertial(5), eps));
1272  CPPUNIT_ASSERT(Equal(-5.249, Tnoninertial(6), eps));
1273 
1274  // get acceleration using inverse symmetric matrix times vector
1275  Eigen::MatrixXd H_eig(nj,nj), L(nj,nj);
1276  Eigen::VectorXd Tnon_eig(nj), D(nj), r(nj), acc_eig(nj);
1277  for(unsigned int i=0;i<nj;i++){
1278  Tnon_eig(i) = -Tnoninertial(i);
1279  for(unsigned int j=0;j<nj;j++){
1280  H_eig(i,j) = H(i,j);
1281  }
1282  }
1283  ldl_solver_eigen(H_eig, Tnon_eig, L, D, r, acc_eig);
1284  for(unsigned int i=0;i<nj;i++){
1285  qdd(i) = acc_eig(i);
1286  }
1287  CPPUNIT_ASSERT(Equal(2.998, qdd(0), eps));
1288  CPPUNIT_ASSERT(Equal(4.289, qdd(1), eps));
1289  CPPUNIT_ASSERT(Equal(0.946, qdd(2), eps));
1290  CPPUNIT_ASSERT(Equal(2.518, qdd(3), eps));
1291  CPPUNIT_ASSERT(Equal(3.530, qdd(4), eps));
1292  CPPUNIT_ASSERT(Equal(8.150, qdd(5), eps));
1293  CPPUNIT_ASSERT(Equal(4.254, qdd(6), eps));
1294 }
1295 
1296 void SolverTest::FdSolverConsistencyTest()
1297 {
1298  int ret;
1299  double eps=1.e-3;
1300 
1301  std::cout<<"KDL FD Solver Consistency Test for Motoman SIA10"<<std::endl;
1302 
1303  // NOTE: Compute the forward and inverse dynamics and test for consistency
1304 
1305  // Forward Dynamics Solver
1306  Vector gravity(0.0, 0.0, -9.81); // base frame
1307  KDL::ChainFdSolver_RNE FdSolver = KDL::ChainFdSolver_RNE(motomansia10dyn, gravity);
1308 
1309  unsigned int nj = motomansia10dyn.getNrOfJoints();
1310  unsigned int ns = motomansia10dyn.getNrOfSegments();
1311 
1312  // Joint position, velocity, and acceleration
1313  KDL::JntArray q(nj);
1314  KDL::JntArray qd(nj);
1315  KDL::JntArray qdd(nj);
1316  KDL::JntArray tau(nj);
1317 
1318  // random
1319  q(0) = 0.2;
1320  q(1) = 0.6;
1321  q(2) = 1.;
1322  q(3) = 0.5;
1323  q(4) = -1.4;
1324  q(5) = 0.3;
1325  q(6) = -0.8;
1326 
1327  qd(0) = 1.;
1328  qd(1) = -2.;
1329  qd(2) = 3.;
1330  qd(3) = -4.;
1331  qd(4) = 5.;
1332  qd(5) = -6.;
1333  qd(6) = 7.;
1334 
1335  // actuator torques
1336  tau(0) = 50.;
1337  tau(1) = -20.;
1338  tau(2) = 10.;
1339  tau(3) = 40.;
1340  tau(4) = -60.;
1341  tau(5) = 15.;
1342  tau(6) = -10.;
1343 
1344  KDL::Vector f(10,-20,30) ;
1345  KDL::Vector n(3,-4,5) ;
1346  KDL::Wrench f_tool(f,n);
1347  // In local link coordinates
1348  KDL::Wrenches f_ext(ns);
1349  for(unsigned int i=0;i<ns;i++){
1350  SetToZero(f_ext[i]);
1351  }
1352  f_ext[ns-1]=f_tool;
1353 
1354  // Call FD function
1355  ret = FdSolver.CartToJnt(q, qd, tau, f_ext, qdd);
1356  if (ret < 0) std::cout << "KDL: forward dynamics ERROR: " << ret << std::endl;
1357  CPPUNIT_ASSERT(Equal(9.486, qdd(0), eps));
1358  CPPUNIT_ASSERT(Equal(1.830, qdd(1), eps));
1359  CPPUNIT_ASSERT(Equal(4.726, qdd(2), eps));
1360  CPPUNIT_ASSERT(Equal(11.665, qdd(3), eps));
1361  CPPUNIT_ASSERT(Equal(-50.108, qdd(4), eps));
1362  CPPUNIT_ASSERT(Equal(21.403, qdd(5), eps));
1363  CPPUNIT_ASSERT(Equal(-0.381, qdd(6), eps));
1364 
1365  // Check against ID solver for consistency
1366  KDL::ChainIdSolver_RNE IdSolver = KDL::ChainIdSolver_RNE(motomansia10dyn, gravity);
1367  KDL::JntArray torque(nj);
1368  IdSolver.CartToJnt(q, qd, qdd, f_ext, torque);
1369  for ( unsigned int i=0; i<nj; i++ )
1370  {
1371  CPPUNIT_ASSERT(Equal(torque(i), tau(i), eps));
1372  }
1373 
1374  return;
1375 }
1376 
1377 void SolverTest::LDLdecompTest()
1378 {
1379  std::cout<<"LDL Solver Test"<<std::endl;
1380  double eps=1.e-6;
1381 
1382  // Given A and b, solve Ax=b for x, where A is a symmetric real matrix
1383  // https://en.wikipedia.org/wiki/Cholesky_decomposition
1384  Eigen::MatrixXd A(3,3), Aout(3,3);
1385  Eigen::VectorXd b(3);
1386  Eigen::MatrixXd L(3,3), Lout(3,3);
1387  Eigen::VectorXd d(3), dout(3);
1388  Eigen::VectorXd x(3), xout(3);
1389  Eigen::VectorXd r(3); // temp variable used internally by ldl solver
1390  Eigen::MatrixXd Dout(3,3); // diagonal matrix
1391 
1392  // Given
1393  A << 4, 12, -16,
1394  12, 37, -43,
1395  -16, -43, 98;
1396  b << 28, 117, 98;
1397  // Results to verify
1398  L << 1, 0, 0,
1399  3, 1, 0,
1400  -4, 5, 1;
1401  d << 4, 1, 9;
1402  x << 3, 8, 5;
1403 
1404  ldl_solver_eigen(A, b, Lout, dout, r, xout);
1405 
1406  for(int i=0;i<3;i++){
1407  for(int j=0;j<3;j++){
1408  CPPUNIT_ASSERT(Equal(L(i,j), Lout(i,j), eps));
1409  }
1410  }
1411 
1412  Dout.setZero();
1413  for(int i=0;i<3;i++){
1414  Dout(i,i) = dout(i);
1415  }
1416 
1417  // Verify solution for x
1418  for(int i=0;i<3;i++){
1419  CPPUNIT_ASSERT(Equal(xout(i), x(i), eps));
1420  }
1421 
1422  // Test reconstruction of A from LDL^T decomposition
1423  Aout = Lout * Dout * Lout.transpose();
1424  for(int i=0;i<3;i++){
1425  for(int j=0;j<3;j++){
1426  CPPUNIT_ASSERT(Equal(A(i,j), Aout(i,j), eps));
1427  }
1428  }
1429 
1430  return;
1431 }
1432 
1433 void SolverTest::FdAndVereshchaginSolversConsistencyTest()
1434 {
1435  int ret;
1436  double eps=1.e-3;
1437  ChainFkSolverPos_recursive fksolverpos(kukaLWR);
1438  Frame end_effector_pose;
1439 
1444  std::cout << "KDL FD (inverse-inertia version) and Vereshchagin Solvers Consistency Test for KUKA LWR 4 robot" << std::endl;
1445 
1446  // ########################################################################################
1447  // Experiment (common state) setup
1448  unsigned int nj = kukaLWR.getNrOfJoints();
1449  unsigned int ns = kukaLWR.getNrOfSegments();
1450 
1451  // Necessary test for the used robot model: KDL's implementation of the Vereshchagin solver
1452  // can only work with the robot chains that have equal number of joints and segments
1453  CPPUNIT_ASSERT(Equal(nj, ns));
1454 
1455  // Joint position, velocity, acceleration and torques
1456  KDL::JntArray q(nj);
1457  KDL::JntArray qd(nj);
1458  KDL::JntArray qdd(nj);
1459  KDL::JntArray ff_tau(nj);
1460 
1461  // random input state
1462  q(0) = 1.0;
1463  q(1) = 0.0;
1464  q(2) = 0.0;
1465  q(3) = -1.57;
1466  q(4) = 0.0;
1467  q(5) = 1.57;
1468  q(6) = -0.8;
1469 
1470  qd(0) = 1.0;
1471  qd(1) = -2.0;
1472  qd(2) = 3.0;
1473  qd(3) = -4.0;
1474  qd(4) = 5.0;
1475  qd(5) = -6.0;
1476  qd(6) = 7.0;
1477 
1478  // actuator torques
1479  ff_tau(0) = 50.0;
1480  ff_tau(1) = -20.0;
1481  ff_tau(2) = 10.0;
1482  ff_tau(3) = 40.0;
1483  ff_tau(4) = -60.0;
1484  ff_tau(5) = 15.0;
1485  ff_tau(6) = -10.0;
1486 
1487  // External Wrench acting on the end-effector, expressed in local link coordinates
1488  KDL::Vector f(10.0, -20.0, 30.0);
1489  KDL::Vector n(3.0, -4.0, 5.0);
1490  KDL::Wrench f_tool(f, n);
1491 
1492  KDL::Wrenches f_ext(ns);
1493  for(unsigned int i=0 ;i<ns; i++)
1494  SetToZero(f_ext[i]);
1495  f_ext[ns - 1] = f_tool;
1496 
1497  // ########################################################################################
1498  // Forward Dynamics Solver (inverse-inertia version)
1499  Vector gravity(0.0, 0.0, -9.81); // base frame (Robot base mounted on an even surface)
1500  KDL::ChainFdSolver_RNE FdSolver = KDL::ChainFdSolver_RNE(kukaLWR, gravity);
1501 
1502  // Call FD function
1503  ret = FdSolver.CartToJnt(q, qd, ff_tau, f_ext, qdd);
1504  if (ret < 0)
1505  std::cout << "KDL: forward dynamics ERROR: " << ret << std::endl;
1506 
1507  // #########################################################################################
1508  // Vereshchagin Hybrid Dynamics solver
1509  // When the Cartesian Acceleration Constraints are deactivated, the computations perfomed
1510  // in the Vereshchagin solver are completely the same as the computations perfomed in
1511  // the well-known FD Articulated Body Algorithm (ABA) developed by Featherstone
1512 
1513  // Constraint Unit forces at the end-effector. Set to zero to deactivate all constraints
1514  int numberOfConstraints = 6;
1515  Jacobian alpha(numberOfConstraints);
1516  KDL::SetToZero(alpha);
1517 
1518  // Acceleration energy at the end-effector. Set to zero since all constraints are deactivated
1519  JntArray beta(numberOfConstraints); //set to zero
1520  KDL::SetToZero(beta);
1521 
1522  // Arm root acceleration (robot's base mounted on an even surface)
1523  // Note: Vereshchagin solver takes root acc. with opposite sign comparead to the above FD and RNE solvers
1524  Vector linearAcc(0.0, 0.0, 9.81); Vector angularAcc(0.0, 0.0, 0.0);
1525  Twist root_Acc(linearAcc, angularAcc);
1526 
1527  // Torques felt in robot's joints due to constrait forces acting on the end-effector
1528  JntArray constraint_tau(nj); // In this test, all elements of this array should result to zero
1529  JntArray q_dd_Ver(nj); // Resultant joint accelerations
1530 
1531  // External Wrench acting on the end-effector, this time expressed in base link coordinates
1532  // Vereshchagin solver expects that external wrenches are expressed w.r.t. robot's base frame
1533  fksolverpos.JntToCart(q, end_effector_pose, kukaLWR.getNrOfSegments());
1534  f_ext[ns - 1] = end_effector_pose.M * f_tool;
1535 
1536  ChainHdSolver_Vereshchagin constraintSolver(kukaLWR, root_Acc, numberOfConstraints);
1537  ret = constraintSolver.CartToJnt(q, qd, q_dd_Ver, alpha, beta, f_ext, ff_tau, constraint_tau);
1538  if (ret < 0)
1539  std::cout << "KDL: Vereshchagin solver ERROR: " << ret << std::endl;
1540 
1541  // ########################################################################################
1542  // Final comparison
1543  CPPUNIT_ASSERT(Equal(q_dd_Ver(0), qdd(0), eps));
1544  CPPUNIT_ASSERT(Equal(q_dd_Ver(1), qdd(1), eps));
1545  CPPUNIT_ASSERT(Equal(q_dd_Ver(2), qdd(2), eps));
1546  CPPUNIT_ASSERT(Equal(q_dd_Ver(3), qdd(3), eps));
1547  CPPUNIT_ASSERT(Equal(q_dd_Ver(4), qdd(4), eps));
1548  CPPUNIT_ASSERT(Equal(q_dd_Ver(5), qdd(5), eps));
1549  CPPUNIT_ASSERT(Equal(q_dd_Ver(6), qdd(6), eps));
1550 
1551  return;
1552 }
SolverTest::FkVelAndIkVelTest
void FkVelAndIkVelTest()
Definition: solvertest.cpp:415
KDL::ChainIkSolverVel_wdls::getLambdaScaled
double getLambdaScaled() const
Definition: chainiksolvervel_wdls.hpp:204
KDL::ChainIkSolverPos
This abstract class encapsulates the inverse position solver for a KDL::Chain.
Definition: chainiksolver.hpp:42
KDL::Twist::Zero
static Twist Zero()
Definition: frames.hpp:291
KDL::ChainIdSolver_RNE::updateInternalDataStructures
virtual void updateInternalDataStructures()
Definition: chainidsolver_recursive_newton_euler.cpp:34
framevel_io.hpp
KDL::ChainFkSolverPos_recursive::JntToCart
virtual int JntToCart(const JntArray &q_in, Frame &p_out, int segmentNr=-1)
Definition: chainfksolverpos_recursive.cpp:34
KDL::SolverI::getError
virtual int getError() const
Return the latest error.
Definition: solveri.hpp:119
KDL::ChainFkSolverPos_recursive
Definition: chainfksolverpos_recursive.hpp:36
SolverTest::tearDown
void tearDown()
Definition: solvertest.cpp:224
KDL::PI_2
const double PI_2
the value of pi/2
KDL::Segment
This class encapsulates a simple segment, that is a "rigid body" (i.e., a frame and a rigid body...
Definition: segment.hpp:46
KDL::ChainIkSolverVel_pinv_givens
Definition: chainiksolvervel_pinv_givens.hpp:24
KDL::ChainHdSolver_Vereshchagin::getTotalTorque
void getTotalTorque(JntArray &total_tau)
Definition: chainhdsolver_vereshchagin.cpp:362
KDL::ChainIkSolverPos::CartToJnt
virtual int CartToJnt(const JntArray &q_init, const Frame &p_in, JntArray &q_out)=0
KDL::ChainJntToJacDotSolver
Computes the Jacobian time derivative (Jdot) by calculating the partial derivatives regarding to a jo...
Definition: chainjnttojacdotsolver.hpp:48
SolverTest::IkVelSolverWDLSTest
void IkVelSolverWDLSTest()
Definition: solvertest.cpp:630
KDL::Chain
This class encapsulates a serial kinematic interconnection structure. It is built out of segments...
Definition: chain.hpp:35
KDL::ChainIkSolverVel_pinv_nso::CartToJnt
virtual int CartToJnt(const JntArray &q_in, const Twist &v_in, JntArray &qdot_out)
Definition: chainiksolvervel_pinv_nso.cpp:84
KDL::ChainIkSolverPos_NR::CartToJnt
virtual int CartToJnt(const JntArray &q_init, const Frame &p_in, JntArray &q_out)
Definition: chainiksolverpos_nr.cpp:42
KDL::PI
const double PI
the value of pi
KDL::SolverI::E_NOT_UP_TO_DATE
Chain size changed.
Definition: solveri.hpp:97
KDL::ChainHdSolver_Vereshchagin::updateInternalDataStructures
virtual void updateInternalDataStructures()
Definition: chainhdsolver_vereshchagin.cpp:50
KDL::diff
IMETHOD Vector diff(const Vector &p_w_a, const Vector &p_w_b, double dt=1)
Definition: frames.hpp:1130
KDL::MultiplyJacobian
void MultiplyJacobian(const Jacobian &jac, const JntArray &src, Twist &dest)
Definition: jntarray.cpp:102
KDL::SolverI::E_MAX_ITERATIONS_EXCEEDED
Maximum number of iterations exceeded.
Definition: solveri.hpp:101
KDL::JntArray
This class represents an fixed size array containing joint values of a KDL::Chain.
Definition: jntarray.hpp:69
SolverTest::FkVelAndJacTest
void FkVelAndJacTest()
Definition: solvertest.cpp:399
KDL::RigidBodyInertia
6D Inertia of a rigid body
Definition: rigidbodyinertia.hpp:37
KDL::ChainIkSolverVel_pinv::CartToJnt
virtual int CartToJnt(const JntArray &q_in, const Twist &v_in, JntArray &qdot_out)
Definition: chainiksolvervel_pinv.cpp:62
KDL::ldl_solver_eigen
int ldl_solver_eigen(const Eigen::MatrixXd &A, const Eigen::VectorXd &v, Eigen::MatrixXd &L, Eigen::VectorXd &D, Eigen::VectorXd &vtmp, Eigen::VectorXd &q)
Solves the system of equations Aq = v for q via LDL decomposition, where A is a square positive defin...
Definition: ldl_solver_eigen.cpp:26
KDL::SolverI::E_SIZE_MISMATCH
Input size does not match internal state.
Definition: solveri.hpp:99
SolverTest::FdAndVereshchaginSolversConsistencyTest
void FdAndVereshchaginSolversConsistencyTest()
Definition: solvertest.cpp:1433
frames_io.hpp
KDL::ChainIkSolverVel_pinv::getNrZeroSigmas
unsigned int getNrZeroSigmas() const
Definition: chainiksolvervel_pinv.hpp:87
KDL::ChainIkSolverVel_pinv_nso
Definition: chainiksolvervel_pinv_nso.hpp:46
SolverTest::FkVelAndJacLocal
void FkVelAndJacLocal(Chain &chain, ChainFkSolverVel &fksolvervel, ChainJntToJacSolver &jacsolver)
Definition: solvertest.cpp:726
KDL::ChainFdSolver_RNE
Recursive newton euler forward dynamics solver.
Definition: chainfdsolver_recursive_newton_euler.hpp:42
KDL::ChainIkSolverVel_wdls::E_CONVERGE_PINV_SINGULAR
static const int E_CONVERGE_PINV_SINGULAR
solution converged but (pseudo)inverse is singular
Definition: chainiksolvervel_wdls.hpp:67
KDL::ChainIkSolverVel::CartToJnt
virtual int CartToJnt(const JntArray &q_in, const Twist &v_in, JntArray &qdot_out)=0
KDL::ChainFdSolver_RNE::CartToJnt
int CartToJnt(const JntArray &q, const JntArray &q_dot, const JntArray &torques, const Wrenches &f_ext, JntArray &q_dotdot)
Definition: chainfdsolver_recursive_newton_euler.cpp:51
KDL::Frame::M
Rotation M
Orientation of the Frame.
Definition: frames.hpp:573
KDL::ChainIkSolverVel_wdls::CartToJnt
virtual int CartToJnt(const JntArray &q_in, const Twist &v_in, JntArray &qdot_out)
Definition: chainiksolvervel_wdls.cpp:116
KDL::ChainJntToJacSolver
Class to calculate the jacobian of a general KDL::Chain, it is used by other solvers.
Definition: chainjnttojacsolver.hpp:38
KDL::Segment::setInertia
void setInertia(const RigidBodyInertia &Iin)
Definition: segment.hpp:138
SolverTest::setUp
void setUp()
Definition: solvertest.cpp:12
KDL::ChainIkSolverVel_pinv::updateInternalDataStructures
virtual void updateInternalDataStructures()
Definition: chainiksolvervel_pinv.cpp:43
KDL::Twist
represents both translational and rotational velocities.
Definition: frames.hpp:720
KDL::ChainIkSolverPos_NR
Definition: chainiksolverpos_nr.hpp:38
KDL::SetToZero
IMETHOD void SetToZero(Vector &v)
Definition: frames.hpp:1069
KDL::ChainJntToJacSolver::updateInternalDataStructures
virtual void updateInternalDataStructures()
Definition: chainjnttojacsolver.cpp:31
KDL::Frame::DH_Craig1989
static Frame DH_Craig1989(double a, double alpha, double d, double theta)
Definition: frames.cpp:53
KDL::Equal
IMETHOD bool Equal(const FrameAcc &r1, const FrameAcc &r2, double eps=epsilon)
Definition: frameacc.hpp:394
KDL::ChainIkSolverVel_pinv::E_CONVERGE_PINV_SINGULAR
static const int E_CONVERGE_PINV_SINGULAR
solution converged but (pseudo)inverse is singular
Definition: chainiksolvervel_pinv.hpp:44
KDL::ChainFkSolverPos::JntToCart
virtual int JntToCart(const JntArray &q_in, Frame &p_out, int segmentNr=-1)=0
KDL::Vector
A concrete implementation of a 3 dimensional vector class.
Definition: frames.hpp:160
KDL::Rotation::RPY
static Rotation RPY(double roll, double pitch, double yaw)
Definition: frames.cpp:237
KDL::ChainIkSolverPos_LMA
Solver for the inverse position kinematics that uses Levenberg-Marquardt.
Definition: chainiksolverpos_lma.hpp:64
KDL::Frame::DH
static Frame DH(double a, double alpha, double d, double theta)
Definition: frames.cpp:70
SolverTest::FkVelVectTest
void FkVelVectTest()
Definition: solvertest.cpp:1107
KDL::ChainIkSolverPos_NR_JL
Definition: chainiksolverpos_nr_jl.hpp:40
KDL::ChainIkSolverVel_wdls::updateInternalDataStructures
virtual void updateInternalDataStructures()
Definition: chainiksolvervel_wdls.cpp:54
KDL::ChainHdSolver_Vereshchagin::getTransformedLinkAcceleration
void getTransformedLinkAcceleration(Twists &x_dotdot)
Definition: chainhdsolver_vereshchagin.cpp:353
SolverTest::FkPosAndJacLocal
void FkPosAndJacLocal(Chain &chain, ChainFkSolverPos &fksolverpos, ChainJntToJacSolver &jacsolver)
Definition: solvertest.cpp:691
KDL::ChainFkSolverVel_recursive
Definition: chainfksolvervel_recursive.hpp:37
KDL::SolverI::E_OUT_OF_RANGE
Requested index out of range.
Definition: solveri.hpp:103
KDL::ChainIkSolverVel_pinv
Definition: chainiksolvervel_pinv.hpp:40
KDL::ChainFkSolverVel::JntToCart
virtual int JntToCart(const JntArrayVel &q_in, FrameVel &out, int segmentNr=-1)=0
KDL::Jacobian::setColumn
void setColumn(unsigned int i, const Twist &t)
Definition: jacobian.cpp:150
KDL::ChainIdSolver_RNE
Recursive newton euler inverse dynamics solver.
Definition: chainidsolver_recursive_newton_euler.hpp:40
KDL::Rotation::Identity
static Rotation Identity()
Gives back an identity rotaton matrix.
Definition: frames.hpp:553
KDL::Frame::p
Vector p
origine of the Frame
Definition: frames.hpp:572
KDL::ChainIdSolver_RNE::CartToJnt
int CartToJnt(const JntArray &q, const JntArray &q_dot, const JntArray &q_dotdot, const Wrenches &f_ext, JntArray &torques)
Definition: chainidsolver_recursive_newton_euler.cpp:44
KDL::JntArrayVel::deriv
JntArray deriv() const
Definition: jntarrayvel.cpp:52
KDL::Chain::getNrOfJoints
unsigned int getNrOfJoints() const
Definition: chain.hpp:71
KDL::sqrt
INLINE Rall1d< T, V, S > sqrt(const Rall1d< T, V, S > &arg)
Definition: rall1d.h:369
SolverTest::IkSingularValueTest
void IkSingularValueTest()
Definition: solvertest.cpp:509
KDL::ChainIkSolverPos_LMA::updateInternalDataStructures
void updateInternalDataStructures()
Definition: chainiksolverpos_lma.cpp:122
KDL::ChainHdSolver_Vereshchagin::getContraintForceMagnitude
void getContraintForceMagnitude(Eigen::VectorXd &nu_)
Definition: chainhdsolver_vereshchagin.cpp:369
SolverTest::FdSolverConsistencyTest
void FdSolverConsistencyTest()
Definition: solvertest.cpp:1296
kinfam_io.hpp
SolverTest::VereshchaginTest
void VereshchaginTest()
Definition: solvertest.cpp:809
SolverTest::FkPosAndIkPosTest
void FkPosAndIkPosTest()
Definition: solvertest.cpp:458
KDL::ChainDynParam::JntToCoriolis
virtual int JntToCoriolis(const JntArray &q, const JntArray &q_dot, JntArray &coriolis)
Definition: chaindynparam.cpp:127
KDL::ChainHdSolver_Vereshchagin::CartToJnt
int CartToJnt(const JntArray &q, const JntArray &q_dot, JntArray &q_dotdot, const Jacobian &alfa, const JntArray &beta, const Wrenches &f_ext, const JntArray &ff_torques, JntArray &constraint_torques)
Definition: chainhdsolver_vereshchagin.cpp:57
KDL::JntArray::resize
void resize(unsigned int newSize)
Definition: jntarray.cpp:55
KDL::Wrenches
std::vector< Wrench > Wrenches
Definition: chainfdsolver.hpp:34
KDL::ChainIkSolverVel_wdls::setLambda
void setLambda(const double lambda)
Definition: chainiksolvervel_wdls.cpp:93
KDL::ChainFkSolverVel_recursive::JntToCart
virtual int JntToCart(const JntArrayVel &q_in, FrameVel &out, int segmentNr=-1)
Definition: chainfksolvervel_recursive.cpp:37
KDL::FrameVel::deriv
Twist deriv() const
Definition: framevel.hpp:237
KDL::ChainJntToJacDotSolver::JntToJacDot
virtual int JntToJacDot(const KDL::JntArrayVel &q_in, KDL::Twist &jac_dot_q_dot, int seg_nr=-1)
Computes .
Definition: chainjnttojacdotsolver.cpp:55
SolverTest::FdSolverDevelopmentTest
void FdSolverDevelopmentTest()
Definition: solvertest.cpp:1128
KDL::Frame::Identity
static Frame Identity()
Definition: frames.hpp:701
KDL::Frame
represents a frame transformation in 3D space (rotation + translation)
Definition: frames.hpp:570
KDL::ChainDynParam::updateInternalDataStructures
virtual void updateInternalDataStructures()
Definition: chaindynparam.cpp:46
KDL::Joint
This class encapsulates a simple joint, that is with one parameterized degree of freedom and with sca...
Definition: joint.hpp:45
KDL::ChainIkSolverVel_wdls::getNrZeroSigmas
unsigned int getNrZeroSigmas() const
Definition: chainiksolvervel_wdls.hpp:178
KDL::Wrench
represents both translational and rotational acceleration.
Definition: frames.hpp:878
KDL::ChainIkSolverPos_LMA::CartToJnt
virtual int CartToJnt(const KDL::JntArray &q_init, const KDL::Frame &T_base_goal, KDL::JntArray &q_out)
computes the inverse position kinematics.
Definition: chainiksolverpos_lma.cpp:187
CPPUNIT_TEST_SUITE_REGISTRATION
CPPUNIT_TEST_SUITE_REGISTRATION(SolverTest)
KDL::ChainIkSolverVel_pinv_nso::updateInternalDataStructures
virtual void updateInternalDataStructures()
Definition: chainiksolvervel_pinv_nso.cpp:65
SolverTest::UpdateChainTest
void UpdateChainTest()
Definition: solvertest.cpp:233
KDL::ChainIkSolverVel_wdls
Definition: chainiksolvervel_wdls.hpp:63
SolverTest::FkVelAndIkVelLocal
void FkVelAndIkVelLocal(Chain &chain, ChainFkSolverVel &fksolvervel, ChainIkSolverVel &iksolvervel)
Definition: solvertest.cpp:748
SolverTest::FkPosVectTest
void FkPosVectTest()
Definition: solvertest.cpp:1087
SolverTest::FkPosAndJacTest
void FkPosAndJacTest()
Definition: solvertest.cpp:383
KDL::ChainIkSolverVel_wdls::getSigmaMin
double getSigmaMin() const
Definition: chainiksolvervel_wdls.hpp:183
KDL::ChainJntToJacSolver::JntToJac
virtual int JntToJac(const JntArray &q_in, Jacobian &jac, int seg_nr=-1)
Definition: chainjnttojacsolver.cpp:48
KDL::ChainDynParam::JntToMass
virtual int JntToMass(const JntArray &q, JntSpaceInertiaMatrix &H)
Definition: chaindynparam.cpp:60
SolverTest::FkPosAndIkPosLocal
void FkPosAndIkPosLocal(Chain &chain, ChainFkSolverPos &fksolverpos, ChainIkSolverPos &iksolverpos)
Definition: solvertest.cpp:781
KDL::ChainIkSolverPos_NR_JL::updateInternalDataStructures
virtual void updateInternalDataStructures()
Definition: chainiksolverpos_nr_jl.cpp:53
KDL::random
IMETHOD void random(Vector &a)
addDelta operator for displacement rotational velocity.
Definition: frames.hpp:1215
KDL::ChainIkSolverVel_pinv_givens::updateInternalDataStructures
virtual void updateInternalDataStructures()
Definition: chainiksolvervel_pinv_givens.cpp:48
KDL::ChainFkSolverVel
This abstract class encapsulates a solver for the forward velocity kinematics for a KDL::Chain...
Definition: chainfksolver.hpp:74
KDL::ChainHdSolver_Vereshchagin
Abstract: Acceleration constrained hybrid dynamics calculations for a chain, based on Vereshchagin 19...
Definition: chainhdsolver_vereshchagin.hpp:366
KDL::Vector::Zero
static Vector Zero()
Definition: frames.hpp:139
KDL::ChainIkSolverPos_NR_JL::CartToJnt
virtual int CartToJnt(const JntArray &q_init, const Frame &p_in, JntArray &q_out)
Definition: chainiksolverpos_nr_jl.cpp:62
KDL::ChainDynParam::JntToGravity
virtual int JntToGravity(const JntArray &q, JntArray &gravity)
Definition: chaindynparam.cpp:139
SolverTest::LDLdecompTest
void LDLdecompTest()
Definition: solvertest.cpp:1377
KDL::ChainIkSolverPos_NR::updateInternalDataStructures
virtual void updateInternalDataStructures()
Definition: chainiksolverpos_nr.cpp:35
KDL::ChainIkSolverVel_pinv_givens::CartToJnt
virtual int CartToJnt(const JntArray &q_in, const Twist &v_in, JntArray &qdot_out)
Definition: chainiksolvervel_pinv_givens.cpp:70
KDL::ChainIkSolverVel
This abstract class encapsulates the inverse velocity solver for a KDL::Chain.
Definition: chainiksolver.hpp:66
KDL::ChainFkSolverPos
This abstract class encapsulates a solver for the forward position kinematics for a KDL::Chain...
Definition: chainfksolver.hpp:41

orocos_kdl
Author(s):
autogenerated on Fri Mar 12 2021 03:05:44