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<<std::endl;
204  std::cout << "Jdot solver:\n"<<jdot_by_solver<<std::endl;
205 
206  std::cout << "Error : " <<jdot_qdot_by_diff-jdot_qdot_by_solver<<q<<qdot<<std::endl;
207  }
208  double err = jdot_qdot_by_diff.vel.Norm() - jdot_qdot_by_solver.vel.Norm()
209  + jdot_qdot_by_diff.rot.Norm() - jdot_qdot_by_solver.rot.Norm();
210  return std::abs(err);
211 }
212 
213 double compare_d2_Jdot_Symbolic_vs_Solver(bool verbose)
214 {
215  Chain chain=d2();
216  JntArray q(chain.getNrOfJoints());
217  JntArray qdot(chain.getNrOfJoints());
218 
219  random(q);
220  random(qdot);
221 
222  ChainJntToJacDotSolver jdot_solver(chain);
223 
224  Jacobian jdot_sym = Jdot_d2_symbolic(q,qdot);
225 
226  Jacobian jdot_by_solver(chain.getNrOfJoints());
227  jdot_solver.JntToJacDot(JntArrayVel(q,qdot),jdot_by_solver);
228 
229  Twist jdot_qdot_by_solver;
230  MultiplyJacobian(jdot_by_solver,qdot,jdot_qdot_by_solver);
231 
232  Twist jdot_qdot_sym;
233  MultiplyJacobian(jdot_sym,qdot,jdot_qdot_sym);
234 
235  if(verbose){
236  std::cout << "Jdot symbolic : \n" << jdot_sym<<std::endl;
237  std::cout << "Jdot solver:\n"<<jdot_by_solver<<std::endl;
238  std::cout << "Error : " <<jdot_qdot_sym-jdot_qdot_by_solver<<q<<qdot<<std::endl;
239  }
240  double err = jdot_qdot_sym.vel.Norm() - jdot_qdot_by_solver.vel.Norm()
241  + jdot_qdot_sym.rot.Norm() - jdot_qdot_by_solver.rot.Norm();
242  return std::abs(err);
243 }
244 
245 bool runTest(const Chain& chain,const int& representation)
246 {
247  bool success=true;
248  bool verbose = false;
249  double err;
250  bool print_err = false;
251 
252  for(double dt=1e-6;dt<0.1;dt*=10)
253  {
254  double eps_diff_vs_solver = 4.0*dt; // Apparently :)
255 
256  for(int i=0;i<100;i++)
257  {
258  err = compare_Jdot_Diff_vs_Solver(chain,dt,representation,verbose);
259 
260  success &= err<=eps_diff_vs_solver;
261 
262  if(!success || print_err){
263  std::cout<<" dt:"<< dt<<" err:"<<err
264  <<" eps_diff_vs_solver:"<<eps_diff_vs_solver
265  <<std::endl;
266  if(!success)
267  break;
268  }
269  }
270  }
271 
272  return success;
273 }
274 
275 void JacobianDotTest::testD2DiffHybrid(){
276  CPPUNIT_ASSERT(runTest(d2(),0));
277 }
278 void JacobianDotTest::testD6DiffHybrid(){
279  CPPUNIT_ASSERT(runTest(d6(),0));
280 }
281 void JacobianDotTest::testKukaDiffHybrid(){
282  CPPUNIT_ASSERT(runTest(KukaLWR_DHnew(),0));
283 }
284 
285 void JacobianDotTest::testD2DiffInertial(){
286  CPPUNIT_ASSERT(runTest(d2(),2));
287 }
288 void JacobianDotTest::testD6DiffInertial(){
289  CPPUNIT_ASSERT(runTest(d6(),2));
290 }
291 void JacobianDotTest::testKukaDiffInertial(){
292  CPPUNIT_ASSERT(runTest(KukaLWR_DHnew(),2));
293 }
294 void JacobianDotTest::testD2DiffBodyFixed(){
295  CPPUNIT_ASSERT(runTest(d2(),1));
296 }
297 void JacobianDotTest::testD6DiffBodyFixed(){
298  CPPUNIT_ASSERT(runTest(d6(),1));
299 }
300 void JacobianDotTest::testKukaDiffBodyFixed(){
301  CPPUNIT_ASSERT(runTest(KukaLWR_DHnew(),1));
302 }
303 
304 void JacobianDotTest::testD2Symbolic(){
305  // This test verifies if the solvers gives the same result as the symbolic Jdot (Hybrid only)
306  bool success=true;
307  bool verbose = false;
308  double err_d2_sym;
309  bool print_err = false;
310 
311  double eps_sym_vs_solver = 1e-10;
312 
313  for(int i=0;i<100;i++)
314  {
315  err_d2_sym = compare_d2_Jdot_Symbolic_vs_Solver(verbose);
316 
317  success &= err_d2_sym<=eps_sym_vs_solver;
318 
319  if(!success || print_err){
320  std::cout <<" err_d2_sym:"<<err_d2_sym
321  <<" eps_sym_vs_solver:"<<eps_sym_vs_solver<<std::endl;
322  if(!success)
323  break;
324  }
325 
326  }
327 
328  CPPUNIT_ASSERT(success);
329 }
KDL::KukaLWR_DHnew
Chain KukaLWR_DHnew()
Definition: kukaLWR_DHnew.cpp:26
KDL::ChainJntToJacDotSolver::setRepresentation
void setRepresentation(const int &representation)
Sets the internal variable for the representation (with a check on the value)
Definition: chainjnttojacdotsolver.cpp:210
KDL::d2
Chain d2()
Definition: jacobiandottest.cpp:18
KDL::ChainFkSolverPos_recursive::JntToCart
virtual int JntToCart(const JntArray &q_in, Frame &p_out, int segmentNr=-1)
Definition: chainfksolverpos_recursive.cpp:33
KDL::ChainFkSolverPos_recursive
Definition: chainfksolverpos_recursive.hpp:36
KDL::Segment
This class encapsulates a simple segment, that is a "rigid body" (i.e., a frame and a rigid body...
Definition: segment.hpp:46
KDL::JntArray::rows
unsigned int rows() const
Definition: jntarray.cpp:72
KDL::ChainJntToJacDotSolver
Computes the Jacobian time derivative (Jdot) by calculating the partial derivatives regarding to a jo...
Definition: chainjnttojacdotsolver.hpp:48
KDL::Rotation::Inverse
Rotation Inverse() const
Gives back the inverse rotation matrix of *this.
Definition: frames.hpp:633
KDL::Chain
This class encapsulates a serial kinematic interconnection structure. It is built out of segments...
Definition: chain.hpp:35
KDL::L3
static const double L3
Definition: jacobiandottest.cpp:15
KDL::Twist::vel
Vector vel
The velocity of that point.
Definition: frames.hpp:722
KDL::diff
IMETHOD Vector diff(const Vector &p_w_a, const Vector &p_w_b, double dt=1)
Definition: frames.hpp:1123
KDL::MultiplyJacobian
void MultiplyJacobian(const Jacobian &jac, const JntArray &src, Twist &dest)
Definition: jntarray.cpp:102
KDL::Chain::addSegment
void addSegment(const Segment &segment)
Definition: chain.cpp:54
KDL::JntArray
This class represents an fixed size array containing joint values of a KDL::Chain.
Definition: jntarray.hpp:69
Jdot_d2_symbolic
Jacobian Jdot_d2_symbolic(const JntArray &q, const JntArray &qdot)
Definition: jacobiandottest.cpp:121
KDL::RigidBodyInertia
6D Inertia of a rigid body
Definition: rigidbodyinertia.hpp:37
compare_d2_Jdot_Symbolic_vs_Solver
double compare_d2_Jdot_Symbolic_vs_Solver(bool verbose)
Definition: jacobiandottest.cpp:213
KDL::L5
static const double L5
Definition: jacobiandottest.cpp:17
J_d2_symbolic
Jacobian J_d2_symbolic(const JntArray &q, const JntArray &qdot)
Definition: jacobiandottest.cpp:133
CPPUNIT_TEST_SUITE_REGISTRATION
CPPUNIT_TEST_SUITE_REGISTRATION(JacobianDotTest)
KDL::L0
static const double L0
Definition: jacobiandottest.cpp:12
Jdot_diff
void Jdot_diff(const Jacobian &J_q, const Jacobian &J_qdt, const double &dt, Jacobian &Jdot)
Definition: jacobiandottest.cpp:109
KDL::Jacobian::columns
unsigned int columns() const
Definition: jacobian.cpp:75
KDL::Frame::M
Rotation M
Orientation of the Frame.
Definition: frames.hpp:573
KDL::ChainJntToJacSolver
Class to calculate the jacobian of a general KDL::Chain, it is used by other solvers. It should not be used outside of KDL.
Definition: chainjnttojacsolver.hpp:41
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:720
KDL::SetToZero
IMETHOD void SetToZero(Vector &v)
Definition: frames.hpp:1062
KDL::Frame::DH_Craig1989
static Frame DH_Craig1989(double a, double alpha, double d, double theta)
Definition: frames.cpp:54
KDL::L2
static const double L2
Definition: jacobiandottest.cpp:14
KDL::ChainJntToJacSolver::JntToJac
virtual int JntToJac(const JntArray &q_in, Jacobian &jac, int segmentNR=-1)
Definition: chainjnttojacsolver.cpp:48
KDL::Vector
A concrete implementation of a 3 dimensional vector class.
Definition: frames.hpp:160
KDL::Twist::rot
Vector rot
The rotational velocity of that point.
Definition: frames.hpp:723
KDL::Vector::Norm
double Norm() const
Definition: frames.cpp:118
KDL::Frame::p
Vector p
origine of the Frame
Definition: frames.hpp:572
KDL::Jacobian::changeBase
friend bool changeBase(const Jacobian &src1, const Rotation &rot, Jacobian &dest)
Definition: jacobian.cpp:104
KDL::L4
static const double L4
Definition: jacobiandottest.cpp:16
KDL::Jacobian::changeRefPoint
friend bool changeRefPoint(const Jacobian &src1, const Vector &base_AB, Jacobian &dest)
Definition: jacobian.cpp:90
KDL::Chain::getNrOfJoints
unsigned int getNrOfJoints() const
Definition: chain.hpp:71
runTest
bool runTest(const Chain &chain, const int &representation)
Definition: jacobiandottest.cpp:245
KDL::abs
INLINE Rall1d< T, V, S > abs(const Rall1d< T, V, S > &x)
Definition: rall1d.h:409
KDL::ChainJntToJacDotSolver::JntToJacDot
virtual int JntToJacDot(const KDL::JntArrayVel &q_in, KDL::Twist &jac_dot_q_dot, int seg_nr=-1)
Computes .
Definition: chainjnttojacdotsolver.cpp:47
KDL::Frame::Identity
static Frame Identity()
Definition: frames.hpp:696
KDL::Frame
represents a frame transformation in 3D space (rotation + translation)
Definition: frames.hpp:570
KDL::Joint
This class encapsulates a simple joint, that is with one parameterized degree of freedom and with sca...
Definition: joint.hpp:45
KDL::L1
static const double L1
Definition: jacobiandottest.cpp:13
KDL::cos
INLINE Rall1d< T, V, S > cos(const Rall1d< T, V, S > &arg)
Definition: rall1d.h:321
KDL::random
IMETHOD void random(Vector &a)
addDelta operator for displacement rotational velocity.
Definition: frames.hpp:1208
changeRepresentation
void changeRepresentation(Jacobian &J, const Frame &F_bs_ee, const int &representation)
Definition: jacobiandottest.cpp:93
KDL::sin
INLINE Rall1d< T, V, S > sin(const Rall1d< T, V, S > &arg)
Definition: rall1d.h:313
KDL::Vector::Zero
static Vector Zero()
Definition: frames.hpp:139
KDL::d6
Chain d6()
Definition: jacobiandottest.cpp:24

orocos_kdl
Author(s):
autogenerated on Sat Jun 15 2019 19:07:36