jacobiantest.cpp
Go to the documentation of this file.
1 #include "jacobiantest.hpp"
2 #include <kinfam_io.hpp>
3 #include <Eigen/Core>
4 
6 
7 using namespace KDL;
8 
11 
13  //Create a random jacobian
14  Jacobian j1(5);
15  j1.data.setRandom();
16  //Create a random Vector
17  Vector p;
18  random(p);
19 
20  Jacobian j2(5);
21  CPPUNIT_ASSERT(changeRefPoint(j1,p,j2));
22  CPPUNIT_ASSERT(j1!=j2);
23  Jacobian j3(4);
24  CPPUNIT_ASSERT(!changeRefPoint(j1,p,j3));
25  j3.resize(5);
26  CPPUNIT_ASSERT(changeRefPoint(j2,-p,j3));
27  CPPUNIT_ASSERT_EQUAL(j1,j3);
28 
29 }
30 
32  //Create a random jacobian
33  Jacobian j1(5);
34  j1.data.setRandom();
35  //Create a random frame
36  Frame f;
37  random(f);
38 
39  Jacobian j2(5);
40  CPPUNIT_ASSERT(changeRefFrame(j1,f,j2));
41  CPPUNIT_ASSERT(j1!=j2);
42  Jacobian j3(4);
43  CPPUNIT_ASSERT(!changeRefFrame(j1,f,j3));
44  j3.resize(5);
45  CPPUNIT_ASSERT(changeRefFrame(j2,f.Inverse(),j3));
46  CPPUNIT_ASSERT_EQUAL(j1,j3);
47 }
48 
50  //Create a random jacobian
51  Jacobian j1(5);
52  j1.data.setRandom();
53  //Create a random rotation
54  Rotation r;
55  random(r);
56 
57  Jacobian j2(5);
58  CPPUNIT_ASSERT(changeBase(j1,r,j2));
59  CPPUNIT_ASSERT(j1!=j2);
60  Jacobian j3(4);
61  CPPUNIT_ASSERT(!changeBase(j1,r,j3));
62  j3.resize(5);
63  CPPUNIT_ASSERT(changeBase(j2,r.Inverse(),j3));
64  CPPUNIT_ASSERT_EQUAL(j1,j3);
65 }
66 
68  //Create an empty Jacobian
69  Jacobian j1(2);
70  //Get size
71  CPPUNIT_ASSERT_EQUAL(j1.rows(),(unsigned int)6);
72  CPPUNIT_ASSERT_EQUAL(j1.columns(),(unsigned int)2);
73  //Create a second Jacobian from empty
74  Jacobian j2(j1);
75  //Get size
76  CPPUNIT_ASSERT_EQUAL(j2.rows(),(unsigned int)6);
77  CPPUNIT_ASSERT_EQUAL(j2.columns(),(unsigned int)2);
78  Jacobian j3=j1;
79  //Get size
80  CPPUNIT_ASSERT_EQUAL(j3.rows(),(unsigned int)6);
81  CPPUNIT_ASSERT_EQUAL(j3.columns(),(unsigned int)2);
82 
83  //Test resize
84  j1.resize(5);
85  //Get size
86  CPPUNIT_ASSERT_EQUAL(j1.rows(),(unsigned int)6);
87  CPPUNIT_ASSERT_EQUAL(j1.columns(),(unsigned int)5);
88 
89  j2=j1;
90  //Get size
91  CPPUNIT_ASSERT_EQUAL(j2.rows(),(unsigned int)6);
92  CPPUNIT_ASSERT_EQUAL(j2.columns(),(unsigned int)5);
93 }
94 
96  Jacobian j1(1);
97  Jacobian j2(j1);
98  for (unsigned int i=0; i<6; ++i)
99  {
100  random(j1(i,0));
101  j1(i,0) = j1(i,0)*1e-7;
102  }
103  CPPUNIT_ASSERT(Equal(j1,j2,1));
104  CPPUNIT_ASSERT(Equal(j1,j2,1e-6));
105  CPPUNIT_ASSERT(!Equal(j1,j2,1e-8));
106 
107 }
108 
109 
represents rotations in 3 dimensional space.
Definition: frames.hpp:301
unsigned int rows() const
Definition: jacobian.cpp:71
void TestEqual()
unsigned int columns() const
Definition: jacobian.cpp:76
void resize(unsigned int newNrOfColumns)
Allocates memory for new size (can break realtime behavior)
Definition: jacobian.cpp:56
bool changeBase(const Jacobian &src1, const Rotation &rot, Jacobian &dest)
Definition: jacobian.cpp:105
void TestChangeRefPoint()
bool changeRefPoint(const Jacobian &src1, const Vector &base_AB, Jacobian &dest)
Definition: jacobian.cpp:91
EIGEN_MAKE_ALIGNED_OPERATOR_NEW Eigen::Matrix< double, 6, Eigen::Dynamic > data
Definition: jacobian.hpp:41
IMETHOD bool Equal(const FrameAcc &r1, const FrameAcc &r2, double eps=epsilon)
Definition: frameacc.hpp:394
Frame Inverse() const
Gives back inverse transformation of a Frame.
Definition: frames.hpp:423
A concrete implementation of a 3 dimensional vector class.
Definition: frames.hpp:160
Rotation Inverse() const
Gives back the inverse rotation matrix of *this.
Definition: frames.hpp:638
void TestConstructor()
void TestChangeBase()
represents a frame transformation in 3D space (rotation + translation)
Definition: frames.hpp:570
void TestChangeRefFrame()
CPPUNIT_TEST_SUITE_REGISTRATION(JacobianTest)
IMETHOD void random(Vector &a)
addDelta operator for displacement rotational velocity.
Definition: frames.hpp:1215
bool changeRefFrame(const Jacobian &src1, const Frame &frame, Jacobian &dest)
Definition: jacobian.cpp:119


orocos_kdl
Author(s):
autogenerated on Sun Nov 22 2020 03:16:43