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