orocos_kdl
jacobiandottest.cpp
Go to the documentation of this file.
1 #include "jacobiandottest.hpp"
2 
3 CPPUNIT_TEST_SUITE_REGISTRATION(JacobianDotTest);
4 
5 using namespace KDL;
6 
7 void JacobianDotTest::setUp(){}
8 void JacobianDotTest::tearDown(){}
9 
10 
11 namespace KDL{
12  static const double L0 = 1.0;
13  static const double L1 = 0.5;
14  static const double L2 = 0.4;
15  static const double L3 = 0;
16  static const double L4 = 0;
17  static const double L5 = 0;
18  Chain d2(){
19  Chain d2;
20  d2.addSegment(Segment(Joint(Joint::RotZ),Frame(Vector(L0,0,0))));
21  d2.addSegment(Segment(Joint(Joint::RotZ),Frame(Vector(L1,0,0))));
22  return d2;
23  }
24  Chain d6(){
25  Chain d6;
26  d6.addSegment(Segment(Joint(Joint::RotZ),Frame(Vector(L0,0,0))));
27  d6.addSegment(Segment(Joint(Joint::RotX),Frame(Vector(L1,0,0))));
28  d6.addSegment(Segment(Joint(Joint::RotX),Frame(Vector(L2,0,0))));
29  d6.addSegment(Segment(Joint(Joint::RotZ),Frame(Vector(L3,0,0))));
30  d6.addSegment(Segment(Joint(Joint::RotX),Frame(Vector(L4,0,0))));
31  d6.addSegment(Segment(Joint(Joint::RotZ),Frame(Vector(L5,0,0))));
32  return d6;
33  }
34  Chain KukaLWR_DHnew(){
35  Chain kukaLWR_DHnew;
36 
37  //joint 0
38  kukaLWR_DHnew.addSegment(Segment(Joint(Joint::None),
39  Frame::DH_Craig1989(0.0, 0.0, 0.31, 0.0)
40  ));
41  //joint 1
42  kukaLWR_DHnew.addSegment(Segment(Joint(Joint::RotZ),
43  Frame::DH_Craig1989(0.0, 1.5707963, 0.0, 0.0),
44  Frame::DH_Craig1989(0.0, 1.5707963, 0.0, 0.0).Inverse()*RigidBodyInertia(2,
45  Vector::Zero(),
46  RotationalInertia(0.0,0.0,0.0115343,0.0,0.0,0.0))));
47 
48  //joint 2
49  kukaLWR_DHnew.addSegment(Segment(Joint(Joint::RotZ),
50  Frame::DH_Craig1989(0.0, -1.5707963, 0.4, 0.0),
51  Frame::DH_Craig1989(0.0, -1.5707963, 0.4, 0.0).Inverse()*RigidBodyInertia(2,
52  Vector(0.0,-0.3120511,-0.0038871),
53  RotationalInertia(-0.5471572,-0.0000302,-0.5423253,0.0,0.0,0.0018828))));
54 
55  //joint 3
56  kukaLWR_DHnew.addSegment(Segment(Joint(Joint::RotZ),
57  Frame::DH_Craig1989(0.0, -1.5707963, 0.0, 0.0),
58  Frame::DH_Craig1989(0.0, -1.5707963, 0.0, 0.0).Inverse()*RigidBodyInertia(2,
59  Vector(0.0,-0.0015515,0.0),
60  RotationalInertia(0.0063507,0.0,0.0107804,0.0,0.0,-0.0005147))));
61 
62  //joint 4
63  kukaLWR_DHnew.addSegment(Segment(Joint(Joint::RotZ),
64  Frame::DH_Craig1989(0.0, 1.5707963, 0.39, 0.0),
65  Frame::DH_Craig1989(0.0, 1.5707963, 0.39, 0.0).Inverse()*RigidBodyInertia(2,
66  Vector(0.0,0.5216809,0.0),
67  RotationalInertia(-1.0436952,0.0,-1.0392780,0.0,0.0,0.0005324))));
68 
69  //joint 5
70  kukaLWR_DHnew.addSegment(Segment(Joint(Joint::RotZ),
71  Frame::DH_Craig1989(0.0, 1.5707963, 0.0, 0.0),
72  Frame::DH_Craig1989(0.0, 1.5707963, 0.0, 0.0).Inverse()*RigidBodyInertia(2,
73  Vector(0.0,0.0119891,0.0),
74  RotationalInertia(0.0036654,0.0,0.0060429,0.0,0.0,0.0004226))));
75 
76  //joint 6
77  kukaLWR_DHnew.addSegment(Segment(Joint(Joint::RotZ),
78  Frame::DH_Craig1989(0.0, -1.5707963, 0.0, 0.0),
79  Frame::DH_Craig1989(0.0, -1.5707963, 0.0, 0.0).Inverse()*RigidBodyInertia(2,
80  Vector(0.0,0.0080787,0.0),
81  RotationalInertia(0.0010431,0.0,0.0036376,0.0,0.0,0.0000101))));
82  //joint 7
83  kukaLWR_DHnew.addSegment(Segment(Joint(Joint::RotZ),
84  Frame::Identity(),
85  RigidBodyInertia(2,
86  Vector::Zero(),
87  RotationalInertia(0.000001,0.0,0.0001203,0.0,0.0,0.0))));
88  return kukaLWR_DHnew;
89  }
90 }
91 
92 
93 void changeRepresentation(Jacobian& J,const Frame& F_bs_ee,const int& representation)
94 {
95  switch(representation)
96  {
97  case ChainJntToJacDotSolver::HYBRID:
98  break;
99  case ChainJntToJacDotSolver::BODYFIXED:
100  // Ref Frame {ee}, Ref Point {ee}
101  J.changeBase(F_bs_ee.M.Inverse());
102  break;
103  case ChainJntToJacDotSolver::INERTIAL:
104  // Ref Frame {bs}, Ref Point {bs}
105  J.changeRefPoint(-F_bs_ee.p);
106  break;
107  }
108 }
109 void Jdot_diff(const Jacobian& J_q,
110  const Jacobian& J_qdt,
111  const double& dt,
112  Jacobian& Jdot)
113 {
114  assert(J_q.columns() == J_qdt.columns());
115  assert(J_q.columns() == Jdot.columns());
116  for(unsigned int l=0;l<6;l++)
117  for(unsigned int c=0;c<J_q.columns();c++)
118  Jdot(l,c) = (J_qdt(l,c) - J_q(l,c))/dt;
119 }
120 
121 Jacobian Jdot_d2_symbolic(const JntArray& q,const JntArray& qdot)
122 {
123  // Returns Jdot for the simple 2DOF arm
124  Jacobian Jdot(q.rows());
125  SetToZero(Jdot);
126  Jdot(0,0) = -L1 * (qdot(0) + qdot(1))*cos(q(0)+q(1))-L0*cos(q(0))*qdot(0);
127  Jdot(0,1) = -L1 * (qdot(0) + qdot(1))*cos(q(0)+q(1));
128  Jdot(1,0) = -L1 * (qdot(0) + qdot(1))*sin(q(0)+q(1))-L0*sin(q(0))*qdot(0);
129  Jdot(1,1) = -L1 * (qdot(0) + qdot(1))*sin(q(0)+q(1));
130  return Jdot;
131 }
132 
133 Jacobian J_d2_symbolic(const JntArray& q,const JntArray& /*qdot*/)
134 {
135  // Returns J for the simple 2DOF arm
136  Jacobian J(q.rows());
137  SetToZero(J);
138  J(0,0) = -L1 * sin(q(0)+q(1))-L0*sin(q(0));
139  J(0,1) = -L1 * sin(q(0)+q(1));
140  J(1,0) = L1 * cos(q(0)+q(1))+L0*cos(q(0));
141  J(1,1) = L1 * cos(q(0)+q(1));
142  J(5,0) = J(5,1) = 1;
143  return J;
144 }
145 
146 JntArray diff(const JntArray& q,const JntArray& qdot,const double& dt)
147 {
148  JntArray q_qdqt(q);
149  for(unsigned int i=0; i<q.rows(); i++)
150  q_qdqt(i) += dt*qdot(i);
151  return q_qdqt;
152 }
153 
154 void random(JntArray& q)
155 {
156  for(unsigned int i=0; i<q.rows(); i++)
157  random(q(i));
158 }
159 
160 double compare_Jdot_Diff_vs_Solver(const Chain& chain,const double& dt,const int& representation,bool verbose)
161 {
162  // This test verifies if the solvers gives approx. the same result as [ J(q+qdot*dot) - J(q) ]/dot
163  JntArray q(chain.getNrOfJoints());
164  JntArray qdot(chain.getNrOfJoints());
165  JntArray q_dqdt(chain.getNrOfJoints());
166 
167 
168  random(q);
169  random(qdot);
170  q_dqdt = diff(q,qdot,dt);
171 
172  ChainJntToJacDotSolver jdot_solver(chain);
173  ChainJntToJacSolver j_solver(chain);
174  ChainFkSolverPos_recursive fk_solver(chain);
175 
176  Frame F_bs_ee_q,F_bs_ee_q_dqdt;
177  Jacobian jac_q(chain.getNrOfJoints()),
178  jac_q_dqdt(chain.getNrOfJoints()),
179  jdot_by_diff(chain.getNrOfJoints());
180 
181  j_solver.JntToJac(q,jac_q);
182  j_solver.JntToJac(q_dqdt,jac_q_dqdt);
183 
184  fk_solver.JntToCart(q,F_bs_ee_q);
185  fk_solver.JntToCart(q_dqdt,F_bs_ee_q_dqdt);
186 
187  changeRepresentation(jac_q,F_bs_ee_q,representation);
188  changeRepresentation(jac_q_dqdt,F_bs_ee_q_dqdt,representation);
189 
190  Jdot_diff(jac_q,jac_q_dqdt,dt,jdot_by_diff);
191 
192  Jacobian jdot_by_solver(chain.getNrOfJoints());
193  jdot_solver.setRepresentation(representation);
194  jdot_solver.JntToJacDot(JntArrayVel(q_dqdt,qdot),jdot_by_solver);
195 
196  Twist jdot_qdot_by_solver;
197  MultiplyJacobian(jdot_by_solver,qdot,jdot_qdot_by_solver);
198 
199  Twist jdot_qdot_by_diff;
200  MultiplyJacobian(jdot_by_diff,qdot,jdot_qdot_by_diff);
201 
202  if(verbose){
203  std::cout << "Jdot diff:\n" << jdot_by_diff << "\n"
204  << "Jdot solver:\n" << jdot_by_solver << "\n"
205  << "Error:\n"
206  << jdot_qdot_by_diff-jdot_qdot_by_solver << "\n"
207  << q << "\n"
208  << qdot <<std::endl;
209  }
210  double err = jdot_qdot_by_diff.vel.Norm() - jdot_qdot_by_solver.vel.Norm()
211  + jdot_qdot_by_diff.rot.Norm() - jdot_qdot_by_solver.rot.Norm();
212  return std::abs(err);
213 }
214 
215 double compare_d2_Jdot_Symbolic_vs_Solver(bool verbose)
216 {
217  Chain chain=d2();
218  JntArray q(chain.getNrOfJoints());
219  JntArray qdot(chain.getNrOfJoints());
220 
221  random(q);
222  random(qdot);
223 
224  ChainJntToJacDotSolver jdot_solver(chain);
225 
226  Jacobian jdot_sym = Jdot_d2_symbolic(q,qdot);
227 
228  Jacobian jdot_by_solver(chain.getNrOfJoints());
229  jdot_solver.JntToJacDot(JntArrayVel(q,qdot),jdot_by_solver);
230 
231  Twist jdot_qdot_by_solver;
232  MultiplyJacobian(jdot_by_solver,qdot,jdot_qdot_by_solver);
233 
234  Twist jdot_qdot_sym;
235  MultiplyJacobian(jdot_sym,qdot,jdot_qdot_sym);
236 
237  if(verbose){
238  std::cout << "Jdot symbolic:\n" << jdot_sym << "\n"
239  << "Jdot solver:\n" << jdot_by_solver << "\n"
240  << "Error:\n" << jdot_qdot_sym-jdot_qdot_by_solver << "\n"
241  << q << "\n"
242  << qdot << std::endl;
243  }
244  double err = jdot_qdot_sym.vel.Norm() - jdot_qdot_by_solver.vel.Norm()
245  + jdot_qdot_sym.rot.Norm() - jdot_qdot_by_solver.rot.Norm();
246  return std::abs(err);
247 }
248 
249 bool runTest(const Chain& chain,const int& representation)
250 {
251  bool success = true;
252  bool verbose = false;
253  double err;
254  bool print_err = false;
255 
256  for(double dt=1e-6;dt<0.1;dt*=10)
257  {
258  double eps_diff_vs_solver = 4.0*dt; // Apparently :)
259 
260  for(int i=0; i<100 ; ++i)
261  {
262  err = compare_Jdot_Diff_vs_Solver(chain,dt,representation,verbose);
263 
264  success &= err<=eps_diff_vs_solver;
265 
266  if(!success || print_err){
267  std::cout << "dt: "<< dt << "\n"
268  << "err: "<< err << "\n"
269  << "eps_diff_vs_solver: " << eps_diff_vs_solver << std::endl;
270  if(!success)
271  break;
272  }
273  }
274  }
275 
276  return success;
277 }
278 
279 void JacobianDotTest::testD2DiffHybrid(){
280  CPPUNIT_ASSERT(runTest(d2(),0));
281 }
282 void JacobianDotTest::testD6DiffHybrid(){
283  CPPUNIT_ASSERT(runTest(d6(),0));
284 }
285 void JacobianDotTest::testKukaDiffHybrid(){
286  CPPUNIT_ASSERT(runTest(KukaLWR_DHnew(),0));
287 }
288 
289 void JacobianDotTest::testD2DiffInertial(){
290  CPPUNIT_ASSERT(runTest(d2(),2));
291 }
292 void JacobianDotTest::testD6DiffInertial(){
293  CPPUNIT_ASSERT(runTest(d6(),2));
294 }
295 void JacobianDotTest::testKukaDiffInertial(){
296  CPPUNIT_ASSERT(runTest(KukaLWR_DHnew(),2));
297 }
298 void JacobianDotTest::testD2DiffBodyFixed(){
299  CPPUNIT_ASSERT(runTest(d2(),1));
300 }
301 void JacobianDotTest::testD6DiffBodyFixed(){
302  CPPUNIT_ASSERT(runTest(d6(),1));
303 }
304 void JacobianDotTest::testKukaDiffBodyFixed(){
305  CPPUNIT_ASSERT(runTest(KukaLWR_DHnew(),1));
306 }
307 
308 void JacobianDotTest::testD2Symbolic(){
309  // This test verifies if the solvers gives the same result as the symbolic Jdot (Hybrid only)
310  bool success = true;
311  bool verbose = false;
312  double err_d2_sym;
313  bool print_err = false;
314 
315  double eps_sym_vs_solver = 1e-10;
316 
317  for(int i=0; i<100; ++i)
318  {
319  err_d2_sym = compare_d2_Jdot_Symbolic_vs_Solver(verbose);
320 
321  success &= err_d2_sym <= eps_sym_vs_solver;
322 
323  if(!success || print_err){
324  std::cout << "err_d2_sym: " << err_d2_sym << "\n"
325  << "eps_sym_vs_solver: "<< eps_sym_vs_solver <<std::endl;
326  if(!success)
327  break;
328  }
329 
330  }
331 
332  CPPUNIT_ASSERT(success);
333 }
KDL::L0
static const double L0
Definition: jacobiandottest.cpp:12
KDL::L2
static const double L2
Definition: jacobiandottest.cpp:14
KDL::Jacobian::columns
unsigned int columns() const
Definition: jacobian.cpp:74
KDL::Frame::Identity
static Frame Identity()
Definition: frames.inl:700
KDL::Joint::RotZ
@ RotZ
Definition: joint.hpp:47
KDL::Twist::rot
Vector rot
The rotational velocity of that point.
Definition: frames.hpp:726
KDL::JntArray
Definition: jntarray.hpp:69
KDL::Jacobian::changeBase
friend bool changeBase(const Jacobian &src1, const Rotation &rot, Jacobian &dest)
Definition: jacobian.cpp:103
KDL::ChainJntToJacDotSolver::BODYFIXED
static const int BODYFIXED
Definition: chainjnttojacdotsolver.hpp:58
KDL::Rotation::Inverse
Rotation Inverse() const
Gives back the inverse rotation matrix of *this.
Definition: frames.inl:637
KDL::Vector::Zero
static Vector Zero()
Definition: frames.inl:138
KDL::random
IMETHOD void random(doubleVel &F)
Definition: framevel.hpp:65
KDL::Frame::p
Vector p
origine of the Frame
Definition: frames.hpp:574
JacobianDotTest
Definition: jacobiandottest.hpp:9
jacobiandottest.hpp
KDL::L4
static const double L4
Definition: jacobiandottest.cpp:16
KDL::Vector::Norm
double Norm(double eps=epsilon) const
Definition: frames.cpp:142
KDL::sin
INLINE Rall1d< T, V, S > sin(const Rall1d< T, V, S > &arg)
Definition: rall1d.h:335
JacobianDotTest::tearDown
void tearDown()
Definition: jacobiandottest.cpp:8
KDL::RigidBodyInertia
6D Inertia of a rigid body
Definition: rigidbodyinertia.hpp:37
runTest
bool runTest(const Chain &chain, const int &representation)
Definition: jacobiandottest.cpp:249
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
KDL::Jacobian::changeRefPoint
friend bool changeRefPoint(const Jacobian &src1, const Vector &base_AB, Jacobian &dest)
Definition: jacobian.cpp:89
KDL::L1
static const double L1
Definition: jacobiandottest.cpp:13
KDL::Vector
A concrete implementation of a 3 dimensional vector class.
Definition: frames.hpp:162
std::cout
KDL::cos
INLINE Rall1d< T, V, S > cos(const Rall1d< T, V, S > &arg)
Definition: rall1d.h:343
JacobianDotTest::setUp
void setUp()
Definition: jacobiandottest.cpp:7
compare_Jdot_Diff_vs_Solver
double compare_Jdot_Diff_vs_Solver(const Chain &chain, const double &dt, const int &representation, bool verbose)
Definition: jacobiandottest.cpp:160
KDL::Twist
represents both translational and rotational velocities.
Definition: frames.hpp:723
KDL::JntArray::rows
unsigned int rows() const
Definition: jntarray.cpp:70
JacobianDotTest::testKukaDiffInertial
void testKukaDiffInertial()
Definition: jacobiandottest.cpp:295
JacobianDotTest::testD2DiffHybrid
void testD2DiffHybrid()
Definition: jacobiandottest.cpp:279
KDL::ChainJntToJacDotSolver
Computes the Jacobian time derivative (Jdot) by calculating the partial derivatives regarding to a jo...
Definition: chainjnttojacdotsolver.hpp:48
KDL::Joint::RotX
@ RotX
Definition: joint.hpp:47
KDL::ChainJntToJacDotSolver::INERTIAL
static const int INERTIAL
Definition: chainjnttojacdotsolver.hpp:60
KDL::Frame
represents a frame transformation in 3D space (rotation + translation)
Definition: frames.hpp:572
KDL::d6
Chain d6()
Definition: jacobiandottest.cpp:24
KDL::RotationalInertia
Definition: rotationalinertia.hpp:34
JacobianDotTest::testD6DiffInertial
void testD6DiffInertial()
Definition: jacobiandottest.cpp:292
KDL::L3
static const double L3
Definition: jacobiandottest.cpp:15
KDL::ChainFkSolverPos_recursive::JntToCart
virtual int JntToCart(const JntArray &q_in, Frame &p_out, int segmentNr=-1)
Definition: chainfksolverpos_recursive.cpp:34
CPPUNIT_TEST_SUITE_REGISTRATION
CPPUNIT_TEST_SUITE_REGISTRATION(JacobianDotTest)
JacobianDotTest::testD2DiffBodyFixed
void testD2DiffBodyFixed()
Definition: jacobiandottest.cpp:298
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::L5
static const double L5
Definition: jacobiandottest.cpp:17
KDL::Joint::None
@ None
Definition: joint.hpp:47
KDL::Frame::DH_Craig1989
static Frame DH_Craig1989(double a, double alpha, double d, double theta)
Definition: frames.cpp:78
KDL::ChainJntToJacDotSolver::HYBRID
static const int HYBRID
Definition: chainjnttojacdotsolver.hpp:56
JacobianDotTest::testD6DiffHybrid
void testD6DiffHybrid()
Definition: jacobiandottest.cpp:282
KDL::Twist::vel
Vector vel
The velocity of that point.
Definition: frames.hpp:725
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::KukaLWR_DHnew
Chain KukaLWR_DHnew()
Definition: kukaLWR_DHnew.cpp:26
KDL::JntArrayVel
Definition: jntarrayvel.hpp:45
std::endl
T endl(T... args)
KDL::Chain::addSegment
void addSegment(const Segment &segment)
Definition: chain.cpp:53
JacobianDotTest::testD2DiffInertial
void testD2DiffInertial()
Definition: jacobiandottest.cpp:289
KDL::Frame::M
Rotation M
Orientation of the Frame.
Definition: frames.hpp:575
JacobianDotTest::testKukaDiffHybrid
void testKukaDiffHybrid()
Definition: jacobiandottest.cpp:285
J_d2_symbolic
Jacobian J_d2_symbolic(const JntArray &q, const JntArray &)
Definition: jacobiandottest.cpp:133
JacobianDotTest::testKukaDiffBodyFixed
void testKukaDiffBodyFixed()
Definition: jacobiandottest.cpp:304
KDL::d2
Chain d2()
Definition: jacobiandottest.cpp:18
Jdot_diff
void Jdot_diff(const Jacobian &J_q, const Jacobian &J_qdt, const double &dt, Jacobian &Jdot)
Definition: jacobiandottest.cpp:109
Jdot_d2_symbolic
Jacobian Jdot_d2_symbolic(const JntArray &q, const JntArray &qdot)
Definition: jacobiandottest.cpp:121
compare_d2_Jdot_Symbolic_vs_Solver
double compare_d2_Jdot_Symbolic_vs_Solver(bool verbose)
Definition: jacobiandottest.cpp:215
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::diff
IMETHOD Vector diff(const Vector &p_w_a, const Vector &p_w_b, double dt=1)
KDL::ChainFkSolverPos_recursive
Definition: chainfksolverpos_recursive.hpp:36
JacobianDotTest::testD2Symbolic
void testD2Symbolic()
Definition: jacobiandottest.cpp:308
KDL::MultiplyJacobian
void MultiplyJacobian(const Jacobian &jac, const JntArray &src, Twist &dest)
Definition: jntarray.cpp:100
KDL::Chain
This class encapsulates a serial kinematic interconnection structure. It is built out of segments.
Definition: chain.hpp:35
JacobianDotTest::testD6DiffBodyFixed
void testD6DiffBodyFixed()
Definition: jacobiandottest.cpp:301
changeRepresentation
void changeRepresentation(Jacobian &J, const Frame &F_bs_ee, const int &representation)
Definition: jacobiandottest.cpp:93
KDL::Chain::getNrOfJoints
unsigned int getNrOfJoints() const
Definition: chain.hpp:71
KDL::ChainJntToJacDotSolver::setRepresentation
void setRepresentation(const int &representation)
Sets the internal variable for the representation (with a check on the value)
Definition: chainjnttojacdotsolver.cpp:218
KDL::Jacobian
Definition: jacobian.hpp:36
KDL::abs
INLINE Rall1d< T, V, S > abs(const Rall1d< T, V, S > &x)
Definition: rall1d.h:431
Generated on Tue Nov 26 2024 04:35:24 for orocos_kdl by   doxygen 1.8.17