kinfamtest.cpp
Go to the documentation of this file.
1 #include "kinfamtest.hpp"
2 #include <frames_io.hpp>
3 #include <kinfam_io.hpp>
4 #include <chainfksolverpos_recursive.hpp>
5 
6 CPPUNIT_TEST_SUITE_REGISTRATION( KinFamTest );
7 
8 #ifdef __APPLE__
9 typedef unsigned int uint;
10 #endif
11 
12 using namespace KDL;
13 using namespace std;
14 
15 void KinFamTest::setUp()
16 {
17 }
18 
19 void KinFamTest::tearDown()
20 {
21 }
22 
23 void KinFamTest::JointTest()
24 {
25  double q;
26  Joint j;
27  j=Joint("Joint 1", Joint::None);
28  CPPUNIT_ASSERT_EQUAL(Joint::None,j.getType());
29  random(q);
30  CPPUNIT_ASSERT_EQUAL(j.pose(q),Frame::Identity());
31  random(q);
32  CPPUNIT_ASSERT_EQUAL(j.twist(q),Twist::Zero());
33  random(q);
34  j=Joint("Joint 2", Joint::RotX);
35  CPPUNIT_ASSERT_EQUAL(Joint::RotX,j.getType());
36  CPPUNIT_ASSERT_EQUAL(j.pose(q),Frame(Rotation::RotX(q)));
37  random(q);
38  CPPUNIT_ASSERT_EQUAL(j.twist(q),Twist(Vector::Zero(),Vector(q,0,0)));
39  random(q);
40  j=Joint("Joint 3", Joint::RotY);
41  CPPUNIT_ASSERT_EQUAL(Joint::RotY,j.getType());
42  CPPUNIT_ASSERT_EQUAL(j.pose(q),Frame(Rotation::RotY(q)));
43  random(q);
44  CPPUNIT_ASSERT_EQUAL(j.twist(q),Twist(Vector::Zero(),Vector(0,q,0)));
45  random(q);
46  j=Joint("Joint 4", Joint::RotZ);
47  CPPUNIT_ASSERT_EQUAL(Joint::RotZ,j.getType());
48  CPPUNIT_ASSERT_EQUAL(j.pose(q),Frame(Rotation::RotZ(q)));
49  random(q);
50  CPPUNIT_ASSERT_EQUAL(j.twist(q),Twist(Vector::Zero(),Vector(0,0,q)));
51  random(q);
52  j=Joint("Joint 5", Joint::TransX);
53  CPPUNIT_ASSERT_EQUAL(Joint::TransX,j.getType());
54  CPPUNIT_ASSERT_EQUAL(j.pose(q),Frame(Vector(q,0,0)));
55  random(q);
56  CPPUNIT_ASSERT_EQUAL(j.twist(q),Twist(Vector(q,0,0),Vector::Zero()));
57  random(q);
58  j=Joint("Joint 6", Joint::TransY);
59  CPPUNIT_ASSERT_EQUAL(Joint::TransY,j.getType());
60  CPPUNIT_ASSERT_EQUAL(j.pose(q),Frame(Vector(0,q,0)));
61  random(q);
62  CPPUNIT_ASSERT_EQUAL(j.twist(q),Twist(Vector(0,q,0),Vector::Zero()));
63  random(q);
64  j=Joint("Joint 7", Joint::TransZ);
65  CPPUNIT_ASSERT_EQUAL(Joint::TransZ,j.getType());
66  CPPUNIT_ASSERT_EQUAL(j.pose(q),Frame(Vector(0,0,q)));
67  random(q);
68  CPPUNIT_ASSERT_EQUAL(j.twist(q),Twist(Vector(0,0,q),Vector::Zero()));
69 
70 }
71 
72 void KinFamTest::SegmentTest()
73 {
74  Segment s;
75  double q,qdot;
76  Frame f,f1;
77  random(f);
78  s = Segment("Segment 0", Joint("Joint 0", Joint::None),f);
79  random(q);
80  random(qdot);
81  f1=s.getJoint().pose(q)*f;
82  CPPUNIT_ASSERT_EQUAL(f1,s.pose(q));
83  CPPUNIT_ASSERT_EQUAL(s.getJoint().twist(qdot).RefPoint(f1.p),s.twist(q,qdot));
84  random(f);
85  s = Segment("Segment 1", Joint("Joint 1", Joint::RotX),f);
86  random(q);
87  random(qdot);
88  f1=s.getJoint().pose(q)*f;
89  CPPUNIT_ASSERT_EQUAL(f1,s.pose(q));
90  CPPUNIT_ASSERT_EQUAL(s.getJoint().twist(qdot).RefPoint(f1.p),s.twist(q,qdot));
91  random(f);
92  s = Segment("Segment 3", Joint("Joint 3", Joint::RotY),f);
93  random(q);
94  random(qdot);
95  f1=s.getJoint().pose(q)*f;
96  CPPUNIT_ASSERT_EQUAL(f1,s.pose(q));
97  CPPUNIT_ASSERT_EQUAL(s.getJoint().twist(qdot).RefPoint(f1.p),s.twist(q,qdot));
98  random(f);
99  s = Segment("Segment 4", Joint("Joint 4", Joint::RotZ),f);
100  random(q);
101  random(qdot);
102  f1=s.getJoint().pose(q)*f;
103  CPPUNIT_ASSERT_EQUAL(f1,s.pose(q));
104  CPPUNIT_ASSERT_EQUAL(s.getJoint().twist(qdot).RefPoint(f1.p),s.twist(q,qdot));
105  random(f);
106  s = Segment("Segment 5", Joint("Joint 5", Joint::TransX),f);
107  random(q);
108  random(qdot);
109  f1=s.getJoint().pose(q)*f;
110  CPPUNIT_ASSERT_EQUAL(f1,s.pose(q));
111  CPPUNIT_ASSERT_EQUAL(s.getJoint().twist(qdot).RefPoint(f1.p),s.twist(q,qdot));
112  random(f);
113  s = Segment("Segment 6", Joint("Joint 6", Joint::TransY),f);
114  random(q);
115  random(qdot);
116  f1=s.getJoint().pose(q)*f;
117  CPPUNIT_ASSERT_EQUAL(f1,s.pose(q));
118  CPPUNIT_ASSERT_EQUAL(s.getJoint().twist(qdot).RefPoint(f1.p),s.twist(q,qdot));
119  random(f);
120  s = Segment("Segment 7", Joint("Joint 7", Joint::TransZ),f);
121  random(q);
122  random(qdot);
123  f1=s.getJoint().pose(q)*f;
124  CPPUNIT_ASSERT_EQUAL(f1,s.pose(q));
125  CPPUNIT_ASSERT_EQUAL(s.getJoint().twist(qdot).RefPoint(f1.p),s.twist(q,qdot));
126 }
127 
128 void KinFamTest::ChainTest()
129 {
130  Chain chain1;
131 
132  chain1.addSegment(Segment("Segment 0", Joint("Joint 0", Joint::RotZ),
133  Frame(Vector(0.0,0.0,0.0))));
134  chain1.addSegment(Segment("Segment 1", Joint("Joint 1", Joint::RotX),
135  Frame(Vector(0.0,0.0,0.9))));
136  chain1.addSegment(Segment("Segment 2", Joint("Joint 2", Joint::RotX),
137  Frame(Vector(0.0,0.0,1.2))));
138  chain1.addSegment(Segment("Segment 3", Joint("Joint 3", Joint::RotZ),
139  Frame(Vector(0.0,0.0,1.5))));
140  chain1.addSegment(Segment("Segment 4", Joint("Joint 4", Joint::RotX),
141  Frame(Vector(0.0,0.0,0.0))));
142  chain1.addSegment(Segment("Segment 5", Joint("Joint 5", Joint::RotZ),
143  Frame(Vector(0.0,0.0,0.4))));
144  CPPUNIT_ASSERT_EQUAL(chain1.getNrOfJoints(),(uint)6);
145  CPPUNIT_ASSERT_EQUAL(chain1.getNrOfSegments(),(uint)6);
146  chain1.addSegment(Segment("Segment 6", Joint("Joint 6", Joint::None),Frame(Vector(0.0,0.1,0.0))));
147  CPPUNIT_ASSERT_EQUAL(chain1.getNrOfJoints(),(uint)6);
148  CPPUNIT_ASSERT_EQUAL(chain1.getNrOfSegments(),(uint)7);
149 
150  Chain chain2 = chain1;
151  CPPUNIT_ASSERT_EQUAL(chain2.getNrOfJoints(),chain1.getNrOfJoints());
152  CPPUNIT_ASSERT_EQUAL(chain2.getNrOfSegments(),chain1.getNrOfSegments());
153  chain2.addChain(chain1);
154  CPPUNIT_ASSERT_EQUAL(chain2.getNrOfJoints(),chain1.getNrOfJoints()*(uint)2);
155  CPPUNIT_ASSERT_EQUAL(chain2.getNrOfSegments(),chain1.getNrOfSegments()*(uint)2);
156 }
157 
158 void KinFamTest::TreeTest()
159 {
160  Tree tree1;
161  Segment segment1("Segment 1", Joint("Joint 1", Joint::None));
162  Segment segment2("Segment 2", Joint("Joint 2", Joint::RotX),Frame(Vector(0.1,0.2,0.3)));
163  Segment segment3("Segment 3", Joint("Joint 3", Joint::TransZ),Frame(Rotation::RotX(1.57)));
164  Segment segment4("Segment 4", Joint("Joint 4", Joint::RotX),Frame(Vector(0.1,0.2,0.3)));
165  Segment segment5("Segment 5", Joint("Joint 5", Joint::RotX),Frame(Vector(0.1,0.2,0.3)));
166  Segment segment6("Segment 6", Joint("Joint 6", Joint::RotX),Frame(Vector(0.1,0.2,0.3)));
167  Segment segment7("Segment 7", Joint("Joint 7", Joint::RotX),Frame(Vector(0.1,0.2,0.3)));
168 
169  cout<<tree1<<endl;
170 
171  CPPUNIT_ASSERT(tree1.addSegment(segment1,"root"));
172  CPPUNIT_ASSERT(tree1.addSegment(segment2,"root"));
173  CPPUNIT_ASSERT(tree1.addSegment(segment3,"Segment 1"));
174  CPPUNIT_ASSERT(tree1.addSegment(segment4,"Segment 3"));
175  CPPUNIT_ASSERT(!tree1.addSegment(segment1,"Segment 6"));
176  CPPUNIT_ASSERT(!tree1.addSegment(segment1,"Segment 4"));
177 
178  cout<<tree1<<endl;
179 
180  Tree tree2;
181  CPPUNIT_ASSERT(tree2.addSegment(segment5,"root"));
182  CPPUNIT_ASSERT(tree2.addSegment(segment6,"root"));
183  CPPUNIT_ASSERT(tree2.addSegment(segment7,"Segment 6"));
184 
185  cout<<tree2<<endl;
186 
187  Chain chain1;
188  chain1.addSegment(Segment("Segment 8", Joint("Joint 8", Joint::RotZ),
189  Frame(Vector(0.0,0.0,0.0))));
190  chain1.addSegment(Segment("Segment 9", Joint("Joint 9", Joint::RotX),
191  Frame(Vector(0.0,0.0,0.9))));
192  chain1.addSegment(Segment("Segment 10", Joint("Joint 10", Joint::RotX),
193  Frame(Vector(0.0,0.0,1.2))));
194  chain1.addSegment(Segment("Segment 11", Joint("Joint 11", Joint::RotZ),
195  Frame(Vector(0.0,0.0,1.5))));
196  chain1.addSegment(Segment("Segment 12", Joint("Joint 12", Joint::RotX),
197  Frame(Vector(0.0,0.0,0.0))));
198  chain1.addSegment(Segment("Segment 13", Joint("Joint 13", Joint::RotZ),
199  Frame(Vector(0.0,0.0,0.4))));
200 
201 
202  CPPUNIT_ASSERT(tree2.addChain(chain1, "Segment 6"));
203  cout<<tree2<<endl;
204  CPPUNIT_ASSERT(tree1.addTree(tree2, "Segment 2"));
205  cout<<tree1<<endl;
206 
207  Chain extract_chain1;
208  CPPUNIT_ASSERT(tree1.getChain("Segment 2", "Segment 4", extract_chain1));
209  Chain extract_chain2;
210  CPPUNIT_ASSERT(tree1.getChain("Segment 4", "Segment 2", extract_chain2));
211  CPPUNIT_ASSERT(tree1.getChain("Segment 4", "Segment 2", extract_chain2));
212  CPPUNIT_ASSERT(extract_chain1.getNrOfJoints()==extract_chain2.getNrOfJoints());
213  CPPUNIT_ASSERT(extract_chain1.getNrOfSegments()==extract_chain2.getNrOfSegments());
214  ChainFkSolverPos_recursive solver1(extract_chain1);
215  ChainFkSolverPos_recursive solver2(extract_chain2);
216 
217 
218  Frame f1, f2;
219  JntArray jnt1(extract_chain2.getNrOfJoints());
220  JntArray jnt2(extract_chain2.getNrOfJoints());
221  for (int i=0; i<(int)extract_chain2.getNrOfJoints(); i++){
222  jnt1(i) = (i+1)*2;
223  jnt2((int)extract_chain2.getNrOfJoints()-i-1) = jnt1(i);
224  }
225  solver1.JntToCart(jnt1, f1);
226  solver2.JntToCart(jnt2, f2);
227  CPPUNIT_ASSERT(f1 == f2.Inverse());
228 }
229 
230 
231 
chainfksolverpos_recursive.hpp
KDL::Twist::Zero
static Twist Zero()
Definition: frames.hpp:291
KDL::ChainFkSolverPos_recursive::JntToCart
virtual int JntToCart(const JntArray &q_in, Frame &p_out, int segmentNr=-1)
Definition: chainfksolverpos_recursive.cpp:33
KinFamTest::JointTest
void JointTest()
Definition: kinfamtest.cpp:23
KDL::ChainFkSolverPos_recursive
Definition: chainfksolverpos_recursive.hpp:36
KinFamTest::TreeTest
void TreeTest()
Definition: kinfamtest.cpp:158
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::Rotation::RotX
static Rotation RotX(double angle)
The Rot... static functions give the value of the appropriate rotation matrix back.
Definition: frames.hpp:601
KDL::Twist::RefPoint
Twist RefPoint(const Vector &v_base_AB) const
Definition: frames.hpp:303
KDL::Chain
This class encapsulates a serial kinematic interconnection structure. It is built out of segments...
Definition: chain.hpp:35
KinFamTest::ChainTest
void ChainTest()
Definition: kinfamtest.cpp:128
KDL::Rotation::RotZ
static Rotation RotZ(double angle)
The Rot... static functions give the value of the appropriate rotation matrix back.
Definition: frames.hpp:611
KDL::Chain::getNrOfSegments
unsigned int getNrOfSegments() const
Definition: chain.hpp:76
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
KDL::Joint::pose
Frame pose(const double &q) const
Definition: joint.cpp:76
frames_io.hpp
KDL::Rotation::RotY
static Rotation RotY(double angle)
The Rot... static functions give the value of the appropriate rotation matrix back.
Definition: frames.hpp:606
KDL::Segment::pose
Frame pose(const double &q) const
Definition: segment.cpp:57
KDL::Segment::twist
Twist twist(const double &q, const double &qdot) const
Definition: segment.cpp:62
KDL::Twist
represents both translational and rotational velocities.
Definition: frames.hpp:720
KDL::Tree::addChain
bool addChain(const Chain &chain, const std::string &hook_name)
Definition: tree.cpp:83
KDL::Vector
A concrete implementation of a 3 dimensional vector class.
Definition: frames.hpp:160
KinFamTest::setUp
void setUp()
Definition: kinfamtest.cpp:15
KinFamTest::tearDown
void tearDown()
Definition: kinfamtest.cpp:19
KDL::Segment::getJoint
const Joint & getJoint() const
Definition: segment.hpp:118
KDL::Tree::addTree
bool addTree(const Tree &tree, const std::string &hook_name)
Definition: tree.cpp:94
KDL::Frame::p
Vector p
origine of the Frame
Definition: frames.hpp:572
KDL::Chain::addChain
void addChain(const Chain &chain)
Definition: chain.cpp:62
KDL::Chain::getNrOfJoints
unsigned int getNrOfJoints() const
Definition: chain.hpp:71
KDL::Frame::Inverse
Frame Inverse() const
Gives back inverse transformation of a Frame.
Definition: frames.hpp:423
kinfam_io.hpp
KDL::Tree::getChain
bool getChain(const std::string &chain_root, const std::string &chain_tip, Chain &chain) const
Definition: tree.cpp:117
CPPUNIT_TEST_SUITE_REGISTRATION
CPPUNIT_TEST_SUITE_REGISTRATION(KinFamTest)
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::Tree::addSegment
bool addSegment(const Segment &segment, const std::string &hook_name)
Definition: tree.cpp:55
KinFamTest::SegmentTest
void SegmentTest()
Definition: kinfamtest.cpp:72
KDL::random
IMETHOD void random(Vector &a)
addDelta operator for displacement rotational velocity.
Definition: frames.hpp:1208
KDL::Tree
This class encapsulates a tree kinematic interconnection structure. It is built out of segments...
Definition: tree.hpp:99
KDL::Vector::Zero
static Vector Zero()
Definition: frames.hpp:139
KDL::Joint::twist
Twist twist(const double &qdot) const
Definition: joint.cpp:106
KDL::Joint::getType
const JointType & getType() const
Definition: joint.hpp:159

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