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 
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, M_PI_2, 0.0, 0.0)));
110  motomansia10.addSegment(Segment(Joint(Joint::RotZ),
111  Frame::DH_Craig1989(0.0, -M_PI_2, 0.36, 0.0)));
112  motomansia10.addSegment(Segment(Joint(Joint::RotZ),
113  Frame::DH_Craig1989(0.0, M_PI_2, 0.0, 0.0)));
114  motomansia10.addSegment(Segment(Joint(Joint::RotZ),
115  Frame::DH_Craig1989(0.0, -M_PI_2, 0.36, 0.0)));
116  motomansia10.addSegment(Segment(Joint(Joint::RotZ),
117  Frame::DH_Craig1989(0.0, M_PI_2, 0.0, 0.0)));
118  motomansia10.addSegment(Segment(Joint(Joint::RotZ),
119  Frame::DH_Craig1989(0.0, -M_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, M_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, -M_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, M_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, -M_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, M_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, -M_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 }
174 
175 void SolverTest::tearDown()
176 {
177 // delete fksolverpos;
178 // delete jacsolver;
179 // delete fksolvervel;
180 // delete iksolvervel;
181 // delete iksolverpos;
182 }
183 
184 void SolverTest::UpdateChainTest()
185 {
186  ChainFkSolverPos_recursive fksolverpos(chain2);
187  ChainFkSolverVel_recursive fksolvervel(chain2);
188  ChainJntToJacSolver jacsolver1(chain2);
189  ChainJntToJacDotSolver jacdotsolver1(chain2);
190  ChainIkSolverVel_pinv iksolver2(chain2);
191  ChainIkSolverVel_pinv_givens iksolver_pinv_givens2(chain2);
192  ChainIkSolverVel_pinv_nso iksolver_pinv_nso(chain2);
193  ChainIkSolverVel_wdls iksolver_wdls(chain2,1e-6,30);
194  ChainIkSolverPos_NR iksolverpos(chain2,fksolverpos,iksolver2);
195  ChainIkSolverPos_NR_JL iksolverpos2(chain2,fksolverpos,iksolver2);
196  ChainIkSolverPos_LMA iksolverpos3(chain2);
197  ChainDynParam dynparam(chain2, Vector::Zero());
198  ChainIdSolver_RNE idsolver1(chain2, Vector::Zero());
199  unsigned int nr_of_constraints = 4;
200  ChainIdSolver_Vereshchagin idsolver2(chain2,Twist::Zero(),4);
201 
202  JntArray q_in(chain2.getNrOfJoints());
203  JntArray q_in2(chain2.getNrOfJoints());
204  JntArrayVel q_in3(chain2.getNrOfJoints());
205  for(unsigned int i=0; i<chain2.getNrOfJoints(); i++)
206  {
207  random(q_in(i));
208  random(q_in2(i));
209  }
210  JntArray q_out(chain2.getNrOfJoints());
211  JntArray q_out2(chain2.getNrOfJoints());
212  Jacobian jac(chain2.getNrOfJoints());
213  Frame T;
214  Twist t;
215  FrameVel T2;
216  Wrenches wrenches(chain2.getNrOfSegments());
217  JntSpaceInertiaMatrix m(chain2.getNrOfJoints());
218 
219  Jacobian alpha(nr_of_constraints - 1);
220  JntArray beta(nr_of_constraints - 1);
221  CPPUNIT_ASSERT_EQUAL((int)SolverI::E_OUT_OF_RANGE,fksolverpos.JntToCart(q_in, T, chain2.getNrOfSegments()+1));
222  CPPUNIT_ASSERT_EQUAL((int)SolverI::E_OUT_OF_RANGE,fksolvervel.JntToCart(q_in3, T2, chain2.getNrOfSegments()+1));
223  CPPUNIT_ASSERT_EQUAL((int)SolverI::E_OUT_OF_RANGE, jacsolver1.JntToJac(q_in, jac, chain2.getNrOfSegments()+1));
224  CPPUNIT_ASSERT_EQUAL((int)SolverI::E_OUT_OF_RANGE, jacdotsolver1.JntToJacDot(q_in3, t, chain2.getNrOfSegments()+1));
225  CPPUNIT_ASSERT_EQUAL((int)SolverI::E_OUT_OF_RANGE, jacdotsolver1.JntToJacDot(q_in3, jac, chain2.getNrOfSegments()+1));
226  chain2.addSegment(Segment("Segment 6", Joint("Joint 6", Joint::RotX),
227  Frame(Vector(0.0,0.0,0.1))));
228  CPPUNIT_ASSERT_EQUAL((int)SolverI::E_NOT_UP_TO_DATE, jacsolver1.JntToJac(q_in, jac, chain2.getNrOfSegments()));
229  CPPUNIT_ASSERT_EQUAL((int)SolverI::E_NOT_UP_TO_DATE, jacdotsolver1.JntToJacDot(q_in3, jac, chain2.getNrOfSegments()));
230  CPPUNIT_ASSERT_EQUAL((int)SolverI::E_NOT_UP_TO_DATE, jacdotsolver1.JntToJacDot(q_in3, t, chain2.getNrOfSegments()));
231  CPPUNIT_ASSERT_EQUAL((int)SolverI::E_NOT_UP_TO_DATE, iksolver2.CartToJnt(q_in,t,q_out));
232  CPPUNIT_ASSERT_EQUAL((int)SolverI::E_NOT_UP_TO_DATE, iksolver_pinv_givens2.CartToJnt(q_in,t,q_out));
233  CPPUNIT_ASSERT_EQUAL((int)SolverI::E_NOT_UP_TO_DATE, iksolver_pinv_nso.CartToJnt(q_in,t,q_out));
234  CPPUNIT_ASSERT_EQUAL((int)SolverI::E_NOT_UP_TO_DATE, iksolver_wdls.CartToJnt(q_in,t,q_out));
235  CPPUNIT_ASSERT_EQUAL((int)SolverI::E_NOT_UP_TO_DATE, iksolverpos.CartToJnt(q_in,T,q_out));
236  CPPUNIT_ASSERT_EQUAL((int)SolverI::E_NOT_UP_TO_DATE, iksolverpos2.CartToJnt(q_in,T,q_out));
237  CPPUNIT_ASSERT_EQUAL((int)SolverI::E_NOT_UP_TO_DATE, iksolverpos3.CartToJnt(q_in,T,q_out));
238  CPPUNIT_ASSERT_EQUAL((int)SolverI::E_NOT_UP_TO_DATE, idsolver1.CartToJnt(q_in,q_in2,q_out,wrenches,q_out2));
239  CPPUNIT_ASSERT_EQUAL((int)SolverI::E_NOT_UP_TO_DATE, idsolver2.CartToJnt(q_in,q_in2,q_out,alpha,beta,wrenches,q_out2));
240  CPPUNIT_ASSERT_EQUAL((int)SolverI::E_NOT_UP_TO_DATE, dynparam.JntToCoriolis(q_in, q_in2, q_out));
241  CPPUNIT_ASSERT_EQUAL((int)SolverI::E_NOT_UP_TO_DATE, dynparam.JntToGravity(q_in, q_out));
242  CPPUNIT_ASSERT_EQUAL((int)SolverI::E_NOT_UP_TO_DATE, dynparam.JntToMass(q_in, m));
243 
244  jacsolver1.updateInternalDataStructures();
245  jacdotsolver1.updateInternalDataStructures();
246  iksolver2.updateInternalDataStructures();
247  iksolver_pinv_givens2.updateInternalDataStructures();
248  iksolver_pinv_nso.updateInternalDataStructures();
249  iksolver_wdls.updateInternalDataStructures();
250  iksolverpos.updateInternalDataStructures();
251  iksolverpos2.updateInternalDataStructures();
252  iksolverpos3.updateInternalDataStructures();
253  idsolver1.updateInternalDataStructures();
254  idsolver2.updateInternalDataStructures();
255  dynparam.updateInternalDataStructures();
256 
257  CPPUNIT_ASSERT_EQUAL((int)SolverI::E_SIZE_MISMATCH,fksolverpos.JntToCart(q_in, T, chain2.getNrOfSegments()));
258  CPPUNIT_ASSERT_EQUAL((int)SolverI::E_SIZE_MISMATCH,fksolvervel.JntToCart(q_in3, T2, chain2.getNrOfSegments()));
259  CPPUNIT_ASSERT_EQUAL((int)SolverI::E_SIZE_MISMATCH, jacsolver1.JntToJac(q_in, jac, chain2.getNrOfSegments()));
260  CPPUNIT_ASSERT_EQUAL((int)SolverI::E_SIZE_MISMATCH, jacdotsolver1.JntToJacDot(q_in3, t, chain2.getNrOfSegments()));
261  CPPUNIT_ASSERT_EQUAL((int)SolverI::E_SIZE_MISMATCH, jacdotsolver1.JntToJacDot(q_in3, jac, chain2.getNrOfSegments()));
262  CPPUNIT_ASSERT_EQUAL((int)SolverI::E_SIZE_MISMATCH, iksolver2.CartToJnt(q_in,t,q_out));
263  CPPUNIT_ASSERT_EQUAL((int)SolverI::E_SIZE_MISMATCH, iksolver_pinv_givens2.CartToJnt(q_in,t,q_out));
264  CPPUNIT_ASSERT_EQUAL((int)SolverI::E_SIZE_MISMATCH, iksolver_pinv_nso.CartToJnt(q_in,t,q_out));
265  CPPUNIT_ASSERT_EQUAL((int)SolverI::E_SIZE_MISMATCH, iksolver_wdls.CartToJnt(q_in,t,q_out));
266  CPPUNIT_ASSERT_EQUAL((int)SolverI::E_SIZE_MISMATCH, iksolverpos.CartToJnt(q_in,T,q_out));
267  CPPUNIT_ASSERT_EQUAL((int)SolverI::E_SIZE_MISMATCH, iksolverpos2.CartToJnt(q_in,T,q_out));
268  CPPUNIT_ASSERT_EQUAL((int)SolverI::E_SIZE_MISMATCH, iksolverpos3.CartToJnt(q_in,T,q_out));
269  CPPUNIT_ASSERT_EQUAL((int)SolverI::E_SIZE_MISMATCH, idsolver1.CartToJnt(q_in,q_in2,q_out,wrenches,q_out2));
270  CPPUNIT_ASSERT_EQUAL((int)SolverI::E_SIZE_MISMATCH, idsolver2.CartToJnt(q_in,q_in2,q_out,alpha,beta,wrenches,q_out2));
271  CPPUNIT_ASSERT_EQUAL((int)SolverI::E_SIZE_MISMATCH, dynparam.JntToCoriolis(q_in, q_in2, q_out));
272  CPPUNIT_ASSERT_EQUAL((int)SolverI::E_SIZE_MISMATCH, dynparam.JntToGravity(q_in, q_out));
273  CPPUNIT_ASSERT_EQUAL((int)SolverI::E_SIZE_MISMATCH, dynparam.JntToMass(q_in, m));
274 
275  q_in.resize(chain2.getNrOfJoints());
276  CPPUNIT_ASSERT_EQUAL((int)SolverI::E_SIZE_MISMATCH, jacsolver1.JntToJac(q_in, jac, chain2.getNrOfSegments()));
277  CPPUNIT_ASSERT_EQUAL((int)SolverI::E_SIZE_MISMATCH, iksolver2.CartToJnt(q_in,t,q_out));
278  CPPUNIT_ASSERT_EQUAL((int)SolverI::E_SIZE_MISMATCH, iksolver_pinv_givens2.CartToJnt(q_in,t,q_out));
279  CPPUNIT_ASSERT_EQUAL((int)SolverI::E_SIZE_MISMATCH, iksolver_pinv_nso.CartToJnt(q_in,t,q_out));
280  CPPUNIT_ASSERT_EQUAL((int)SolverI::E_SIZE_MISMATCH, iksolver_wdls.CartToJnt(q_in,t,q_out));
281  CPPUNIT_ASSERT_EQUAL((int)SolverI::E_SIZE_MISMATCH, iksolverpos.CartToJnt(q_in,T,q_out));
282  CPPUNIT_ASSERT_EQUAL((int)SolverI::E_SIZE_MISMATCH, iksolverpos2.CartToJnt(q_in,T,q_out));
283  CPPUNIT_ASSERT_EQUAL((int)SolverI::E_SIZE_MISMATCH, iksolverpos3.CartToJnt(q_in,T,q_out));
284  CPPUNIT_ASSERT_EQUAL((int)SolverI::E_SIZE_MISMATCH, iksolverpos2.CartToJnt(q_in,T,q_out));
285  CPPUNIT_ASSERT_EQUAL((int)SolverI::E_SIZE_MISMATCH, iksolverpos3.CartToJnt(q_in,T,q_out));
286  CPPUNIT_ASSERT_EQUAL((int)SolverI::E_SIZE_MISMATCH, idsolver1.CartToJnt(q_in,q_in2,q_out,wrenches,q_out2));
287  CPPUNIT_ASSERT_EQUAL((int)SolverI::E_SIZE_MISMATCH, idsolver2.CartToJnt(q_in,q_in2,q_out,alpha,beta,wrenches,q_out2));
288  CPPUNIT_ASSERT_EQUAL((int)SolverI::E_SIZE_MISMATCH, dynparam.JntToCoriolis(q_in, q_in2, q_out));
289  CPPUNIT_ASSERT_EQUAL((int)SolverI::E_SIZE_MISMATCH, dynparam.JntToGravity(q_in, q_out));
290  CPPUNIT_ASSERT_EQUAL((int)SolverI::E_SIZE_MISMATCH, dynparam.JntToMass(q_in, m));
291  q_in2.resize(chain2.getNrOfJoints());
292  CPPUNIT_ASSERT_EQUAL((int)SolverI::E_SIZE_MISMATCH, dynparam.JntToCoriolis(q_in, q_in2, q_out));
293  CPPUNIT_ASSERT_EQUAL((int)SolverI::E_SIZE_MISMATCH, idsolver1.CartToJnt(q_in,q_in2,q_out,wrenches,q_out2));
294  CPPUNIT_ASSERT_EQUAL((int)SolverI::E_SIZE_MISMATCH, idsolver2.CartToJnt(q_in,q_in2,q_out,alpha,beta,wrenches,q_out2));
295  wrenches.resize(chain2.getNrOfSegments());
296  CPPUNIT_ASSERT_EQUAL((int)SolverI::E_SIZE_MISMATCH, idsolver1.CartToJnt(q_in,q_in2,q_out,wrenches,q_out2));
297  CPPUNIT_ASSERT_EQUAL((int)SolverI::E_SIZE_MISMATCH, idsolver2.CartToJnt(q_in,q_in2,q_out,alpha,beta,wrenches,q_out2));
298  q_out2.resize(chain2.getNrOfSegments());
299  CPPUNIT_ASSERT_EQUAL((int)SolverI::E_SIZE_MISMATCH, idsolver1.CartToJnt(q_in,q_in2,q_out,wrenches,q_out2));
300  CPPUNIT_ASSERT_EQUAL((int)SolverI::E_SIZE_MISMATCH, idsolver2.CartToJnt(q_in,q_in2,q_out,alpha,beta,wrenches,q_out2));
301  alpha.resize(nr_of_constraints);
302  CPPUNIT_ASSERT_EQUAL((int)SolverI::E_SIZE_MISMATCH, idsolver2.CartToJnt(q_in,q_in2,q_out,alpha,beta,wrenches,q_out2));
303  beta.resize(nr_of_constraints);
304  CPPUNIT_ASSERT_EQUAL((int)SolverI::E_SIZE_MISMATCH, idsolver2.CartToJnt(q_in,q_in2,q_out,alpha,beta,wrenches,q_out2));
305  jac.resize(chain2.getNrOfJoints());
306  CPPUNIT_ASSERT_EQUAL((int)SolverI::E_SIZE_MISMATCH, jacdotsolver1.JntToJacDot(q_in3, jac, chain2.getNrOfSegments()));
307  q_out.resize(chain2.getNrOfJoints());
308  q_in3.resize(chain2.getNrOfJoints());
309  m.resize(chain2.getNrOfJoints());
310  CPPUNIT_ASSERT_EQUAL((int)SolverI::E_NOERROR,fksolverpos.JntToCart(q_in, T, chain2.getNrOfSegments()));
311  CPPUNIT_ASSERT_EQUAL((int)SolverI::E_NOERROR,fksolvervel.JntToCart(q_in3, T2, chain2.getNrOfSegments()));
312  CPPUNIT_ASSERT_EQUAL((int)SolverI::E_NOERROR, jacsolver1.JntToJac(q_in, jac, chain2.getNrOfSegments()));
313  CPPUNIT_ASSERT_EQUAL((int)SolverI::E_NOERROR, jacdotsolver1.JntToJacDot(q_in3, jac, chain2.getNrOfSegments()));
314  CPPUNIT_ASSERT_EQUAL((int)SolverI::E_NOERROR, jacdotsolver1.JntToJacDot(q_in3, t, chain2.getNrOfSegments()));
315  CPPUNIT_ASSERT((int)SolverI::E_NOERROR <= iksolver2.CartToJnt(q_in,t,q_out));
316  CPPUNIT_ASSERT((int)SolverI::E_NOERROR <= iksolver_pinv_givens2.CartToJnt(q_in,t,q_out));
317  CPPUNIT_ASSERT((int)SolverI::E_NOERROR <= iksolver_pinv_nso.CartToJnt(q_in,t,q_out));
318  CPPUNIT_ASSERT((int)SolverI::E_NOERROR <= iksolver_wdls.CartToJnt(q_in,t,q_out));
319  CPPUNIT_ASSERT((int)SolverI::E_NOERROR <= iksolverpos.CartToJnt(q_in,T,q_out));
320  CPPUNIT_ASSERT((int)SolverI::E_NOERROR <= iksolverpos2.CartToJnt(q_in,T,q_out));
321  CPPUNIT_ASSERT((int)SolverI::E_NOERROR <= iksolverpos3.CartToJnt(q_in,T,q_out));
322  CPPUNIT_ASSERT((int)SolverI::E_NOERROR <= iksolverpos2.CartToJnt(q_in,T,q_out));
323  CPPUNIT_ASSERT((int)SolverI::E_NOERROR <= iksolverpos3.CartToJnt(q_in,T,q_out));
324  CPPUNIT_ASSERT((int)SolverI::E_NOERROR <= idsolver1.CartToJnt(q_in,q_in2,q_out,wrenches,q_out2));
325  CPPUNIT_ASSERT((int)SolverI::E_NOERROR <= idsolver2.CartToJnt(q_in,q_in2,q_out,alpha,beta,wrenches,q_out2));
326  CPPUNIT_ASSERT((int)SolverI::E_NOERROR <= dynparam.JntToCoriolis(q_in, q_in2, q_out));
327  CPPUNIT_ASSERT((int)SolverI::E_NOERROR <= dynparam.JntToGravity(q_in, q_out));
328  CPPUNIT_ASSERT((int)SolverI::E_NOERROR <= dynparam.JntToMass(q_in, m));
329 }
330 void SolverTest::FkPosAndJacTest()
331 {
332  ChainFkSolverPos_recursive fksolver1(chain1);
333  ChainJntToJacSolver jacsolver1(chain1);
334  FkPosAndJacLocal(chain1,fksolver1,jacsolver1);
335  ChainFkSolverPos_recursive fksolver2(chain2);
336  ChainJntToJacSolver jacsolver2(chain2);
337  FkPosAndJacLocal(chain2,fksolver2,jacsolver2);
338  ChainFkSolverPos_recursive fksolver3(chain3);
339  ChainJntToJacSolver jacsolver3(chain3);
340  FkPosAndJacLocal(chain3,fksolver3,jacsolver3);
341  ChainFkSolverPos_recursive fksolver4(chain4);
342  ChainJntToJacSolver jacsolver4(chain4);
343  FkPosAndJacLocal(chain4,fksolver4,jacsolver4);
344 }
345 
346 void SolverTest::FkVelAndJacTest()
347 {
348  ChainFkSolverVel_recursive fksolver1(chain1);
349  ChainJntToJacSolver jacsolver1(chain1);
350  FkVelAndJacLocal(chain1,fksolver1,jacsolver1);
351  ChainFkSolverVel_recursive fksolver2(chain2);
352  ChainJntToJacSolver jacsolver2(chain2);
353  FkVelAndJacLocal(chain2,fksolver2,jacsolver2);
354  ChainFkSolverVel_recursive fksolver3(chain3);
355  ChainJntToJacSolver jacsolver3(chain3);
356  FkVelAndJacLocal(chain3,fksolver3,jacsolver3);
357  ChainFkSolverVel_recursive fksolver4(chain4);
358  ChainJntToJacSolver jacsolver4(chain4);
359  FkVelAndJacLocal(chain4,fksolver4,jacsolver4);
360 }
361 
362 void SolverTest::FkVelAndIkVelTest()
363 {
364  //Chain1
365  std::cout<<"square problem"<<std::endl;
366  ChainFkSolverVel_recursive fksolver1(chain1);
367  ChainIkSolverVel_pinv iksolver1(chain1);
368  ChainIkSolverVel_pinv_givens iksolver_pinv_givens1(chain1);
369  std::cout<<"KDL-SVD-HouseHolder"<<std::endl;
370  FkVelAndIkVelLocal(chain1,fksolver1,iksolver1);
371  std::cout<<"KDL-SVD-Givens"<<std::endl;
372  FkVelAndIkVelLocal(chain1,fksolver1,iksolver_pinv_givens1);
373 
374  //Chain2
375  std::cout<<"underdetermined problem"<<std::endl;
376  ChainFkSolverVel_recursive fksolver2(chain2);
377  ChainIkSolverVel_pinv iksolver2(chain2);
378  ChainIkSolverVel_pinv_givens iksolver_pinv_givens2(chain2);
379  std::cout<<"KDL-SVD-HouseHolder"<<std::endl;
380  FkVelAndIkVelLocal(chain2,fksolver2,iksolver2);
381  std::cout<<"KDL-SVD-Givens"<<std::endl;
382  FkVelAndIkVelLocal(chain2,fksolver2,iksolver_pinv_givens2);
383 
384  //Chain3
385  std::cout<<"overdetermined problem"<<std::endl;
386  ChainFkSolverVel_recursive fksolver3(chain3);
387  ChainIkSolverVel_pinv iksolver3(chain3);
388  ChainIkSolverVel_pinv_givens iksolver_pinv_givens3(chain3);
389  std::cout<<"KDL-SVD-HouseHolder"<<std::endl;
390  FkVelAndIkVelLocal(chain3,fksolver3,iksolver3);
391  std::cout<<"KDL-SVD-Givens"<<std::endl;
392  FkVelAndIkVelLocal(chain3,fksolver3,iksolver_pinv_givens3);
393 
394  //Chain4
395  std::cout<<"overdetermined problem"<<std::endl;
396  ChainFkSolverVel_recursive fksolver4(chain4);
397  ChainIkSolverVel_pinv iksolver4(chain4);
398  ChainIkSolverVel_pinv_givens iksolver_pinv_givens4(chain4);
399  std::cout<<"KDL-SVD-HouseHolder"<<std::endl;
400  FkVelAndIkVelLocal(chain4,fksolver4,iksolver4);
401  std::cout<<"KDL-SVD-Givens"<<std::endl;
402  FkVelAndIkVelLocal(chain4,fksolver4,iksolver_pinv_givens4);
403 }
404 
405 void SolverTest::FkPosAndIkPosTest()
406 {
407  std::cout<<"square problem"<<std::endl;
408  ChainFkSolverPos_recursive fksolver1(chain1);
409  ChainIkSolverVel_pinv iksolver1v(chain1);
410  ChainIkSolverVel_pinv_givens iksolverv_pinv_givens1(chain1);
411  ChainIkSolverPos_NR iksolver1(chain1,fksolver1,iksolver1v);
412  ChainIkSolverPos_NR iksolver1_givens(chain1,fksolver1,iksolverv_pinv_givens1,1000);
413 
414  std::cout<<"KDL-SVD-HouseHolder"<<std::endl;
415  FkPosAndIkPosLocal(chain1,fksolver1,iksolver1);
416  std::cout<<"KDL-SVD-Givens"<<std::endl;
417  FkPosAndIkPosLocal(chain1,fksolver1,iksolver1_givens);
418 
419  std::cout<<"underdetermined problem"<<std::endl;
420  ChainFkSolverPos_recursive fksolver2(chain2);
421  ChainIkSolverVel_pinv iksolverv2(chain2);
422  ChainIkSolverVel_pinv_givens iksolverv_pinv_givens2(chain2);
423  ChainIkSolverPos_NR iksolver2(chain2,fksolver2,iksolverv2);
424  ChainIkSolverPos_NR iksolver2_givens(chain2,fksolver2,iksolverv_pinv_givens2,1000);
425 
426  std::cout<<"KDL-SVD-HouseHolder"<<std::endl;
427  FkPosAndIkPosLocal(chain2,fksolver2,iksolver2);
428  std::cout<<"KDL-SVD-Givens"<<std::endl;
429  FkPosAndIkPosLocal(chain2,fksolver2,iksolver2_givens);
430 
431  std::cout<<"overdetermined problem"<<std::endl;
432  ChainFkSolverPos_recursive fksolver3(chain3);
433  ChainIkSolverVel_pinv iksolverv3(chain3);
434  ChainIkSolverVel_pinv_givens iksolverv_pinv_givens3(chain3);
435  ChainIkSolverPos_NR iksolver3(chain3,fksolver3,iksolverv3);
436  ChainIkSolverPos_NR iksolver3_givens(chain3,fksolver3,iksolverv_pinv_givens3,1000);
437 
438  std::cout<<"KDL-SVD-HouseHolder"<<std::endl;
439  FkPosAndIkPosLocal(chain3,fksolver3,iksolver3);
440  std::cout<<"KDL-SVD-Givens"<<std::endl;
441  FkPosAndIkPosLocal(chain3,fksolver3,iksolver3_givens);
442 
443  std::cout<<"underdetermined problem with WGs segment constructor"<<std::endl;
444  ChainFkSolverPos_recursive fksolver4(chain4);
445  ChainIkSolverVel_pinv iksolverv4(chain4);
446  ChainIkSolverVel_pinv_givens iksolverv_pinv_givens4(chain4);
447  ChainIkSolverPos_NR iksolver4(chain4,fksolver4,iksolverv4,1000);
448  ChainIkSolverPos_NR iksolver4_givens(chain4,fksolver4,iksolverv_pinv_givens4,1000);
449 
450  std::cout<<"KDL-SVD-HouseHolder"<<std::endl;
451  FkPosAndIkPosLocal(chain4,fksolver4,iksolver4);
452  std::cout<<"KDL-SVD-Givens"<<std::endl;
453  FkPosAndIkPosLocal(chain4,fksolver4,iksolver4_givens);
454 }
455 
456 void SolverTest::IkSingularValueTest()
457 {
458  unsigned int maxiter = 30;
459  double eps = 1e-6 ;
460  int maxiter_vel = 30;
461  double eps_vel = 0.1 ;
462  Frame F, dF, F_des,F_solved;
463  KDL::Twist F_error ;
464 
465  std::cout<<"KDL-IK Solver Tests for Near Zero SVs"<<std::endl;
466 
467  ChainFkSolverPos_recursive fksolver(motomansia10);
468  ChainIkSolverVel_pinv ikvelsolver1(motomansia10,eps_vel,maxiter_vel);
469  ChainIkSolverPos_NR iksolver1(motomansia10,fksolver,ikvelsolver1,maxiter,eps);
470  unsigned int nj = motomansia10.getNrOfJoints();
471  JntArray q(nj), q_solved(nj) ;
472 
473 
474  std::cout<<"norminal case: convergence"<<std::endl;
475 
476  q(0) = 0. ;
477  q(1) = 0.5 ;
478  q(2) = 0.4 ;
479  q(3) = -M_PI_2 ;
480  q(4) = 0. ;
481  q(5) = 0. ;
482  q(6) = 0. ;
483 
484  dF.M = KDL::Rotation::RPY(0.1, 0.1, 0.1) ;
485  dF.p = KDL::Vector(0.01,0.01,0.01) ;
486 
487  CPPUNIT_ASSERT_EQUAL(0, fksolver.JntToCart(q,F));
488  F_des = F * dF ;
489 
490  CPPUNIT_ASSERT_EQUAL((int)SolverI::E_NOERROR,
491  iksolver1.CartToJnt(q, F_des, q_solved)); // converges
492  CPPUNIT_ASSERT_EQUAL((int)SolverI::E_NOERROR,
493  ikvelsolver1.getError());
494  CPPUNIT_ASSERT_EQUAL((unsigned int)1,
495  ikvelsolver1.getNrZeroSigmas()) ; // 1 singular value
496 
497  CPPUNIT_ASSERT_EQUAL(0, fksolver.JntToCart(q_solved,F_solved));
498  F_error = KDL::diff(F_solved,F_des);
499  CPPUNIT_ASSERT_EQUAL(F_des,F_solved);
500 
501  std::cout<<"nonconvergence: pseudoinverse singular"<<std::endl;
502 
503  q(0) = 0. ;
504  q(1) = 0.2 ;
505  q(2) = 0.4 ;
506  q(3) = -M_PI_2 ;
507  q(4) = 0. ;
508  q(5) = 0. ;
509  q(6) = 0. ;
510 
511  dF.M = KDL::Rotation::RPY(0.1, 0.1, 0.1) ;
512  dF.p = KDL::Vector(0.01,0.01,0.01) ;
513 
514  CPPUNIT_ASSERT_EQUAL(0, fksolver.JntToCart(q,F));
515  F_des = F * dF ;
516 
517  CPPUNIT_ASSERT_EQUAL((int)SolverI::E_MAX_ITERATIONS_EXCEEDED,
518  iksolver1.CartToJnt(q,F_des,q_solved)); // no converge
519  CPPUNIT_ASSERT_EQUAL((int)ChainIkSolverVel_pinv::E_CONVERGE_PINV_SINGULAR,
520  ikvelsolver1.getError()); // truncated SV solution
521  CPPUNIT_ASSERT_EQUAL((unsigned int)2,
522  ikvelsolver1.getNrZeroSigmas()) ; // 2 singular values (jac pseudoinverse singular)
523 
524  std::cout<<"nonconvergence: large displacement, low iterations"<<std::endl;
525 
526  q(0) = 0. ;
527  q(1) = 0.5 ;
528  q(2) = 0.4 ;
529  q(3) = -M_PI_2 ;
530  q(4) = 0. ;
531  q(5) = 0. ;
532  q(6) = 0. ;
533 
534  // big displacement
535  dF.M = KDL::Rotation::RPY(0.2, 0.2, 0.2) ;
536  dF.p = KDL::Vector(-0.2,-0.2, -0.2) ;
537 
538  // low iterations
539  maxiter = 5 ;
540  ChainIkSolverPos_NR iksolver2(motomansia10,fksolver,ikvelsolver1,maxiter,eps);
541 
542  CPPUNIT_ASSERT_EQUAL(0, fksolver.JntToCart(q,F));
543  F_des = F * dF ;
544 
545  CPPUNIT_ASSERT_EQUAL((int)SolverI::E_MAX_ITERATIONS_EXCEEDED,
546  iksolver2.CartToJnt(q,F_des,q_solved)); // does not converge
547  CPPUNIT_ASSERT_EQUAL((int)SolverI::E_NOERROR,
548  ikvelsolver1.getError());
549  CPPUNIT_ASSERT_EQUAL((unsigned int)1,
550  ikvelsolver1.getNrZeroSigmas()) ; // 1 singular value (jac pseudoinverse exists)
551 
552  std::cout<<"nonconvergence: fully singular"<<std::endl;
553 
554  q(0) = 0. ;
555  q(1) = 0. ;
556  q(2) = 0. ;
557  q(3) = 0. ;
558  q(4) = 0. ;
559  q(5) = 0. ;
560  q(6) = 0. ;
561 
562  dF.M = KDL::Rotation::RPY(0.1, 0.1, 0.1) ;
563  dF.p = KDL::Vector(0.01,0.01,0.01) ;
564 
565  CPPUNIT_ASSERT_EQUAL((int)SolverI::E_NOERROR, fksolver.JntToCart(q,F));
566  F_des = F * dF ;
567 
568  CPPUNIT_ASSERT_EQUAL((int)SolverI::E_MAX_ITERATIONS_EXCEEDED,
569  iksolver1.CartToJnt(q,F_des,q_solved)); // no converge
570  CPPUNIT_ASSERT_EQUAL((int)ChainIkSolverVel_pinv::E_CONVERGE_PINV_SINGULAR,
571  ikvelsolver1.getError()); // truncated SV solution
572  CPPUNIT_ASSERT_EQUAL((unsigned int)3,
573  ikvelsolver1.getNrZeroSigmas());
574 }
575 
576 
577 void SolverTest::IkVelSolverWDLSTest()
578 {
579  int maxiter = 30;
580  double eps = 0.1 ;
581  double lambda = 0.1 ;
582 
583  std::cout<<"KDL-IK WDLS Vel Solver Tests for Near Zero SVs"<<std::endl;
584 
585  KDL::ChainIkSolverVel_wdls ikvelsolver(motomansia10,eps,maxiter) ;
586  ikvelsolver.setLambda(lambda) ;
587  unsigned int nj = motomansia10.getNrOfJoints();
588  JntArray q(nj), dq(nj) ;
589 
590  KDL::Vector v05(0.05,0.05,0.05) ;
591  KDL::Twist dx(v05,v05) ;
592 
593  std::cout<<"smallest singular value is above threshold (no WDLS)"<<std::endl;
594 
595  q(0) = 0. ;
596  q(1) = 0.5 ;
597  q(2) = 0.4 ;
598  q(3) = -M_PI_2 ;
599  q(4) = 0. ;
600  q(5) = 0. ;
601  q(6) = 0. ;
602 
603  CPPUNIT_ASSERT_EQUAL((int)SolverI::E_NOERROR,
604  ikvelsolver.CartToJnt(q, dx, dq)) ; // wdls mode
605  CPPUNIT_ASSERT(1==ikvelsolver.getNrZeroSigmas()) ; // 1 singular value
606 
607 
608  std::cout<<"smallest singular value is below threshold (lambda is scaled)"<<std::endl;
609 
610  q(1) = 0.2 ;
611 
612  CPPUNIT_ASSERT_EQUAL((int)ChainIkSolverVel_wdls::E_CONVERGE_PINV_SINGULAR,
613  ikvelsolver.CartToJnt(q, dx, dq)) ; // wdls mode
614  CPPUNIT_ASSERT_EQUAL((unsigned int)2,ikvelsolver.getNrZeroSigmas()) ; // 2 singular values
615  CPPUNIT_ASSERT_EQUAL(ikvelsolver.getLambdaScaled(),
616  sqrt(1.0-(ikvelsolver.getSigmaMin()/eps)*(ikvelsolver.getSigmaMin()/eps))*lambda) ;
617 
618  std::cout<<"smallest singular value is zero (lambda_scaled=lambda)"<<std::endl;
619 
620  q(1) = 0.0 ;
621 
622  CPPUNIT_ASSERT_EQUAL((int)ChainIkSolverVel_wdls::E_CONVERGE_PINV_SINGULAR,
623  ikvelsolver.CartToJnt(q, dx, dq)) ; // wdls mode
624  CPPUNIT_ASSERT_EQUAL((unsigned int)2,ikvelsolver.getNrZeroSigmas()) ; // 2 singular values
625  CPPUNIT_ASSERT_EQUAL(ikvelsolver.getLambdaScaled(),lambda) ; // full value
626 
627  // fully singular
628  q(2) = 0.0 ;
629  q(3) = 0.0 ;
630 
631  CPPUNIT_ASSERT_EQUAL((int)ChainIkSolverVel_wdls::E_CONVERGE_PINV_SINGULAR,
632  ikvelsolver.CartToJnt(q, dx, dq)) ; // wdls mode
633  CPPUNIT_ASSERT_EQUAL(4,(int)ikvelsolver.getNrZeroSigmas()) ;
634  CPPUNIT_ASSERT_EQUAL(ikvelsolver.getLambdaScaled(),lambda) ; // full value
635 }
636 
637 
638 void SolverTest::FkPosAndJacLocal(Chain& chain,ChainFkSolverPos& fksolverpos,ChainJntToJacSolver& jacsolver)
639 {
640  double deltaq = 1E-4;
641 
642  Frame F1,F2;
643 
644  JntArray q(chain.getNrOfJoints());
645  Jacobian jac(chain.getNrOfJoints());
646 
647  for(unsigned int i=0; i<chain.getNrOfJoints(); i++)
648  {
649  random(q(i));
650  }
651 
652  jacsolver.JntToJac(q,jac);
653 
654  for (unsigned int i=0; i< q.rows() ; i++)
655  {
656  // test the derivative of J towards qi
657  double oldqi = q(i);
658  q(i) = oldqi+deltaq;
659  CPPUNIT_ASSERT_EQUAL((int)SolverI::E_NOERROR, fksolverpos.JntToCart(q,F2));
660  q(i) = oldqi-deltaq;
661  CPPUNIT_ASSERT_EQUAL((int)SolverI::E_NOERROR, fksolverpos.JntToCart(q,F1));
662  q(i) = oldqi;
663  // check Jacobian :
664  Twist Jcol1 = diff(F1,F2,2*deltaq);
665  Twist Jcol2(Vector(jac(0,i),jac(1,i),jac(2,i)),
666  Vector(jac(3,i),jac(4,i),jac(5,i)));
667 
668  //CPPUNIT_ASSERT_EQUAL(true,Equal(Jcol1,Jcol2,epsJ));
669  CPPUNIT_ASSERT_EQUAL(Jcol1,Jcol2);
670  }
671 }
672 
673 void SolverTest::FkVelAndJacLocal(Chain& chain, ChainFkSolverVel& fksolvervel, ChainJntToJacSolver& jacsolver)
674 {
675  JntArray q(chain.getNrOfJoints());
676  JntArray qdot(chain.getNrOfJoints());
677 
678  for(unsigned int i=0; i<chain.getNrOfJoints(); i++)
679  {
680  random(q(i));
681  random(qdot(i));
682  }
683  JntArrayVel qvel(q,qdot);
684  Jacobian jac(chain.getNrOfJoints());
685 
686  FrameVel cart;
687  Twist t;
688 
689  jacsolver.JntToJac(qvel.q,jac);
690  CPPUNIT_ASSERT_EQUAL((int)SolverI::E_NOERROR, fksolvervel.JntToCart(qvel,cart));
691  MultiplyJacobian(jac,qvel.qdot,t);
692  CPPUNIT_ASSERT_EQUAL(cart.deriv(),t);
693 }
694 
695 void SolverTest::FkVelAndIkVelLocal(Chain& chain, ChainFkSolverVel& fksolvervel, ChainIkSolverVel& iksolvervel)
696 {
697 
698  JntArray q(chain.getNrOfJoints());
699  JntArray qdot(chain.getNrOfJoints());
700 
701  for(unsigned int i=0; i<chain.getNrOfJoints(); i++)
702  {
703  random(q(i));
704  random(qdot(i));
705  }
706  JntArrayVel qvel(q,qdot);
707  JntArray qdot_solved(chain.getNrOfJoints());
708 
709  FrameVel cart;
710 
711  CPPUNIT_ASSERT_EQUAL((int)SolverI::E_NOERROR, fksolvervel.JntToCart(qvel,cart));
712 
713  CPPUNIT_ASSERT_EQUAL((int)SolverI::E_NOERROR, iksolvervel.CartToJnt(qvel.q,cart.deriv(),qdot_solved));
714 
715  qvel.deriv()=qdot_solved;
716 
717  if(chain.getNrOfJoints()<=6)
718  CPPUNIT_ASSERT(Equal(qvel.qdot,qdot_solved,1e-5));
719  else
720  {
721  FrameVel cart_solved;
722  CPPUNIT_ASSERT_EQUAL((int)SolverI::E_NOERROR,fksolvervel.JntToCart(qvel,cart_solved));
723  CPPUNIT_ASSERT(Equal(cart.deriv(),cart_solved.deriv(),1e-5));
724  }
725 }
726 
727 
728 void SolverTest::FkPosAndIkPosLocal(Chain& chain,ChainFkSolverPos& fksolverpos, ChainIkSolverPos& iksolverpos)
729 {
730  JntArray q(chain.getNrOfJoints());
731  for(unsigned int i=0; i<chain.getNrOfJoints(); i++)
732  {
733  random(q(i));
734  }
735  JntArray q_init(chain.getNrOfJoints());
736  double tmp;
737  for(unsigned int i=0; i<chain.getNrOfJoints(); i++)
738  {
739  random(tmp);
740  q_init(i)=q(i)+0.1*tmp;
741  }
742  JntArray q_solved(q);
743 
744  Frame F1,F2;
745 
746  CPPUNIT_ASSERT_EQUAL((int)SolverI::E_NOERROR, fksolverpos.JntToCart(q,F1));
747  CPPUNIT_ASSERT_EQUAL((int)SolverI::E_NOERROR, iksolverpos.CartToJnt(q_init,F1,q_solved));
748  CPPUNIT_ASSERT_EQUAL((int)SolverI::E_NOERROR, fksolverpos.JntToCart(q_solved,F2));
749 
750  CPPUNIT_ASSERT_EQUAL(F1,F2);
751  //CPPUNIT_ASSERT_EQUAL(q,q_solved);
752 
753 }
754 
755 
756 void SolverTest::VereshchaginTest()
757 {
758 
759  Vector constrainXLinear(1.0, 0.0, 0.0);
760  Vector constrainXAngular(0.0, 0.0, 0.0);
761  Vector constrainYLinear(0.0, 0.0, 0.0);
762  Vector constrainYAngular(0.0, 0.0, 0.0);
763  // Vector constrainZLinear(0.0, 0.0, 0.0);
764  //Vector constrainZAngular(0.0, 0.0, 0.0);
765  Twist constraintForcesX(constrainXLinear, constrainXAngular);
766  Twist constraintForcesY(constrainYLinear, constrainYAngular);
767  //Twist constraintForcesZ(constrainZLinear, constrainZAngular);
768  Jacobian alpha(1);
769  //alpha.setColumn(0, constraintForcesX);
770  alpha.setColumn(0, constraintForcesX);
771  //alpha.setColumn(0, constraintForcesZ);
772 
773  //Acceleration energy at the end-effector
774  JntArray betha(1); //set to zero
775  betha(0) = 0.0;
776  //betha(1) = 0.0;
777  //betha(2) = 0.0;
778 
779  //arm root acceleration
780  Vector linearAcc(0.0, 10, 0.0); //gravitational acceleration along Y
781  Vector angularAcc(0.0, 0.0, 0.0);
782  Twist twist1(linearAcc, angularAcc);
783 
784  //external forces on the arm
785  Vector externalForce1(0.0, 0.0, 0.0);
786  Vector externalTorque1(0.0, 0.0, 0.0);
787  Vector externalForce2(0.0, 0.0, 0.0);
788  Vector externalTorque2(0.0, 0.0, 0.0);
789  Wrench externalNetForce1(externalForce1, externalTorque1);
790  Wrench externalNetForce2(externalForce2, externalTorque2);
791  Wrenches externalNetForce;
792  externalNetForce.push_back(externalNetForce1);
793  externalNetForce.push_back(externalNetForce2);
794  //~Definition of constraints and external disturbances
795  //-------------------------------------------------------------------------------------//
796 
797 
798  //Definition of solver and initial configuration
799  //-------------------------------------------------------------------------------------//
800  int numberOfConstraints = 1;
801  ChainIdSolver_Vereshchagin constraintSolver(chaindyn, twist1, numberOfConstraints);
802 
803  //These arrays of joint values contain actual and desired values
804  //actual is generated by a solver and integrator
805  //desired is given by an interpolator
806  //error is the difference between desired-actual
807  //in this test only the actual values are printed.
808  const int k = 1;
809  JntArray jointPoses[k];
810  JntArray jointRates[k];
811  JntArray jointAccelerations[k];
812  JntArray jointTorques[k];
813  for (int i = 0; i < k; i++)
814  {
815  JntArray jointValues(chaindyn.getNrOfJoints());
816  jointPoses[i] = jointValues;
817  jointRates[i] = jointValues;
818  jointAccelerations[i] = jointValues;
819  jointTorques[i] = jointValues;
820  }
821 
822  // Initial arm position configuration/constraint
823  JntArray jointInitialPose(chaindyn.getNrOfJoints());
824  jointInitialPose(0) = 0.0; // initial joint0 pose
825  jointInitialPose(1) = M_PI/6.0; //initial joint1 pose, negative in clockwise
826  //j0=0.0, j1=pi/6.0 correspond to x = 0.2, y = -0.7464
827  //j0=2*pi/3.0, j1=pi/4.0 correspond to x = 0.44992, y = 0.58636
828 
829  //actual
830  jointPoses[0](0) = jointInitialPose(0);
831  jointPoses[0](1) = jointInitialPose(1);
832 
833  //~Definition of solver and initial configuration
834  //-------------------------------------------------------------------------------------//
835 
836 
837  //Definition of process main loop
838  //-------------------------------------------------------------------------------------//
839  //Time required to complete the task move(frameinitialPose, framefinalPose)
840  double taskTimeConstant = 0.1;
841  double simulationTime = 1*taskTimeConstant;
842  double timeDelta = 0.01;
843 
844  const std::string msg = "Assertion failed, check matrix and array sizes";
845 
846  for (double t = 0.0; t <=simulationTime; t = t + timeDelta)
847  {
848  CPPUNIT_ASSERT_EQUAL((int)SolverI::E_NOERROR, constraintSolver.CartToJnt(jointPoses[0], jointRates[0], jointAccelerations[0], alpha, betha, externalNetForce, jointTorques[0]));
849 
850  //Integration(robot joint values for rates and poses; actual) at the given "instanteneous" interval for joint position and velocity.
851  jointRates[0](0) = jointRates[0](0) + jointAccelerations[0](0) * timeDelta; //Euler Forward
852  jointPoses[0](0) = jointPoses[0](0) + (jointRates[0](0) - jointAccelerations[0](0) * timeDelta / 2.0) * timeDelta; //Trapezoidal rule
853  jointRates[0](1) = jointRates[0](1) + jointAccelerations[0](1) * timeDelta; //Euler Forward
854  jointPoses[0](1) = jointPoses[0](1) + (jointRates[0](1) - jointAccelerations[0](1) * timeDelta / 2.0) * timeDelta;
855  //printf("time, j0_pose, j1_pose, j0_rate, j1_rate, j0_acc, j1_acc, j0_constraintTau, j1_constraintTau \n");
856  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), jointTorques[0](0), jointTorques[0](1));
857  }
858 }
859 
860 void SolverTest::FkPosVectTest()
861 {
862  ChainFkSolverPos_recursive fksolver1(chain1);
863  std::vector<Frame> v_out(chain1.getNrOfSegments());
864 
865  JntArray q(chain1.getNrOfJoints());
866  JntArray qdot(chain1.getNrOfJoints());
867 
868  for(unsigned int i=0; i<chain1.getNrOfJoints(); i++)
869  {
870  random(q(i));
871  random(qdot(i));
872  }
873  Frame f_out;
874  fksolver1.JntToCart(q,v_out);
875  fksolver1.JntToCart(q,f_out);
876 
877  CPPUNIT_ASSERT(Equal(v_out[chain1.getNrOfSegments()-1],f_out,1e-5));
878 }
879 
880 void SolverTest::FkVelVectTest()
881 {
882  ChainFkSolverVel_recursive fksolver1(chain1);
883  std::vector<FrameVel> v_out(chain1.getNrOfSegments());
884 
885  JntArray q(chain1.getNrOfJoints());
886  JntArray qdot(chain1.getNrOfJoints());
887 
888  for(unsigned int i=0; i<chain1.getNrOfJoints(); i++)
889  {
890  random(q(i));
891  random(qdot(i));
892  }
893  JntArrayVel qvel(q,qdot);
894  FrameVel f_out;
895  fksolver1.JntToCart(qvel,v_out);
896  fksolver1.JntToCart(qvel,f_out);
897 
898  CPPUNIT_ASSERT(Equal(v_out[chain1.getNrOfSegments()-1],f_out,1e-5));
899 }
900 
901 void SolverTest::FdSolverDevelopmentTest()
902 {
903  int ret;
904  double eps=1.e-3;
905 
906  std::cout<<"KDL FD Solver Development Test for Motoman SIA10"<<std::endl;
907 
908  // NOTE: This is prototype code for the KDL forward dynamics solver class
909  // based on the Recurse Newton Euler Method: ChainFdSolver_RNE
910 
911  // Dynamics Solver
912  Vector gravity(0.0, 0.0, -9.81); // base frame
913  ChainDynParam DynSolver = KDL::ChainDynParam(motomansia10dyn, gravity);
914 
915  unsigned int nj = motomansia10dyn.getNrOfJoints();
916  unsigned int ns = motomansia10dyn.getNrOfSegments();
917 
918  // Joint position, velocity, and acceleration
919  JntArray q(nj);
920  JntArray qd(nj);
921  JntArray qdd(nj);
922 
923  // random
924  q(0) = 0.2;
925  q(1) = 0.6;
926  q(2) = 1.;
927  q(3) = 0.5;
928  q(4) = -1.4;
929  q(5) = 0.3;
930  q(6) = -0.8;
931 
932  qd(0) = 1.;
933  qd(1) = -2.;
934  qd(2) = 3.;
935  qd(3) = -4.;
936  qd(4) = 5.;
937  qd(5) = -6.;
938  qd(6) = 7.;
939 
940  // Validate FK
941  ChainFkSolverPos_recursive fksolver(motomansia10dyn);
942  Frame f_out;
943  fksolver.JntToCart(q,f_out);
944  CPPUNIT_ASSERT(Equal(-0.547, f_out.p(0), eps));
945  CPPUNIT_ASSERT(Equal(-0.301, f_out.p(1), eps));
946  CPPUNIT_ASSERT(Equal(0.924, f_out.p(2), eps));
947  CPPUNIT_ASSERT(Equal(0.503, f_out.M(0,0), eps));
948  CPPUNIT_ASSERT(Equal(0.286, f_out.M(0,1), eps));
949  CPPUNIT_ASSERT(Equal(-0.816, f_out.M(0,2), eps));
950  CPPUNIT_ASSERT(Equal(-0.859, f_out.M(1,0), eps));
951  CPPUNIT_ASSERT(Equal(0.269, f_out.M(1,1), eps));
952  CPPUNIT_ASSERT(Equal(-0.436, f_out.M(1,2), eps));
953  CPPUNIT_ASSERT(Equal(0.095, f_out.M(2,0), eps));
954  CPPUNIT_ASSERT(Equal(0.920, f_out.M(2,1), eps));
955  CPPUNIT_ASSERT(Equal(0.381, f_out.M(2,2), eps));
956 
957  // Validate Jacobian
958  ChainJntToJacSolver jacsolver(motomansia10dyn);
959  Jacobian jac(nj);
960  jacsolver.JntToJac(q, jac);
961  double Jac[6][7] =
962  {{0.301,-0.553,0.185,0.019,0.007,-0.086,0.},
963  {-0.547,-0.112,-0.139,-0.376,-0.037,0.063,0.},
964  {0.,-0.596,0.105,-0.342,-0.026,-0.113,0.},
965  {0.,0.199,-0.553,0.788,-0.615,0.162,-0.816},
966  {0.,-0.980,-0.112,-0.392,-0.536,-0.803,-0.436},
967  {1.,0.,0.825,0.475,0.578,-0.573,0.381}};
968  for ( int i=0; i<6; i++ ) {
969  for ( int j=0; j<nj; j++ ) {
970  CPPUNIT_ASSERT(Equal(jac(i,j), Jac[i][j], eps));
971  }
972  }
973 
974  // Return values
975  JntArray taugrav(nj);
976  JntArray taucor(nj);
977  JntSpaceInertiaMatrix H(nj), Heff(nj);
978 
979  // Compute Dynamics (torque in N-m)
980  ret = DynSolver.JntToGravity(q, taugrav);
981  if (ret < 0) std::cout << "KDL: inverse dynamics ERROR: " << ret << std::endl;
982  CPPUNIT_ASSERT(Equal(0.000, taugrav(0), eps));
983  CPPUNIT_ASSERT(Equal(-36.672, taugrav(1), eps));
984  CPPUNIT_ASSERT(Equal(4.315, taugrav(2), eps));
985  CPPUNIT_ASSERT(Equal(-11.205, taugrav(3), eps));
986  CPPUNIT_ASSERT(Equal(0.757, taugrav(4), eps));
987  CPPUNIT_ASSERT(Equal(1.780, taugrav(5), eps));
988  CPPUNIT_ASSERT(Equal(0.000, taugrav(6), eps));
989 
990  ret = DynSolver.JntToCoriolis(q, qd, taucor);
991  if (ret < 0) std::cout << "KDL: inverse dynamics ERROR: " << ret << std::endl;
992  CPPUNIT_ASSERT(Equal(-15.523, taucor(0), eps));
993  CPPUNIT_ASSERT(Equal(24.250, taucor(1), eps));
994  CPPUNIT_ASSERT(Equal(-6.862, taucor(2), eps));
995  CPPUNIT_ASSERT(Equal(6.303, taucor(3), eps));
996  CPPUNIT_ASSERT(Equal(0.110, taucor(4), eps));
997  CPPUNIT_ASSERT(Equal(-4.898, taucor(5), eps));
998  CPPUNIT_ASSERT(Equal(-0.249, taucor(6), eps));
999 
1000  ret = DynSolver.JntToMass(q, H);
1001  if (ret < 0) std::cout << "KDL: inverse dynamics ERROR: " << ret << std::endl;
1002  double Hexp[7][7] =
1003  {{6.8687,-0.4333,0.4599,0.6892,0.0638,-0.0054,0.0381},
1004  {-0.4333,8.8324,-0.5922,0.7905,0.0003,-0.0242,0.0265},
1005  {0.4599,-0.5922,3.3496,-0.0253,0.1150,-0.0243,0.0814},
1006  {0.6892,0.7905,-0.0253,3.9623,-0.0201,0.0087,-0.0291},
1007  {0.0638,0.0003,0.1150,-0.0201,1.1234,0.0029,0.0955},
1008  {-0.0054,-0.0242,-0.0243,0.0087,0.0029,1.1425,0},
1009  {0.0381,0.0265,0.0814,-0.0291,0.0955,0,1.1000}};
1010  for ( int i=0; i<nj; i++ ) {
1011  for ( int j=0; j<nj; j++ ) {
1012  CPPUNIT_ASSERT(Equal(H(i,j), Hexp[i][j], eps));
1013  }
1014  }
1015 
1016  // Inverse Dynamics:
1017  // T = H * qdd + Tcor + Tgrav - J^T * Fext
1018  // Forward Dynamics
1019  // 1. Call JntToMass from ChainDynParam to get H
1020  // 2. Call ID with qdd=0 to get T=Tcor+Tgrav+J^T*Fext
1021  // 3. Calculate qdd = H^-1 * T
1022  KDL::ChainIdSolver_RNE IdSolver = KDL::ChainIdSolver_RNE(motomansia10dyn, gravity);
1023 
1024  // In tool coordinates
1025  Vector f(10,-20,30) ;
1026  Vector n(3,-4,5) ;
1027  Wrench f_tool(f,n);
1028  // In local link coordinates
1029  Wrenches f_ext(ns);
1030  for(int i=0;i<ns;i++){
1031  SetToZero(f_ext[i]);
1032  }
1033  f_ext[ns-1]=f_tool;
1034 
1035  JntArray Tnoninertial(nj);
1036  JntArray jntarraynull(nj);
1037  SetToZero(jntarraynull);
1038  IdSolver.CartToJnt(q, qd, jntarraynull, f_ext, Tnoninertial);
1039  CPPUNIT_ASSERT(Equal(-21.252, Tnoninertial(0), eps));
1040  CPPUNIT_ASSERT(Equal(-37.933, Tnoninertial(1), eps));
1041  CPPUNIT_ASSERT(Equal(-2.497, Tnoninertial(2), eps));
1042  CPPUNIT_ASSERT(Equal(-15.289, Tnoninertial(3), eps));
1043  CPPUNIT_ASSERT(Equal(-4.646, Tnoninertial(4), eps));
1044  CPPUNIT_ASSERT(Equal(-9.201, Tnoninertial(5), eps));
1045  CPPUNIT_ASSERT(Equal(-5.249, Tnoninertial(6), eps));
1046 
1047  // get acceleration using inverse symmetric matrix times vector
1048  Eigen::MatrixXd H_eig(nj,nj), L(nj,nj);
1049  Eigen::VectorXd Tnon_eig(nj), D(nj), r(nj), acc_eig(nj);
1050  for(int i=0;i<nj;i++){
1051  Tnon_eig(i) = -Tnoninertial(i);
1052  for(int j=0;j<nj;j++){
1053  H_eig(i,j) = H(i,j);
1054  }
1055  }
1056  ldl_solver_eigen(H_eig, Tnon_eig, L, D, r, acc_eig);
1057  for(int i=0;i<nj;i++){
1058  qdd(i) = acc_eig(i);
1059  }
1060  CPPUNIT_ASSERT(Equal(2.998, qdd(0), eps));
1061  CPPUNIT_ASSERT(Equal(4.289, qdd(1), eps));
1062  CPPUNIT_ASSERT(Equal(0.946, qdd(2), eps));
1063  CPPUNIT_ASSERT(Equal(2.518, qdd(3), eps));
1064  CPPUNIT_ASSERT(Equal(3.530, qdd(4), eps));
1065  CPPUNIT_ASSERT(Equal(8.150, qdd(5), eps));
1066  CPPUNIT_ASSERT(Equal(4.254, qdd(6), eps));
1067 }
1068 
1069 void SolverTest::FdSolverConsistencyTest()
1070 {
1071  int ret;
1072  double eps=1.e-3;
1073 
1074  std::cout<<"KDL FD Solver Consistency Test for Motoman SIA10"<<std::endl;
1075 
1076  // NOTE: Compute the forward and inverse dynamics and test for consistency
1077 
1078  // Forward Dynamics Solver
1079  Vector gravity(0.0, 0.0, -9.81); // base frame
1080  KDL::ChainFdSolver_RNE FdSolver = KDL::ChainFdSolver_RNE(motomansia10dyn, gravity);
1081 
1082  unsigned int nj = motomansia10dyn.getNrOfJoints();
1083  unsigned int ns = motomansia10dyn.getNrOfSegments();
1084 
1085  // Joint position, velocity, and acceleration
1086  KDL::JntArray q(nj);
1087  KDL::JntArray qd(nj);
1088  KDL::JntArray qdd(nj);
1089  KDL::JntArray tau(nj);
1090 
1091  // random
1092  q(0) = 0.2;
1093  q(1) = 0.6;
1094  q(2) = 1.;
1095  q(3) = 0.5;
1096  q(4) = -1.4;
1097  q(5) = 0.3;
1098  q(6) = -0.8;
1099 
1100  qd(0) = 1.;
1101  qd(1) = -2.;
1102  qd(2) = 3.;
1103  qd(3) = -4.;
1104  qd(4) = 5.;
1105  qd(5) = -6.;
1106  qd(6) = 7.;
1107 
1108  // actuator torques
1109  tau(0) = 50.;
1110  tau(1) = -20.;
1111  tau(2) = 10.;
1112  tau(3) = 40.;
1113  tau(4) = -60.;
1114  tau(5) = 15.;
1115  tau(6) = -10.;
1116 
1117  KDL::Vector f(10,-20,30) ;
1118  KDL::Vector n(3,-4,5) ;
1119  KDL::Wrench f_tool(f,n);
1120  // In local link coordinates
1121  KDL::Wrenches f_ext(ns);
1122  for(int i=0;i<ns;i++){
1123  SetToZero(f_ext[i]);
1124  }
1125  f_ext[ns-1]=f_tool;
1126 
1127  // Call FD function
1128  ret = FdSolver.CartToJnt(q, qd, tau, f_ext, qdd);
1129  if (ret < 0) std::cout << "KDL: forward dynamics ERROR: " << ret << std::endl;
1130  CPPUNIT_ASSERT(Equal(9.486, qdd(0), eps));
1131  CPPUNIT_ASSERT(Equal(1.830, qdd(1), eps));
1132  CPPUNIT_ASSERT(Equal(4.726, qdd(2), eps));
1133  CPPUNIT_ASSERT(Equal(11.665, qdd(3), eps));
1134  CPPUNIT_ASSERT(Equal(-50.108, qdd(4), eps));
1135  CPPUNIT_ASSERT(Equal(21.403, qdd(5), eps));
1136  CPPUNIT_ASSERT(Equal(-0.381, qdd(6), eps));
1137 
1138  // Check against ID solver for consistency
1139  KDL::ChainIdSolver_RNE IdSolver = KDL::ChainIdSolver_RNE(motomansia10dyn, gravity);
1140  KDL::JntArray torque(nj);
1141  IdSolver.CartToJnt(q, qd, qdd, f_ext, torque);
1142  for ( int i=0; i<nj; i++ )
1143  {
1144  CPPUNIT_ASSERT(Equal(torque(i), tau(i), eps));
1145  }
1146 
1147  return;
1148 }
1149 
1150 void SolverTest::LDLdecompTest()
1151 {
1152  std::cout<<"LDL Solver Test"<<std::endl;
1153  double eps=1.e-6;
1154 
1155  // Given A and b, solve Ax=b for x, where A is a symmetric real matrix
1156  // https://en.wikipedia.org/wiki/Cholesky_decomposition
1157  Eigen::MatrixXd A(3,3), Aout(3,3);
1158  Eigen::VectorXd b(3);
1159  Eigen::MatrixXd L(3,3), Lout(3,3);
1160  Eigen::VectorXd d(3), dout(3);
1161  Eigen::VectorXd x(3), xout(3);
1162  Eigen::VectorXd r(3); // temp variable used internally by ldl solver
1163  Eigen::MatrixXd Dout(3,3); // diagonal matrix
1164 
1165  // Given
1166  A << 4, 12, -16,
1167  12, 37, -43,
1168  -16, -43, 98;
1169  b << 28, 117, 98;
1170  // Results to verify
1171  L << 1, 0, 0,
1172  3, 1, 0,
1173  -4, 5, 1;
1174  d << 4, 1, 9;
1175  x << 3, 8, 5;
1176 
1177  ldl_solver_eigen(A, b, Lout, dout, r, xout);
1178 
1179  for(int i=0;i<3;i++){
1180  for(int j=0;j<3;j++){
1181  CPPUNIT_ASSERT(Equal(L(i,j), Lout(i,j), eps));
1182  }
1183  }
1184 
1185  Dout.setZero();
1186  for(int i=0;i<3;i++){
1187  Dout(i,i) = dout(i);
1188  }
1189 
1190  // Verify solution for x
1191  for(int i=0;i<3;i++){
1192  CPPUNIT_ASSERT(Equal(xout(i), x(i), eps));
1193  }
1194 
1195  // Test reconstruction of A from LDL^T decomposition
1196  Aout = Lout * Dout * Lout.transpose();
1197  for(int i=0;i<3;i++){
1198  for(int j=0;j<3;j++){
1199  CPPUNIT_ASSERT(Equal(A(i,j), Aout(i,j), eps));
1200  }
1201  }
1202 
1203  return;
1204 }
SolverTest::FkVelAndIkVelTest
void FkVelAndIkVelTest()
Definition: solvertest.cpp:362
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:33
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:175
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::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:577
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::SolverI::E_NOT_UP_TO_DATE
Chain size changed.
Definition: solveri.hpp:97
KDL::diff
IMETHOD Vector diff(const Vector &p_w_a, const Vector &p_w_b, double dt=1)
Definition: frames.hpp:1123
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:346
KDL::RigidBodyInertia
6D Inertia of a rigid body
Definition: rigidbodyinertia.hpp:37
KDL::ChainIdSolver_Vereshchagin::updateInternalDataStructures
virtual void updateInternalDataStructures()
Definition: chainidsolver_vereshchagin.cpp:46
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
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:673
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. It should not be used outside of KDL.
Definition: chainjnttojacsolver.hpp:41
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:1062
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:54
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::ChainIdSolver_Vereshchagin::CartToJnt
int CartToJnt(const JntArray &q, const JntArray &q_dot, JntArray &q_dotdot, const Jacobian &alfa, const JntArray &beta, const Wrenches &f_ext, JntArray &torques)
Definition: chainidsolver_vereshchagin.cpp:51
KDL::ChainFkSolverPos::JntToCart
virtual int JntToCart(const JntArray &q_in, Frame &p_out, int segmentNr=-1)=0
KDL::ChainJntToJacSolver::JntToJac
virtual int JntToJac(const JntArray &q_in, Jacobian &jac, int segmentNR=-1)
Definition: chainjnttojacsolver.cpp:48
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:238
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:71
SolverTest::FkVelVectTest
void FkVelVectTest()
Definition: solvertest.cpp:880
KDL::ChainIkSolverPos_NR_JL
Definition: chainiksolverpos_nr_jl.hpp:40
KDL::ChainIkSolverVel_wdls::updateInternalDataStructures
virtual void updateInternalDataStructures()
Definition: chainiksolvervel_wdls.cpp:54
SolverTest::FkPosAndJacLocal
void FkPosAndJacLocal(Chain &chain, ChainFkSolverPos &fksolverpos, ChainJntToJacSolver &jacsolver)
Definition: solvertest.cpp:638
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:149
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:548
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:456
KDL::ChainIkSolverPos_LMA::updateInternalDataStructures
void updateInternalDataStructures()
Definition: chainiksolverpos_lma.cpp:122
SolverTest::FdSolverConsistencyTest
void FdSolverConsistencyTest()
Definition: solvertest.cpp:1069
kinfam_io.hpp
SolverTest::VereshchaginTest
void VereshchaginTest()
Definition: solvertest.cpp:756
SolverTest::FkPosAndIkPosTest
void FkPosAndIkPosTest()
Definition: solvertest.cpp:405
KDL::ChainDynParam::JntToCoriolis
virtual int JntToCoriolis(const JntArray &q, const JntArray &q_dot, JntArray &coriolis)
Definition: chaindynparam.cpp:126
KDL::JntArray::resize
void resize(unsigned int newSize)
Definition: jntarray.cpp:55
KDL::Wrenches
std::vector< Wrench > Wrenches
Definition: chainfdsolver.hpp:33
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:36
KDL::FrameVel::deriv
Twist deriv() const
Definition: framevel.hpp:222
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:47
SolverTest::FdSolverDevelopmentTest
void FdSolverDevelopmentTest()
Definition: solvertest.cpp:901
KDL::Frame
represents a frame transformation in 3D space (rotation + translation)
Definition: frames.hpp:570
KDL::ChainDynParam::updateInternalDataStructures
virtual void updateInternalDataStructures()
Definition: chaindynparam.cpp:45
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:184
KDL::ChainIkSolverVel_wdls
Definition: chainiksolvervel_wdls.hpp:63
SolverTest::FkVelAndIkVelLocal
void FkVelAndIkVelLocal(Chain &chain, ChainFkSolverVel &fksolvervel, ChainIkSolverVel &iksolvervel)
Definition: solvertest.cpp:695
SolverTest::FkPosVectTest
void FkPosVectTest()
Definition: solvertest.cpp:860
SolverTest::FkPosAndJacTest
void FkPosAndJacTest()
Definition: solvertest.cpp:330
KDL::ChainIkSolverVel_wdls::getSigmaMin
double getSigmaMin() const
Definition: chainiksolvervel_wdls.hpp:183
KDL::ChainDynParam::JntToMass
virtual int JntToMass(const JntArray &q, JntSpaceInertiaMatrix &H)
Definition: chaindynparam.cpp:59
SolverTest::FkPosAndIkPosLocal
void FkPosAndIkPosLocal(Chain &chain, ChainFkSolverPos &fksolverpos, ChainIkSolverPos &iksolverpos)
Definition: solvertest.cpp:728
KDL::ChainIkSolverPos_NR_JL::updateInternalDataStructures
virtual void updateInternalDataStructures()
Definition: chainiksolverpos_nr_jl.cpp:53
KDL::ChainIdSolver_Vereshchagin
Dynamics calculations by constraints based on Vereshchagin 1989. for a chain. This class creates inst...
Definition: chainidsolver_vereshchagin.hpp:40
KDL::random
IMETHOD void random(Vector &a)
addDelta operator for displacement rotational velocity.
Definition: frames.hpp:1208
KDL::ChainIkSolverVel_pinv_givens::updateInternalDataStructures
virtual void updateInternalDataStructures()
Definition: chainiksolvervel_pinv_givens.cpp:49
KDL::ChainFkSolverVel
This abstract class encapsulates a solver for the forward velocity kinematics for a KDL::Chain...
Definition: chainfksolver.hpp:74
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:138
SolverTest::LDLdecompTest
void LDLdecompTest()
Definition: solvertest.cpp:1150
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:72
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 Sat Jun 15 2019 19:07:36