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  for(unsigned int i=0; i<chain.getNrOfJoints(); i++)
794  {
795  random(q(i));
796  }
797  JntArray q_init(chain.getNrOfJoints());
798  double tmp;
799  for(unsigned int i=0; i<chain.getNrOfJoints(); i++)
800  {
801  random(tmp);
802  q_init(i)=q(i)+0.1*tmp;
803  }
804  JntArray q_solved(q);
805 
806  Frame F1,F2;
807 
808  CPPUNIT_ASSERT_EQUAL((int)SolverI::E_NOERROR, fksolverpos.JntToCart(q,F1));
809  CPPUNIT_ASSERT_EQUAL((int)SolverI::E_NOERROR, iksolverpos.CartToJnt(q_init,F1,q_solved));
810  CPPUNIT_ASSERT_EQUAL((int)SolverI::E_NOERROR, fksolverpos.JntToCart(q_solved,F2));
811 
812  CPPUNIT_ASSERT_EQUAL(F1,F2);
813  //CPPUNIT_ASSERT_EQUAL(q,q_solved);
814 
815 }
816 
817 
818 void SolverTest::VereshchaginTest()
819 {
820  std::cout << "KDL Vereshchagin Hybrid Dynamics Tests" <<std::endl;
821 
822  // ########################################################################################
823  // Vereshchagin solver test 1
824  // ########################################################################################
842  int solver_return = 0;
843  double eps = 1.e-3;
844 
845  unsigned int nj = kukaLWR.getNrOfJoints();
846  unsigned int ns = kukaLWR.getNrOfSegments();
847 
848  // Necessary test for the used robot model: KDL's implementation of the Vereshchagin solver
849  // can only work with the robot chains that have equal number of joints and segments
850  CPPUNIT_ASSERT(Equal(nj, ns));
851 
852  // Joint position, velocity, acceleration, feed-forward and constraint torques
853  KDL::JntArray q(nj); //input
854  KDL::JntArray qd(nj); //input
855  KDL::JntArray qdd(nj); //output
856  KDL::JntArray ff_tau(nj); //input
857  KDL::JntArray constraint_tau(nj); //output
858 
859  // Random configuration
860  q(0) = 1.6;
861  q(1) = 0.0;
862  q(2) = -1.6;
863  q(3) = -1.57;
864  q(4) = 0.0;
865  q(5) = 1.57;
866  q(6) = -0.8;
867 
868  qd(0) = 1.0;
869  qd(1) = -2.0;
870  qd(2) = 3.0;
871  qd(3) = -4.0;
872  qd(4) = 5.0;
873  qd(5) = -6.0;
874  qd(6) = 7.0;
875 
876  // Random feedforwad torques acting on robot joints
877  ff_tau(0) = 5.0;
878  ff_tau(1) = 0.0;
879  ff_tau(2) = 0.0;
880  ff_tau(3) = 0.0;
881  ff_tau(4) = 0.0;
882  ff_tau(5) = -6.0;
883  ff_tau(6) = 0.0;
884 
885  // External Wrench acting on the end-effector, expressed in base link coordinates
886  // Vereshchagin solver expects that external wrenches are expressed w.r.t. robot's base frame
887  KDL::Vector f(10.0, 15.0, 0.0);
888  KDL::Vector n(0.0, 0.0, 5.0);
889  KDL::Wrench f_tool(f, n);
890  KDL::Wrenches f_ext(ns);
891  f_ext[ns - 1] = f_tool; //input
892 
901  int number_of_constraints = 6;
902 
903  // Constraint Unit forces defined for the end-effector
904  Jacobian alpha_unit_force(number_of_constraints);
905 
906  // Set directions in which the constraint force should work. Alpha in the solver
907  Twist unit_force_x_l(
908  Vector(1.0, 0.0, 0.0),
909  Vector(0.0, 0.0, 0.0));
910  alpha_unit_force.setColumn(0, unit_force_x_l); // constraint active
911 
912  Twist unit_force_y_l(
913  Vector(0.0, 1.0, 0.0),
914  Vector(0.0, 0.0, 0.0));
915  alpha_unit_force.setColumn(1, unit_force_y_l); // constraint active
916 
917  Twist unit_force_z_l(
918  Vector(0.0, 0.0, 1.0),
919  Vector(0.0, 0.0, 0.0));
920  alpha_unit_force.setColumn(2, unit_force_z_l); // constraint active
921 
922  Twist unit_force_x_a(
923  Vector(0.0, 0.0, 0.0),
924  Vector(0.0, 0.0, 0.0));
925  alpha_unit_force.setColumn(3, unit_force_x_a); // constraint disabled... In this direction, end-effector's motion is left to emerge naturally
926 
927  Twist unit_force_y_a(
928  Vector(0.0, 0.0, 0.0),
929  Vector(0.0, 0.0, 0.0));
930  alpha_unit_force.setColumn(4, unit_force_y_a); // constraint disabled... In this direction, end-effector's motion is left to emerge naturally
931 
932  Twist unit_force_z_a(
933  Vector(0.0, 0.0, 0.0),
934  Vector(0.0, 0.0, 1.0));
935  alpha_unit_force.setColumn(5, unit_force_z_a); // constraint active
936 
937  // Acceleration energy for the end-effector.
938  JntArray beta_energy(number_of_constraints);
939  beta_energy(0) = -0.5;
940  beta_energy(1) = -0.5;
941  beta_energy(2) = 0.0;
942  beta_energy(3) = 0.0; // this value has no impact on computations, since its corresponding constraint is disabled
943  beta_energy(4) = 0.0; // this value has no impact on computations, since its corresponding constraint is disabled
944  beta_energy(5) = 0.2;
945 
946  // Arm root acceleration (robot's base mounted on an even surface)
947  // Note: Vereshchagin solver takes root acc. with opposite sign comparead to the KDL's FD and RNE solvers
948  Twist root_Acc(Vector(0.0, 0.0, 9.81), Vector(0.0, 0.0, 0.0));
949 
950  ChainHdSolver_Vereshchagin vereshchaginSolver(kukaLWR, root_Acc, number_of_constraints);
951  solver_return = vereshchaginSolver.CartToJnt(q, qd, qdd, alpha_unit_force, beta_energy, f_ext, ff_tau, constraint_tau);
952  if (solver_return < 0) std::cout << "KDL: Vereshchagin solver ERROR: " << solver_return << std::endl;
953 
954  // ########################################################################################
955  // Final comparison of the _resultant_ end-effector's Cartesian accelerations
956  // and the task-specified acceleration constraints
957 
958  // Number of frames on the robot = ns + 1
959  std::vector<Twist> xDotdot(ns + 1);
960  // This solver's function returns Cartesian accelerations of links in robot base coordinates
961  vereshchaginSolver.getTransformedLinkAcceleration(xDotdot);
962  CPPUNIT_ASSERT(Equal(beta_energy(0), xDotdot[ns].vel(0), eps));
963  CPPUNIT_ASSERT(Equal(beta_energy(1), xDotdot[ns].vel(1), eps));
964  CPPUNIT_ASSERT(Equal(beta_energy(2), xDotdot[ns].vel(2), eps));
965  CPPUNIT_ASSERT(Equal(beta_energy(5), xDotdot[ns].rot(2), eps));
966 
967  // Additional getters for the intermediate solver's outputs: Useful for state- simulation and estimation purposes
968  // Magnitude of the constraint forces acting on the end-effector: Lagrange Multiplier
969  Eigen::VectorXd nu(number_of_constraints);
970  vereshchaginSolver.getContraintForceMagnitude(nu);
971  CPPUNIT_ASSERT(Equal(nu(0), 669693.30355, eps));
972  CPPUNIT_ASSERT(Equal(nu(1), 5930.60826, eps));
973  CPPUNIT_ASSERT(Equal(nu(2), -639.5238, eps));
974  CPPUNIT_ASSERT(Equal(nu(3), 0.000, eps)); // constraint was not active in the task specification
975  CPPUNIT_ASSERT(Equal(nu(4), 0.000, eps)); // constraint was not active in the task specification
976  CPPUNIT_ASSERT(Equal(nu(5), 573.90485, eps));
977 
978  // Total torque acting on each joint
979  JntArray total_tau(nj);
980  vereshchaginSolver.getTotalTorque(total_tau);
981  CPPUNIT_ASSERT(Equal(total_tau(0), 2013.3541, eps));
982  CPPUNIT_ASSERT(Equal(total_tau(1), -6073.4999, eps));
983  CPPUNIT_ASSERT(Equal(total_tau(2), 2227.4487, eps));
984  CPPUNIT_ASSERT(Equal(total_tau(3), 56.87456, eps));
985  CPPUNIT_ASSERT(Equal(total_tau(4), -11.3789, eps));
986  CPPUNIT_ASSERT(Equal(total_tau(5), -6.05957, eps));
987  CPPUNIT_ASSERT(Equal(total_tau(6), 569.0776, eps));
988 
989  // ########################################################################################
990  // Vereshchagin solver test 2
991  // ########################################################################################
992  Vector constrainXLinear(1.0, 0.0, 0.0);
993  Vector constrainXAngular(0.0, 0.0, 0.0);
994  Vector constrainYLinear(0.0, 0.0, 0.0);
995  Vector constrainYAngular(0.0, 0.0, 0.0);
996  // Vector constrainZLinear(0.0, 0.0, 0.0);
997  //Vector constrainZAngular(0.0, 0.0, 0.0);
998  Twist constraintForcesX(constrainXLinear, constrainXAngular);
999  Twist constraintForcesY(constrainYLinear, constrainYAngular);
1000  //Twist constraintForcesZ(constrainZLinear, constrainZAngular);
1001  Jacobian alpha(1);
1002  //alpha.setColumn(0, constraintForcesX);
1003  alpha.setColumn(0, constraintForcesX);
1004  //alpha.setColumn(0, constraintForcesZ);
1005 
1006  //Acceleration energy at the end-effector
1007  JntArray betha(1); //set to zero
1008  betha(0) = 0.0;
1009  //betha(1) = 0.0;
1010  //betha(2) = 0.0;
1011 
1012  //arm root acceleration
1013  Vector linearAcc(0.0, 10, 0.0); //gravitational acceleration along Y
1014  Vector angularAcc(0.0, 0.0, 0.0);
1015  Twist twist1(linearAcc, angularAcc);
1016 
1017  //external forces on the arm
1018  Vector externalForce1(0.0, 0.0, 0.0);
1019  Vector externalTorque1(0.0, 0.0, 0.0);
1020  Vector externalForce2(0.0, 0.0, 0.0);
1021  Vector externalTorque2(0.0, 0.0, 0.0);
1022  Wrench externalNetForce1(externalForce1, externalTorque1);
1023  Wrench externalNetForce2(externalForce2, externalTorque2);
1024  Wrenches externalNetForce;
1025  externalNetForce.push_back(externalNetForce1);
1026  externalNetForce.push_back(externalNetForce2);
1027  //~Definition of constraints and external disturbances
1028  //-------------------------------------------------------------------------------------//
1029 
1030 
1031  //Definition of solver and initial configuration
1032  //-------------------------------------------------------------------------------------//
1033  int numberOfConstraints = 1;
1034  ChainHdSolver_Vereshchagin constraintSolver(chaindyn, twist1, numberOfConstraints);
1035 
1036  //These arrays of joint values contain actual and desired values
1037  //actual is generated by a solver and integrator
1038  //desired is given by an interpolator
1039  //error is the difference between desired-actual
1040  //in this test only the actual values are printed.
1041  const int k = 1;
1042  JntArray jointPoses[k];
1043  JntArray jointRates[k];
1044  JntArray jointAccelerations[k];
1045  JntArray jointFFTorques[k];
1046  JntArray jointConstraintTorques[k];
1047  for (int i = 0; i < k; i++)
1048  {
1049  JntArray jointValues(chaindyn.getNrOfJoints());
1050  jointPoses[i] = jointValues;
1051  jointRates[i] = jointValues;
1052  jointAccelerations[i] = jointValues;
1053  jointFFTorques[i] = jointValues;
1054  jointConstraintTorques[i] = jointValues;
1055  }
1056 
1057  // Initial arm position configuration/constraint
1058  JntArray jointInitialPose(chaindyn.getNrOfJoints());
1059  jointInitialPose(0) = 0.0; // initial joint0 pose
1060  jointInitialPose(1) = PI/6.0; //initial joint1 pose, negative in clockwise
1061  //j0=0.0, j1=pi/6.0 correspond to x = 0.2, y = -0.7464
1062  //j0=2*pi/3.0, j1=pi/4.0 correspond to x = 0.44992, y = 0.58636
1063 
1064  //actual
1065  jointPoses[0](0) = jointInitialPose(0);
1066  jointPoses[0](1) = jointInitialPose(1);
1067 
1068  //~Definition of solver and initial configuration
1069  //-------------------------------------------------------------------------------------//
1070 
1071 
1072  //Definition of process main loop
1073  //-------------------------------------------------------------------------------------//
1074  //Time required to complete the task move(frameinitialPose, framefinalPose)
1075  double taskTimeConstant = 0.1;
1076  double simulationTime = 1*taskTimeConstant;
1077  double timeDelta = 0.01;
1078 
1079  const std::string msg = "Assertion failed, check matrix and array sizes";
1080 
1081  for (double t = 0.0; t <=simulationTime; t = t + timeDelta)
1082  {
1083  CPPUNIT_ASSERT_EQUAL((int)SolverI::E_NOERROR, constraintSolver.CartToJnt(jointPoses[0], jointRates[0], jointAccelerations[0], alpha, betha, externalNetForce, jointFFTorques[0], jointConstraintTorques[0]));
1084 
1085  //Integration(robot joint values for rates and poses; actual) at the given "instanteneous" interval for joint position and velocity.
1086  jointRates[0](0) = jointRates[0](0) + jointAccelerations[0](0) * timeDelta; //Euler Forward
1087  jointPoses[0](0) = jointPoses[0](0) + (jointRates[0](0) - jointAccelerations[0](0) * timeDelta / 2.0) * timeDelta; //Trapezoidal rule
1088  jointRates[0](1) = jointRates[0](1) + jointAccelerations[0](1) * timeDelta; //Euler Forward
1089  jointPoses[0](1) = jointPoses[0](1) + (jointRates[0](1) - jointAccelerations[0](1) * timeDelta / 2.0) * timeDelta;
1090  jointFFTorques[0] = jointConstraintTorques[0];
1091  //printf("time, j0_pose, j1_pose, j0_rate, j1_rate, j0_acc, j1_acc, j0_constraintTau, j1_constraintTau \n");
1092  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));
1093  }
1094 }
1095 
1096 void SolverTest::FkPosVectTest()
1097 {
1098  ChainFkSolverPos_recursive fksolver1(chain1);
1099  std::vector<Frame> v_out(chain1.getNrOfSegments());
1100 
1101  JntArray q(chain1.getNrOfJoints());
1102  JntArray qdot(chain1.getNrOfJoints());
1103 
1104  for(unsigned int i=0; i<chain1.getNrOfJoints(); i++)
1105  {
1106  random(q(i));
1107  random(qdot(i));
1108  }
1109  Frame f_out;
1110  fksolver1.JntToCart(q,v_out);
1111  fksolver1.JntToCart(q,f_out);
1112 
1113  CPPUNIT_ASSERT(Equal(v_out[chain1.getNrOfSegments()-1],f_out,1e-5));
1114 }
1115 
1116 void SolverTest::FkVelVectTest()
1117 {
1118  ChainFkSolverVel_recursive fksolver1(chain1);
1119  std::vector<FrameVel> v_out(chain1.getNrOfSegments());
1120 
1121  JntArray q(chain1.getNrOfJoints());
1122  JntArray qdot(chain1.getNrOfJoints());
1123 
1124  for(unsigned int i=0; i<chain1.getNrOfJoints(); i++)
1125  {
1126  random(q(i));
1127  random(qdot(i));
1128  }
1129  JntArrayVel qvel(q,qdot);
1130  FrameVel f_out;
1131  fksolver1.JntToCart(qvel,v_out);
1132  fksolver1.JntToCart(qvel,f_out);
1133 
1134  CPPUNIT_ASSERT(Equal(v_out[chain1.getNrOfSegments()-1],f_out,1e-5));
1135 }
1136 
1137 void SolverTest::FdSolverDevelopmentTest()
1138 {
1139  int ret;
1140  double eps=1.e-3;
1141 
1142  std::cout<<"KDL FD Solver Development Test for Motoman SIA10"<<std::endl;
1143 
1144  // NOTE: This is prototype code for the KDL forward dynamics solver class
1145  // based on the Recurse Newton Euler Method: ChainFdSolver_RNE
1146 
1147  // Dynamics Solver
1148  Vector gravity(0.0, 0.0, -9.81); // base frame
1149  ChainDynParam DynSolver = KDL::ChainDynParam(motomansia10dyn, gravity);
1150 
1151  unsigned int nj = motomansia10dyn.getNrOfJoints();
1152  unsigned int ns = motomansia10dyn.getNrOfSegments();
1153 
1154  // Joint position, velocity, and acceleration
1155  JntArray q(nj);
1156  JntArray qd(nj);
1157  JntArray qdd(nj);
1158 
1159  // random
1160  q(0) = 0.2;
1161  q(1) = 0.6;
1162  q(2) = 1.;
1163  q(3) = 0.5;
1164  q(4) = -1.4;
1165  q(5) = 0.3;
1166  q(6) = -0.8;
1167 
1168  qd(0) = 1.;
1169  qd(1) = -2.;
1170  qd(2) = 3.;
1171  qd(3) = -4.;
1172  qd(4) = 5.;
1173  qd(5) = -6.;
1174  qd(6) = 7.;
1175 
1176  // Validate FK
1177  ChainFkSolverPos_recursive fksolver(motomansia10dyn);
1178  Frame f_out;
1179  fksolver.JntToCart(q,f_out);
1180  CPPUNIT_ASSERT(Equal(-0.547, f_out.p(0), eps));
1181  CPPUNIT_ASSERT(Equal(-0.301, f_out.p(1), eps));
1182  CPPUNIT_ASSERT(Equal(0.924, f_out.p(2), eps));
1183  CPPUNIT_ASSERT(Equal(0.503, f_out.M(0,0), eps));
1184  CPPUNIT_ASSERT(Equal(0.286, f_out.M(0,1), eps));
1185  CPPUNIT_ASSERT(Equal(-0.816, f_out.M(0,2), eps));
1186  CPPUNIT_ASSERT(Equal(-0.859, f_out.M(1,0), eps));
1187  CPPUNIT_ASSERT(Equal(0.269, f_out.M(1,1), eps));
1188  CPPUNIT_ASSERT(Equal(-0.436, f_out.M(1,2), eps));
1189  CPPUNIT_ASSERT(Equal(0.095, f_out.M(2,0), eps));
1190  CPPUNIT_ASSERT(Equal(0.920, f_out.M(2,1), eps));
1191  CPPUNIT_ASSERT(Equal(0.381, f_out.M(2,2), eps));
1192 
1193  // Validate Jacobian
1194  ChainJntToJacSolver jacsolver(motomansia10dyn);
1195  Jacobian jac(nj);
1196  jacsolver.JntToJac(q, jac);
1197  double Jac[6][7] =
1198  {{0.301,-0.553,0.185,0.019,0.007,-0.086,0.},
1199  {-0.547,-0.112,-0.139,-0.376,-0.037,0.063,0.},
1200  {0.,-0.596,0.105,-0.342,-0.026,-0.113,0.},
1201  {0.,0.199,-0.553,0.788,-0.615,0.162,-0.816},
1202  {0.,-0.980,-0.112,-0.392,-0.536,-0.803,-0.436},
1203  {1.,0.,0.825,0.475,0.578,-0.573,0.381}};
1204  for ( unsigned int i=0; i<6; i++ ) {
1205  for ( unsigned int j=0; j<nj; j++ ) {
1206  CPPUNIT_ASSERT(Equal(jac(i,j), Jac[i][j], eps));
1207  }
1208  }
1209 
1210  // Return values
1211  JntArray taugrav(nj);
1212  JntArray taucor(nj);
1213  JntSpaceInertiaMatrix H(nj), Heff(nj);
1214 
1215  // Compute Dynamics (torque in N-m)
1216  ret = DynSolver.JntToGravity(q, taugrav);
1217  if (ret < 0) std::cout << "KDL: inverse dynamics ERROR: " << ret << std::endl;
1218  CPPUNIT_ASSERT(Equal(0.000, taugrav(0), eps));
1219  CPPUNIT_ASSERT(Equal(-36.672, taugrav(1), eps));
1220  CPPUNIT_ASSERT(Equal(4.315, taugrav(2), eps));
1221  CPPUNIT_ASSERT(Equal(-11.205, taugrav(3), eps));
1222  CPPUNIT_ASSERT(Equal(0.757, taugrav(4), eps));
1223  CPPUNIT_ASSERT(Equal(1.780, taugrav(5), eps));
1224  CPPUNIT_ASSERT(Equal(0.000, taugrav(6), eps));
1225 
1226  ret = DynSolver.JntToCoriolis(q, qd, taucor);
1227  if (ret < 0) std::cout << "KDL: inverse dynamics ERROR: " << ret << std::endl;
1228  CPPUNIT_ASSERT(Equal(-15.523, taucor(0), eps));
1229  CPPUNIT_ASSERT(Equal(24.250, taucor(1), eps));
1230  CPPUNIT_ASSERT(Equal(-6.862, taucor(2), eps));
1231  CPPUNIT_ASSERT(Equal(6.303, taucor(3), eps));
1232  CPPUNIT_ASSERT(Equal(0.110, taucor(4), eps));
1233  CPPUNIT_ASSERT(Equal(-4.898, taucor(5), eps));
1234  CPPUNIT_ASSERT(Equal(-0.249, taucor(6), eps));
1235 
1236  ret = DynSolver.JntToMass(q, H);
1237  if (ret < 0) std::cout << "KDL: inverse dynamics ERROR: " << ret << std::endl;
1238  double Hexp[7][7] =
1239  {{6.8687,-0.4333,0.4599,0.6892,0.0638,-0.0054,0.0381},
1240  {-0.4333,8.8324,-0.5922,0.7905,0.0003,-0.0242,0.0265},
1241  {0.4599,-0.5922,3.3496,-0.0253,0.1150,-0.0243,0.0814},
1242  {0.6892,0.7905,-0.0253,3.9623,-0.0201,0.0087,-0.0291},
1243  {0.0638,0.0003,0.1150,-0.0201,1.1234,0.0029,0.0955},
1244  {-0.0054,-0.0242,-0.0243,0.0087,0.0029,1.1425,0},
1245  {0.0381,0.0265,0.0814,-0.0291,0.0955,0,1.1000}};
1246  for ( unsigned int i=0; i<nj; i++ ) {
1247  for ( unsigned int j=0; j<nj; j++ ) {
1248  CPPUNIT_ASSERT(Equal(H(i,j), Hexp[i][j], eps));
1249  }
1250  }
1251 
1252  // Inverse Dynamics:
1253  // T = H * qdd + Tcor + Tgrav - J^T * Fext
1254  // Forward Dynamics
1255  // 1. Call JntToMass from ChainDynParam to get H
1256  // 2. Call ID with qdd=0 to get T=Tcor+Tgrav+J^T*Fext
1257  // 3. Calculate qdd = H^-1 * T
1258  KDL::ChainIdSolver_RNE IdSolver = KDL::ChainIdSolver_RNE(motomansia10dyn, gravity);
1259 
1260  // In tool coordinates
1261  Vector f(10,-20,30) ;
1262  Vector n(3,-4,5) ;
1263  Wrench f_tool(f,n);
1264  // In local link coordinates
1265  Wrenches f_ext(ns);
1266  for(unsigned int i=0;i<ns;i++){
1267  SetToZero(f_ext[i]);
1268  }
1269  f_ext[ns-1]=f_tool;
1270 
1271  JntArray Tnoninertial(nj);
1272  JntArray jntarraynull(nj);
1273  SetToZero(jntarraynull);
1274  IdSolver.CartToJnt(q, qd, jntarraynull, f_ext, Tnoninertial);
1275  CPPUNIT_ASSERT(Equal(-21.252, Tnoninertial(0), eps));
1276  CPPUNIT_ASSERT(Equal(-37.933, Tnoninertial(1), eps));
1277  CPPUNIT_ASSERT(Equal(-2.497, Tnoninertial(2), eps));
1278  CPPUNIT_ASSERT(Equal(-15.289, Tnoninertial(3), eps));
1279  CPPUNIT_ASSERT(Equal(-4.646, Tnoninertial(4), eps));
1280  CPPUNIT_ASSERT(Equal(-9.201, Tnoninertial(5), eps));
1281  CPPUNIT_ASSERT(Equal(-5.249, Tnoninertial(6), eps));
1282 
1283  // get acceleration using inverse symmetric matrix times vector
1284  Eigen::MatrixXd H_eig(nj,nj), L(nj,nj);
1285  Eigen::VectorXd Tnon_eig(nj), D(nj), r(nj), acc_eig(nj);
1286  for(unsigned int i=0;i<nj;i++){
1287  Tnon_eig(i) = -Tnoninertial(i);
1288  for(unsigned int j=0;j<nj;j++){
1289  H_eig(i,j) = H(i,j);
1290  }
1291  }
1292  ldl_solver_eigen(H_eig, Tnon_eig, L, D, r, acc_eig);
1293  for(unsigned int i=0;i<nj;i++){
1294  qdd(i) = acc_eig(i);
1295  }
1296  CPPUNIT_ASSERT(Equal(2.998, qdd(0), eps));
1297  CPPUNIT_ASSERT(Equal(4.289, qdd(1), eps));
1298  CPPUNIT_ASSERT(Equal(0.946, qdd(2), eps));
1299  CPPUNIT_ASSERT(Equal(2.518, qdd(3), eps));
1300  CPPUNIT_ASSERT(Equal(3.530, qdd(4), eps));
1301  CPPUNIT_ASSERT(Equal(8.150, qdd(5), eps));
1302  CPPUNIT_ASSERT(Equal(4.254, qdd(6), eps));
1303 }
1304 
1305 void SolverTest::FdSolverConsistencyTest()
1306 {
1307  int ret;
1308  double eps=1.e-3;
1309 
1310  std::cout<<"KDL FD Solver Consistency Test for Motoman SIA10"<<std::endl;
1311 
1312  // NOTE: Compute the forward and inverse dynamics and test for consistency
1313 
1314  // Forward Dynamics Solver
1315  Vector gravity(0.0, 0.0, -9.81); // base frame
1316  KDL::ChainFdSolver_RNE FdSolver = KDL::ChainFdSolver_RNE(motomansia10dyn, gravity);
1317 
1318  unsigned int nj = motomansia10dyn.getNrOfJoints();
1319  unsigned int ns = motomansia10dyn.getNrOfSegments();
1320 
1321  // Joint position, velocity, and acceleration
1322  KDL::JntArray q(nj);
1323  KDL::JntArray qd(nj);
1324  KDL::JntArray qdd(nj);
1325  KDL::JntArray tau(nj);
1326 
1327  // random
1328  q(0) = 0.2;
1329  q(1) = 0.6;
1330  q(2) = 1.;
1331  q(3) = 0.5;
1332  q(4) = -1.4;
1333  q(5) = 0.3;
1334  q(6) = -0.8;
1335 
1336  qd(0) = 1.;
1337  qd(1) = -2.;
1338  qd(2) = 3.;
1339  qd(3) = -4.;
1340  qd(4) = 5.;
1341  qd(5) = -6.;
1342  qd(6) = 7.;
1343 
1344  // actuator torques
1345  tau(0) = 50.;
1346  tau(1) = -20.;
1347  tau(2) = 10.;
1348  tau(3) = 40.;
1349  tau(4) = -60.;
1350  tau(5) = 15.;
1351  tau(6) = -10.;
1352 
1353  KDL::Vector f(10,-20,30) ;
1354  KDL::Vector n(3,-4,5) ;
1355  KDL::Wrench f_tool(f,n);
1356  // In local link coordinates
1357  KDL::Wrenches f_ext(ns);
1358  for(unsigned int i=0;i<ns;i++){
1359  SetToZero(f_ext[i]);
1360  }
1361  f_ext[ns-1]=f_tool;
1362 
1363  // Call FD function
1364  ret = FdSolver.CartToJnt(q, qd, tau, f_ext, qdd);
1365  if (ret < 0) std::cout << "KDL: forward dynamics ERROR: " << ret << std::endl;
1366  CPPUNIT_ASSERT(Equal(9.486, qdd(0), eps));
1367  CPPUNIT_ASSERT(Equal(1.830, qdd(1), eps));
1368  CPPUNIT_ASSERT(Equal(4.726, qdd(2), eps));
1369  CPPUNIT_ASSERT(Equal(11.665, qdd(3), eps));
1370  CPPUNIT_ASSERT(Equal(-50.108, qdd(4), eps));
1371  CPPUNIT_ASSERT(Equal(21.403, qdd(5), eps));
1372  CPPUNIT_ASSERT(Equal(-0.381, qdd(6), eps));
1373 
1374  // Check against ID solver for consistency
1375  KDL::ChainIdSolver_RNE IdSolver = KDL::ChainIdSolver_RNE(motomansia10dyn, gravity);
1376  KDL::JntArray torque(nj);
1377  IdSolver.CartToJnt(q, qd, qdd, f_ext, torque);
1378  for ( unsigned int i=0; i<nj; i++ )
1379  {
1380  CPPUNIT_ASSERT(Equal(torque(i), tau(i), eps));
1381  }
1382 
1383  return;
1384 }
1385 
1386 void SolverTest::LDLdecompTest()
1387 {
1388  std::cout<<"LDL Solver Test"<<std::endl;
1389  double eps=1.e-6;
1390 
1391  // Given A and b, solve Ax=b for x, where A is a symmetric real matrix
1392  // https://en.wikipedia.org/wiki/Cholesky_decomposition
1393  Eigen::MatrixXd A(3,3), Aout(3,3);
1394  Eigen::VectorXd b(3);
1395  Eigen::MatrixXd L(3,3), Lout(3,3);
1396  Eigen::VectorXd d(3), dout(3);
1397  Eigen::VectorXd x(3), xout(3);
1398  Eigen::VectorXd r(3); // temp variable used internally by ldl solver
1399  Eigen::MatrixXd Dout(3,3); // diagonal matrix
1400 
1401  // Given
1402  A << 4, 12, -16,
1403  12, 37, -43,
1404  -16, -43, 98;
1405  b << 28, 117, 98;
1406  // Results to verify
1407  L << 1, 0, 0,
1408  3, 1, 0,
1409  -4, 5, 1;
1410  d << 4, 1, 9;
1411  x << 3, 8, 5;
1412 
1413  ldl_solver_eigen(A, b, Lout, dout, r, xout);
1414 
1415  for(int i=0;i<3;i++){
1416  for(int j=0;j<3;j++){
1417  CPPUNIT_ASSERT(Equal(L(i,j), Lout(i,j), eps));
1418  }
1419  }
1420 
1421  Dout.setZero();
1422  for(int i=0;i<3;i++){
1423  Dout(i,i) = dout(i);
1424  }
1425 
1426  // Verify solution for x
1427  for(int i=0;i<3;i++){
1428  CPPUNIT_ASSERT(Equal(xout(i), x(i), eps));
1429  }
1430 
1431  // Test reconstruction of A from LDL^T decomposition
1432  Aout = Lout * Dout * Lout.transpose();
1433  for(int i=0;i<3;i++){
1434  for(int j=0;j<3;j++){
1435  CPPUNIT_ASSERT(Equal(A(i,j), Aout(i,j), eps));
1436  }
1437  }
1438 
1439  return;
1440 }
1441 
1442 void SolverTest::FdAndVereshchaginSolversConsistencyTest()
1443 {
1444  int ret;
1445  double eps=1.e-3;
1446  ChainFkSolverPos_recursive fksolverpos(kukaLWR);
1447  Frame end_effector_pose;
1448 
1453  std::cout << "KDL FD (inverse-inertia version) and Vereshchagin Solvers Consistency Test for KUKA LWR 4 robot" << std::endl;
1454 
1455  // ########################################################################################
1456  // Experiment (common state) setup
1457  unsigned int nj = kukaLWR.getNrOfJoints();
1458  unsigned int ns = kukaLWR.getNrOfSegments();
1459 
1460  // Necessary test for the used robot model: KDL's implementation of the Vereshchagin solver
1461  // can only work with the robot chains that have equal number of joints and segments
1462  CPPUNIT_ASSERT(Equal(nj, ns));
1463 
1464  // Joint position, velocity, acceleration and torques
1465  KDL::JntArray q(nj);
1466  KDL::JntArray qd(nj);
1467  KDL::JntArray qdd(nj);
1468  KDL::JntArray ff_tau(nj);
1469 
1470  // random input state
1471  q(0) = 1.0;
1472  q(1) = 0.0;
1473  q(2) = 0.0;
1474  q(3) = -1.57;
1475  q(4) = 0.0;
1476  q(5) = 1.57;
1477  q(6) = -0.8;
1478 
1479  qd(0) = 1.0;
1480  qd(1) = -2.0;
1481  qd(2) = 3.0;
1482  qd(3) = -4.0;
1483  qd(4) = 5.0;
1484  qd(5) = -6.0;
1485  qd(6) = 7.0;
1486 
1487  // actuator torques
1488  ff_tau(0) = 50.0;
1489  ff_tau(1) = -20.0;
1490  ff_tau(2) = 10.0;
1491  ff_tau(3) = 40.0;
1492  ff_tau(4) = -60.0;
1493  ff_tau(5) = 15.0;
1494  ff_tau(6) = -10.0;
1495 
1496  // External Wrench acting on the end-effector, expressed in local link coordinates
1497  KDL::Vector f(10.0, -20.0, 30.0);
1498  KDL::Vector n(3.0, -4.0, 5.0);
1499  KDL::Wrench f_tool(f, n);
1500 
1501  KDL::Wrenches f_ext(ns);
1502  for(unsigned int i=0 ;i<ns; i++)
1503  SetToZero(f_ext[i]);
1504  f_ext[ns - 1] = f_tool;
1505 
1506  // ########################################################################################
1507  // Forward Dynamics Solver (inverse-inertia version)
1508  Vector gravity(0.0, 0.0, -9.81); // base frame (Robot base mounted on an even surface)
1509  KDL::ChainFdSolver_RNE FdSolver = KDL::ChainFdSolver_RNE(kukaLWR, gravity);
1510 
1511  // Call FD function
1512  ret = FdSolver.CartToJnt(q, qd, ff_tau, f_ext, qdd);
1513  if (ret < 0)
1514  std::cout << "KDL: forward dynamics ERROR: " << ret << std::endl;
1515 
1516  // #########################################################################################
1517  // Vereshchagin Hybrid Dynamics solver
1518  // When the Cartesian Acceleration Constraints are deactivated, the computations performed
1519  // in the Vereshchagin solver are completely the same as the computations performed in
1520  // the well-known FD Articulated Body Algorithm (ABA) developed by Featherstone
1521 
1522  // Constraint Unit forces at the end-effector. Set to zero to deactivate all constraints
1523  int numberOfConstraints = 6;
1524  Jacobian alpha(numberOfConstraints);
1525  KDL::SetToZero(alpha);
1526 
1527  // Acceleration energy at the end-effector. Set to zero since all constraints are deactivated
1528  JntArray beta(numberOfConstraints); //set to zero
1529  KDL::SetToZero(beta);
1530 
1531  // Arm root acceleration (robot's base mounted on an even surface)
1532  // Note: Vereshchagin solver takes root acc. with opposite sign comparead to the above FD and RNE solvers
1533  Vector linearAcc(0.0, 0.0, 9.81); Vector angularAcc(0.0, 0.0, 0.0);
1534  Twist root_Acc(linearAcc, angularAcc);
1535 
1536  // Torques felt in robot's joints due to constraint forces acting on the end-effector
1537  JntArray constraint_tau(nj); // In this test, all elements of this array should result to zero
1538  JntArray q_dd_Ver(nj); // Resultant joint accelerations
1539 
1540  // External Wrench acting on the end-effector, this time expressed in base link coordinates
1541  // Vereshchagin solver expects that external wrenches are expressed w.r.t. robot's base frame
1542  fksolverpos.JntToCart(q, end_effector_pose, kukaLWR.getNrOfSegments());
1543  f_ext[ns - 1] = end_effector_pose.M * f_tool;
1544 
1545  ChainHdSolver_Vereshchagin constraintSolver(kukaLWR, root_Acc, numberOfConstraints);
1546  ret = constraintSolver.CartToJnt(q, qd, q_dd_Ver, alpha, beta, f_ext, ff_tau, constraint_tau);
1547  if (ret < 0)
1548  std::cout << "KDL: Vereshchagin solver ERROR: " << ret << std::endl;
1549 
1550  // ########################################################################################
1551  // Final comparison
1552  CPPUNIT_ASSERT(Equal(q_dd_Ver(0), qdd(0), eps));
1553  CPPUNIT_ASSERT(Equal(q_dd_Ver(1), qdd(1), eps));
1554  CPPUNIT_ASSERT(Equal(q_dd_Ver(2), qdd(2), eps));
1555  CPPUNIT_ASSERT(Equal(q_dd_Ver(3), qdd(3), eps));
1556  CPPUNIT_ASSERT(Equal(q_dd_Ver(4), qdd(4), eps));
1557  CPPUNIT_ASSERT(Equal(q_dd_Ver(5), qdd(5), eps));
1558  CPPUNIT_ASSERT(Equal(q_dd_Ver(6), qdd(6), eps));
1559 
1560  return;
1561 }
1562 
1563 void SolverTest::ExternalWrenchEstimatorTest()
1564 {
1572  std::cout << "KDL External Wrench Estimator Test" << std::endl;
1573 
1581  double eps_wrench = 0.5, eps_torque = 0.3;
1582  int ret;
1583  unsigned int nj = kukaLWR.getNrOfJoints();
1584  unsigned int ns = kukaLWR.getNrOfSegments();
1585  CPPUNIT_ASSERT(Equal(nj, ns)); // Current implementation of the Vereshchagin solver can only work with chains that have equal number of joints and segments
1586 
1587  // Initialize state and control variables
1588  JntArray q(nj); // Current joint position
1589  JntArray qd(nj); // Current joint velocity
1590  JntArray qdd(nj); // Resultant joint accelerations
1591  JntArrayVel jnt_position_velocity(nj); // variable necessary for the FK vel solver
1592  JntArray jnt_array_zero(nj); // Zero joint input for RNE
1593  JntArray command_torque(nj); // Control torque to actuate the robot
1594  JntArray constraint_tau(nj); // It will result in zero in Vereshchagin for this test
1595  JntArray gravity_torque(nj); // Gravity torque computed by RNE
1596  JntArray ext_torque_reference(nj); // Ground-truth joint torques due to the external force applied on the end-effector
1597  JntArray ext_torque_estimated(nj); // Estimated joint torques
1598  Wrenches f_ext_base(ns); // External Wrenches acting on the end-effector, expressed in base-link coordinates
1599  Wrenches f_ext_zero(ns); // Zero Wrenches
1600  Wrench f_tool_estimated; // External Wrenches estimated by the momentum-observer
1601  Frame end_effector_pose;
1602  Frame desired_end_eff_pose;
1603  Jacobian jacobian_end_eff(nj);
1604  FrameVel end_eff_twist;
1605  FrameVel desired_end_eff_twist;
1606  Eigen::Matrix<double, 6, 1> end_eff_force; // variable necessary for the control
1607  Eigen::Matrix<double, 6, 1> end_eff_pos_error; // variable necessary for the control
1608  Eigen::Matrix<double, 6, 1> end_eff_vel_error; // variable necessary for the control
1609 
1610  // Arm root acceleration (robot's base mounted on an even surface)
1611  Vector linearAcc(0.0, 0.0, -9.81); Vector angularAcc(0.0, 0.0, 0.0);
1612 
1613  // Initialize kinematics solvers
1614  ChainFkSolverPos_recursive fksolverpos(kukaLWR);
1615  ChainFkSolverVel_recursive fksolvervel(kukaLWR);
1616  ChainJntToJacSolver jacobian_solver(kukaLWR);
1617 
1618  // RNE ID solver for control purposes
1619  KDL::ChainIdSolver_RNE IdSolver(kukaLWR, linearAcc);
1620 
1621  // Vereshchagin Hybrid Dynamics solver for simulation purposes
1622  int numberOfConstraints = 6;
1623  Jacobian alpha(numberOfConstraints); // Constraint Unit forces at the end-effector
1624  JntArray beta(numberOfConstraints); // Acceleration energy at the end-effector
1625  KDL::SetToZero(alpha); // Set to zero to deactivate all constraints
1626  KDL::SetToZero(beta); // Set to zero to deactivate all constraints
1627  Twist vereshchagin_root_Acc(-linearAcc, angularAcc); // Note: Vereshchagin solver takes root acc. with opposite sign comparead to the above FD and RNE solvers
1628  ChainHdSolver_Vereshchagin constraintSolver(kukaLWR, vereshchagin_root_Acc, numberOfConstraints);
1629 
1630  // External Wrench Estimator
1631  double sample_frequency = 1000.0; // Hz
1632  double estimation_gain = 45.0;
1633  double filter_constant = 0.5;
1634  ChainExternalWrenchEstimator extwrench_estimator(kukaLWR, linearAcc, sample_frequency, estimation_gain, filter_constant);
1635 
1636  // Prepare test cases
1637  std::vector<KDL::JntArray> jnt_pos;
1638  std::vector<KDL::Wrench> wrench_reference;
1639 
1640  // Initialize random generator
1641  std::random_device rd; //Will be used to obtain a seed for the random number engine
1642  std::mt19937 gen(rd()); //Standard mersenne_twister_engine seeded with rd()
1643  std::uniform_real_distribution<> dis_force(-15.0, 15.0);
1644  std::uniform_real_distribution<> dis_moment(-0.9, 0.9);
1645  std::uniform_real_distribution<> dis_jnt_vel(-0.5, 0.5);
1646 
1647  // Set first test case
1648  q(0) = 1.0;
1649  q(1) = 0.0;
1650  q(2) = 0.0;
1651  q(3) = 4.71;
1652  q(4) = 0.0;
1653  q(5) = 1.57;
1654  q(6) = 5.48;
1655  jnt_pos.push_back(q);
1656  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
1657 
1658  // Set second test case
1659  q(0) = 2.96;
1660  q(1) = 1.02;
1661  q(2) = 6.15;
1662  q(3) = 1.61;
1663  q(4) = 0.22;
1664  q(5) = 0.17;
1665  q(6) = 0.01;
1666  jnt_pos.push_back(q);
1667  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
1668 
1669  // Set third test case
1670  q(0) = 1.12;
1671  q(1) = 0.66;
1672  q(2) = 6.15;
1673  q(3) = 4.09;
1674  q(4) = 1.64;
1675  q(5) = 0.12;
1676  q(6) = 0.01;
1677  jnt_pos.push_back(q);
1678  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
1679 
1680  // ##########################################################################################
1681  // Control and simulation
1682  // ##########################################################################################
1683 
1684  // Control gains for a simple PD controller
1685  double k_p = 1500.0; // Proportional
1686  double k_d = 300.0; // Derivative
1687 
1688  // Time required to complete the task
1689  double simulationTime = 0.4; // in seconds
1690  double timeDelta = 1.0 / sample_frequency; // unit of seconds
1691 
1692  // Iterate over test cases
1693  for (unsigned int i = 0; i < jnt_pos.size(); i++)
1694  {
1695  // Re-set control and simulation variables
1696  q = jnt_pos[i];
1697  qd(0) = dis_jnt_vel(gen);
1698  qd(1) = dis_jnt_vel(gen);
1699  qd(2) = dis_jnt_vel(gen);
1700  qd(3) = dis_jnt_vel(gen);
1701  qd(4) = dis_jnt_vel(gen);
1702  qd(5) = dis_jnt_vel(gen);
1703  qd(6) = dis_jnt_vel(gen);
1704 
1705  end_eff_force.setZero();
1706  end_eff_pos_error.setZero();
1707  end_eff_vel_error.setZero();
1708  f_ext_base = f_ext_zero;
1709 
1710  // Initialize the estimator
1711  extwrench_estimator.updateInternalDataStructures();
1712  extwrench_estimator.setInitialMomentum(q, qd); // sets the offset for future estimation (momentum calculation)
1713 
1714  // Set the desired Cartesian state
1715  fksolverpos.JntToCart(q, end_effector_pose);
1716  desired_end_eff_pose.p(0) = end_effector_pose.p(0) + 0.02;
1717  desired_end_eff_pose.p(1) = end_effector_pose.p(1) + 0.02;
1718  desired_end_eff_pose.p(2) = end_effector_pose.p(2) + 0.02;
1719  desired_end_eff_twist.p.v(0) = 0.0;
1720  desired_end_eff_twist.p.v(1) = 0.0;
1721  desired_end_eff_twist.p.v(2) = 0.0;
1722 
1723  for (double t = 0.0; t <= simulationTime; t = t + timeDelta)
1724  {
1725  ret = jacobian_solver.JntToJac(q, jacobian_end_eff);
1726  if (ret < 0)
1727  {
1728  std::cout << "Jacobian solver ERROR: " << ret << std::endl;
1729  break;
1730  }
1731 
1732  ret = fksolverpos.JntToCart(q, end_effector_pose);
1733  if (ret < 0)
1734  {
1735  std::cout << "FK pos solver ERROR: " << ret << std::endl;
1736  break;
1737  }
1738 
1739  jnt_position_velocity.q = q;
1740  jnt_position_velocity.qdot = qd;
1741  ret = fksolvervel.JntToCart(jnt_position_velocity, end_eff_twist);
1742  if (ret < 0)
1743  {
1744  std::cout << "FK vel solver ERROR: " << ret << std::endl;
1745  break;
1746  }
1747 
1748  end_eff_pos_error(0) = end_effector_pose.p(0) - desired_end_eff_pose.p(0);
1749  end_eff_pos_error(1) = end_effector_pose.p(1) - desired_end_eff_pose.p(1);
1750  end_eff_pos_error(2) = end_effector_pose.p(2) - desired_end_eff_pose.p(2);
1751 
1752  end_eff_vel_error(0) = end_eff_twist.p.v(0) - desired_end_eff_twist.p.v(0);
1753  end_eff_vel_error(1) = end_eff_twist.p.v(1) - desired_end_eff_twist.p.v(1);
1754  end_eff_vel_error(2) = end_eff_twist.p.v(2) - desired_end_eff_twist.p.v(2);
1755 
1756  end_eff_force = -end_eff_pos_error * k_p - end_eff_vel_error * k_d;
1757 
1758  // Compute gravity joint torques (hide external wrench from this dynamics calculation)
1759  ret = IdSolver.CartToJnt(q, jnt_array_zero, jnt_array_zero, f_ext_zero, gravity_torque);
1760  if (ret < 0)
1761  {
1762  std::cout << "KDL RNE solver ERROR: " << ret << std::endl;
1763  break;
1764  }
1765 
1766  // Compute joint control commands
1767  command_torque.data = jacobian_end_eff.data.transpose() * end_eff_force;
1768  command_torque.data += gravity_torque.data;
1769 
1770  // Start simulating the external force
1771  if (t > 0.2) f_ext_base[ns - 1] = end_effector_pose.M * wrench_reference[i];
1772 
1773  // Compute resultant joint accelerations that simulate robot's behaviour, given the command torques (add external wrench in this dynamics calculation)
1774  ret = constraintSolver.CartToJnt(q, qd, qdd, alpha, beta, f_ext_base, command_torque, constraint_tau);
1775  if (ret < 0)
1776  {
1777  std::cout << "KDL Vereshchagin solver ERROR: " << ret << std::endl;
1778  break;
1779  }
1780 
1781  // State integration: integrate from model accelerations to next joint state (positions and velocities)
1782  qd.data = qd.data + qdd.data * timeDelta; // Euler Forward
1783  q.data = q.data + qd.data * timeDelta; // Symplectic Euler
1784 
1785  // Saturate integrated joint position for full circle crossing
1786  for (unsigned int j = 0; j < nj; j++)
1787  {
1788  q(j) = std::fmod(q(j), 360 * deg2rad);
1789  if (q(j) < 0.0) q(j) += 360 * deg2rad;
1790  }
1791 
1792  // Estimate external wrench
1793  extwrench_estimator.JntToExtWrench(q, qd, command_torque, f_tool_estimated);
1794  }
1795 
1796  // Inverse Force Kinematics
1797  Eigen::Matrix<double, 6, 1> wrench;
1798  wrench(0) = f_ext_base[ns - 1](0);
1799  wrench(1) = f_ext_base[ns - 1](1);
1800  wrench(2) = f_ext_base[ns - 1](2);
1801  wrench(3) = f_ext_base[ns - 1](3);
1802  wrench(4) = f_ext_base[ns - 1](4);
1803  wrench(5) = f_ext_base[ns - 1](5);
1804  ext_torque_reference.data = jacobian_end_eff.data.transpose() * wrench;
1805 
1806  // Get estimated joint torque
1807  extwrench_estimator.getEstimatedJntTorque(ext_torque_estimated);
1808 
1809  // ##################################################################################
1810  // Final comparison
1811  // ##################################################################################
1812  CPPUNIT_ASSERT(Equal(f_tool_estimated(0), wrench_reference[i](0), eps_wrench));
1813  CPPUNIT_ASSERT(Equal(f_tool_estimated(1), wrench_reference[i](1), eps_wrench));
1814  CPPUNIT_ASSERT(Equal(f_tool_estimated(2), wrench_reference[i](2), eps_wrench));
1815  CPPUNIT_ASSERT(Equal(f_tool_estimated(3), wrench_reference[i](3), eps_wrench));
1816  CPPUNIT_ASSERT(Equal(f_tool_estimated(4), wrench_reference[i](4), eps_wrench));
1817  CPPUNIT_ASSERT(Equal(f_tool_estimated(5), wrench_reference[i](5), eps_wrench));
1818 
1819  CPPUNIT_ASSERT(Equal(ext_torque_estimated(0), ext_torque_reference(0), eps_torque));
1820  CPPUNIT_ASSERT(Equal(ext_torque_estimated(1), ext_torque_reference(1), eps_torque));
1821  CPPUNIT_ASSERT(Equal(ext_torque_estimated(2), ext_torque_reference(2), eps_torque));
1822  CPPUNIT_ASSERT(Equal(ext_torque_estimated(3), ext_torque_reference(3), eps_torque));
1823  CPPUNIT_ASSERT(Equal(ext_torque_estimated(4), ext_torque_reference(4), eps_torque));
1824  CPPUNIT_ASSERT(Equal(ext_torque_estimated(5), ext_torque_reference(5), eps_torque));
1825  CPPUNIT_ASSERT(Equal(ext_torque_estimated(6), ext_torque_reference(6), eps_torque));
1826  }
1827 
1828  return;
1829 }
solvertest.hpp
std::vector::resize
T resize(T... args)
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:818
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:1305
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::JntArrayVel::resize
void resize(unsigned int newSize)
Definition: jntarrayvel.cpp:41
SolverTest::FdSolverDevelopmentTest
void FdSolverDevelopmentTest()
Definition: solvertest.cpp:1137
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:106
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:1096
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:1386
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:74
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:198
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:1442
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:1563
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:1116
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 Tue Nov 26 2024 04:35:24 for orocos_kdl by   doxygen 1.8.17