orocos_kdl
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 
14 void KinFamTest::setUp()
15 {
16 }
17 
18 void KinFamTest::tearDown()
19 {
20 }
21 
22 void KinFamTest::JointTest()
23 {
24  double q;
25  Joint j;
26  j=Joint("Joint 1", Joint::None);
27  CPPUNIT_ASSERT_EQUAL(Joint::None,j.getType());
28  random(q);
29  CPPUNIT_ASSERT_EQUAL(j.pose(q),Frame::Identity());
30  random(q);
31  CPPUNIT_ASSERT_EQUAL(j.twist(q),Twist::Zero());
32  random(q);
33  j=Joint("Joint 2", Joint::RotX);
34  CPPUNIT_ASSERT_EQUAL(Joint::RotX,j.getType());
35  CPPUNIT_ASSERT_EQUAL(j.pose(q),Frame(Rotation::RotX(q)));
36  random(q);
37  CPPUNIT_ASSERT_EQUAL(j.twist(q),Twist(Vector::Zero(),Vector(q,0,0)));
38  random(q);
39  j=Joint("Joint 3", Joint::RotY);
40  CPPUNIT_ASSERT_EQUAL(Joint::RotY,j.getType());
41  CPPUNIT_ASSERT_EQUAL(j.pose(q),Frame(Rotation::RotY(q)));
42  random(q);
43  CPPUNIT_ASSERT_EQUAL(j.twist(q),Twist(Vector::Zero(),Vector(0,q,0)));
44  random(q);
45  j=Joint("Joint 4", Joint::RotZ);
46  CPPUNIT_ASSERT_EQUAL(Joint::RotZ,j.getType());
47  CPPUNIT_ASSERT_EQUAL(j.pose(q),Frame(Rotation::RotZ(q)));
48  random(q);
49  CPPUNIT_ASSERT_EQUAL(j.twist(q),Twist(Vector::Zero(),Vector(0,0,q)));
50  random(q);
51  j=Joint("Joint 5", Joint::TransX);
52  CPPUNIT_ASSERT_EQUAL(Joint::TransX,j.getType());
53  CPPUNIT_ASSERT_EQUAL(j.pose(q),Frame(Vector(q,0,0)));
54  random(q);
55  CPPUNIT_ASSERT_EQUAL(j.twist(q),Twist(Vector(q,0,0),Vector::Zero()));
56  random(q);
57  j=Joint("Joint 6", Joint::TransY);
58  CPPUNIT_ASSERT_EQUAL(Joint::TransY,j.getType());
59  CPPUNIT_ASSERT_EQUAL(j.pose(q),Frame(Vector(0,q,0)));
60  random(q);
61  CPPUNIT_ASSERT_EQUAL(j.twist(q),Twist(Vector(0,q,0),Vector::Zero()));
62  random(q);
63  j=Joint("Joint 7", Joint::TransZ);
64  CPPUNIT_ASSERT_EQUAL(Joint::TransZ,j.getType());
65  CPPUNIT_ASSERT_EQUAL(j.pose(q),Frame(Vector(0,0,q)));
66  random(q);
67  CPPUNIT_ASSERT_EQUAL(j.twist(q),Twist(Vector(0,0,q),Vector::Zero()));
68 
69 }
70 
71 void KinFamTest::SegmentTest()
72 {
73  Segment s;
74  double q,qdot;
75  Frame f,f1;
76  random(f);
77  s = Segment("Segment 0", Joint("Joint 0", Joint::None),f);
78  random(q);
79  random(qdot);
80  f1=s.getJoint().pose(q)*f;
81  CPPUNIT_ASSERT_EQUAL(f1,s.pose(q));
82  CPPUNIT_ASSERT_EQUAL(s.getJoint().twist(qdot).RefPoint(f1.p),s.twist(q,qdot));
83  random(f);
84  s = Segment("Segment 1", Joint("Joint 1", Joint::RotX),f);
85  random(q);
86  random(qdot);
87  f1=s.getJoint().pose(q)*f;
88  CPPUNIT_ASSERT_EQUAL(f1,s.pose(q));
89  CPPUNIT_ASSERT_EQUAL(s.getJoint().twist(qdot).RefPoint(f1.p),s.twist(q,qdot));
90  random(f);
91  s = Segment("Segment 3", Joint("Joint 3", Joint::RotY),f);
92  random(q);
93  random(qdot);
94  f1=s.getJoint().pose(q)*f;
95  CPPUNIT_ASSERT_EQUAL(f1,s.pose(q));
96  CPPUNIT_ASSERT_EQUAL(s.getJoint().twist(qdot).RefPoint(f1.p),s.twist(q,qdot));
97  random(f);
98  s = Segment("Segment 4", Joint("Joint 4", Joint::RotZ),f);
99  random(q);
100  random(qdot);
101  f1=s.getJoint().pose(q)*f;
102  CPPUNIT_ASSERT_EQUAL(f1,s.pose(q));
103  CPPUNIT_ASSERT_EQUAL(s.getJoint().twist(qdot).RefPoint(f1.p),s.twist(q,qdot));
104  random(f);
105  s = Segment("Segment 5", Joint("Joint 5", Joint::TransX),f);
106  random(q);
107  random(qdot);
108  f1=s.getJoint().pose(q)*f;
109  CPPUNIT_ASSERT_EQUAL(f1,s.pose(q));
110  CPPUNIT_ASSERT_EQUAL(s.getJoint().twist(qdot).RefPoint(f1.p),s.twist(q,qdot));
111  random(f);
112  s = Segment("Segment 6", Joint("Joint 6", Joint::TransY),f);
113  random(q);
114  random(qdot);
115  f1=s.getJoint().pose(q)*f;
116  CPPUNIT_ASSERT_EQUAL(f1,s.pose(q));
117  CPPUNIT_ASSERT_EQUAL(s.getJoint().twist(qdot).RefPoint(f1.p),s.twist(q,qdot));
118  random(f);
119  s = Segment("Segment 7", Joint("Joint 7", Joint::TransZ),f);
120  random(q);
121  random(qdot);
122  f1=s.getJoint().pose(q)*f;
123  CPPUNIT_ASSERT_EQUAL(f1,s.pose(q));
124  CPPUNIT_ASSERT_EQUAL(s.getJoint().twist(qdot).RefPoint(f1.p),s.twist(q,qdot));
125 }
126 
127 void KinFamTest::ChainTest()
128 {
129  Chain chain1;
130 
131  chain1.addSegment(Segment("Segment 0", Joint("Joint 0", Joint::RotZ),
132  Frame(Vector(0.0,0.0,0.0))));
133  chain1.addSegment(Segment("Segment 1", Joint("Joint 1", Joint::RotX),
134  Frame(Vector(0.0,0.0,0.9))));
135  chain1.addSegment(Segment("Segment 2", Joint("Joint 2", Joint::RotX),
136  Frame(Vector(0.0,0.0,1.2))));
137  chain1.addSegment(Segment("Segment 3", Joint("Joint 3", Joint::RotZ),
138  Frame(Vector(0.0,0.0,1.5))));
139  chain1.addSegment(Segment("Segment 4", Joint("Joint 4", Joint::RotX),
140  Frame(Vector(0.0,0.0,0.0))));
141  chain1.addSegment(Segment("Segment 5", Joint("Joint 5", Joint::RotZ),
142  Frame(Vector(0.0,0.0,0.4))));
143  CPPUNIT_ASSERT_EQUAL(chain1.getNrOfJoints(),(uint)6);
144  CPPUNIT_ASSERT_EQUAL(chain1.getNrOfSegments(),(uint)6);
145  chain1.addSegment(Segment("Segment 6", Joint("Joint 6", Joint::None),Frame(Vector(0.0,0.1,0.0))));
146  CPPUNIT_ASSERT_EQUAL(chain1.getNrOfJoints(),(uint)6);
147  CPPUNIT_ASSERT_EQUAL(chain1.getNrOfSegments(),(uint)7);
148 
149  Chain chain2 = chain1;
150  CPPUNIT_ASSERT_EQUAL(chain2.getNrOfJoints(),chain1.getNrOfJoints());
151  CPPUNIT_ASSERT_EQUAL(chain2.getNrOfSegments(),chain1.getNrOfSegments());
152  chain2.addChain(chain1);
153  CPPUNIT_ASSERT_EQUAL(chain2.getNrOfJoints(),chain1.getNrOfJoints()*(uint)2);
154  CPPUNIT_ASSERT_EQUAL(chain2.getNrOfSegments(),chain1.getNrOfSegments()*(uint)2);
155 }
156 
157 // forward declaration, see below
158 bool isSubtree(const SegmentMap::const_iterator container, const SegmentMap::const_iterator contained);
159 
160 void KinFamTest::TreeTest()
161 {
162  Tree tree1;
163  Segment segment1("Segment 1", Joint("Joint 1", Joint::None));
164  Segment segment2("Segment 2", Joint("Joint 2", Joint::RotX),Frame(Vector(0.1,0.2,0.3)));
165  Segment segment3("Segment 3", Joint("Joint 3", Joint::TransZ),Frame(Rotation::RotX(1.57)));
166  Segment segment4("Segment 4", Joint("Joint 4", Joint::RotX),Frame(Vector(0.1,0.2,0.3)));
167  Segment segment5("Segment 5", Joint("Joint 5", Joint::RotX),Frame(Vector(0.1,0.2,0.3)));
168  Segment segment6("Segment 6", Joint("Joint 6", Joint::RotX),Frame(Vector(0.1,0.2,0.3)));
169  Segment segment7("Segment 7", Joint("Joint 7", Joint::RotX),Frame(Vector(0.1,0.2,0.3)));
170 
171  std::cout<<tree1<<std::endl;
172 
173  CPPUNIT_ASSERT(tree1.addSegment(segment1,"root"));
174  CPPUNIT_ASSERT(tree1.addSegment(segment2,"root"));
175  CPPUNIT_ASSERT(tree1.addSegment(segment3,"Segment 1"));
176  CPPUNIT_ASSERT(tree1.addSegment(segment4,"Segment 3"));
177  CPPUNIT_ASSERT(!tree1.addSegment(segment1,"Segment 6"));
178  CPPUNIT_ASSERT(!tree1.addSegment(segment1,"Segment 4"));
179 
180  std::cout<<tree1<<std::endl;
181 
182  Tree tree2;
183  CPPUNIT_ASSERT(tree2.addSegment(segment5,"root"));
184  CPPUNIT_ASSERT(tree2.addSegment(segment6,"root"));
185  CPPUNIT_ASSERT(tree2.addSegment(segment7,"Segment 6"));
186 
187  std::cout<<tree2<<std::endl;
188 
189  Chain chain1;
190  chain1.addSegment(Segment("Segment 8", Joint("Joint 8", Joint::RotZ),
191  Frame(Vector(0.0,0.0,0.0))));
192  chain1.addSegment(Segment("Segment 9", Joint("Joint 9", Joint::RotX),
193  Frame(Vector(0.0,0.0,0.9))));
194  chain1.addSegment(Segment("Segment 10", Joint("Joint 10", Joint::RotX),
195  Frame(Vector(0.0,0.0,1.2))));
196  chain1.addSegment(Segment("Segment 11", Joint("Joint 11", Joint::RotZ),
197  Frame(Vector(0.0,0.0,1.5))));
198  chain1.addSegment(Segment("Segment 12", Joint("Joint 12", Joint::RotX),
199  Frame(Vector(0.0,0.0,0.0))));
200  chain1.addSegment(Segment("Segment 13", Joint("Joint 13", Joint::RotZ),
201  Frame(Vector(0.0,0.0,0.4))));
202 
203 
204  CPPUNIT_ASSERT(tree2.addChain(chain1, "Segment 6"));
205  std::cout<<tree2<<std::endl;
206  CPPUNIT_ASSERT(tree1.addTree(tree2, "Segment 2"));
207  std::cout<<tree1<<std::endl;
208 
209  Chain extract_chain1;
210  CPPUNIT_ASSERT(tree1.getChain("Segment 2", "Segment 4", extract_chain1));
211  Chain extract_chain2;
212  CPPUNIT_ASSERT(tree1.getChain("Segment 4", "Segment 2", extract_chain2));
213  CPPUNIT_ASSERT(tree1.getChain("Segment 4", "Segment 2", extract_chain2));
214  CPPUNIT_ASSERT(extract_chain1.getNrOfJoints()==extract_chain2.getNrOfJoints());
215  CPPUNIT_ASSERT(extract_chain1.getNrOfSegments()==extract_chain2.getNrOfSegments());
216  ChainFkSolverPos_recursive solver1(extract_chain1);
217  ChainFkSolverPos_recursive solver2(extract_chain2);
218 
219 
220  Frame f1, f2;
221  JntArray jnt1(extract_chain2.getNrOfJoints());
222  JntArray jnt2(extract_chain2.getNrOfJoints());
223  for (int i=0; i<(int)extract_chain2.getNrOfJoints(); i++){
224  jnt1(i) = (i+1)*2;
225  jnt2((int)extract_chain2.getNrOfJoints()-i-1) = jnt1(i);
226  }
227  solver1.JntToCart(jnt1, f1);
228  solver2.JntToCart(jnt2, f2);
229  CPPUNIT_ASSERT(f1 == f2.Inverse());
230 
231  Tree subtree;
232  const std::string subroot("Segment 2");
233  CPPUNIT_ASSERT(tree1.getSubTree(subroot, subtree));
234  std::cout << "Tree 1:" << std::endl << tree2str(tree1) << std::endl;
235  std::cout << "Subtree (rooted at " << subroot << "):" << std::endl << tree2str(subtree) << std::endl;
236  CPPUNIT_ASSERT(isSubtree(tree1.getSegment(subroot), subtree.getRootSegment()));
237  CPPUNIT_ASSERT(isSubtree(subtree.getRootSegment(), tree1.getSegment(subroot)));
238 
239  Segment segment101("Segment 101", Joint("Joint 101", Joint::RotZ), Frame(Vector(0.0,0.0,0.5)));
240  Segment segment102("Segment 102", Joint("Joint 102", Joint::RotZ), Frame(Vector(0.0,0.0,1.0)));
241  subtree.addSegment(segment101, subtree.getRootSegment()->first);
242  subtree.addSegment(segment102, subtree.getSegment("Segment 5")->first);
243  std::cout << "Subtree (rooted at " << subroot << "):" << std::endl << tree2str(subtree) << std::endl;
244  CPPUNIT_ASSERT(!isSubtree(tree1.getSegment(subroot), subtree.getRootSegment()));
245  CPPUNIT_ASSERT(isSubtree(subtree.getRootSegment(), tree1.getSegment(subroot)));
246 }
247 
248 //Utility to check if the set of segments in contained is a subset of container.
249 //In addition, all the children of a segment in contained must be present in
250 //container as children of the same segment.
251 bool isSubtree(const SegmentMap::const_iterator container, const SegmentMap::const_iterator contained) {
252  //Check that the container and contained point to the same link
253  if(container->first != contained->first)
254  return false;
255  //Check that each child of contained is a child of container
256  std::vector<SegmentMap::const_iterator> children = GetTreeElementChildren(contained->second);
257  for(unsigned int i=0; i < children.size(); i++) {
258  //look for a child of container whose name matches the one of the current child from contained
259  std::vector<SegmentMap::const_iterator>::const_iterator it = GetTreeElementChildren(container->second).begin();
260  while(it != GetTreeElementChildren(container->second).end()) {
261  if((*it)->first == children[i]->first)
262  break; //segment found, exit the loop
263  it++;
264  }
265  if(it == GetTreeElementChildren(container->second).end())
266  return false; //child of contained not found as child of container
267  //inspect recursively all the children
268  if(!isSubtree((*it), children[i]))
269  return false;
270  }
271  return true;
272 }
KDL::tree2str
std::string tree2str(const SegmentMap::const_iterator it, const std::string &separator, const std::string &preamble, unsigned int level)
Definition: kinfam_io.cpp:117
KDL::Frame::Identity
static Frame Identity()
Definition: frames.inl:700
KDL::Joint::RotZ
@ RotZ
Definition: joint.hpp:47
KDL::Chain::addChain
void addChain(const Chain &chain)
Definition: chain.cpp:61
std::string
KDL::Frame::Inverse
Frame Inverse() const
Gives back inverse transformation of a Frame.
Definition: frames.inl:422
kinfam_io.hpp
KDL::JntArray
Definition: jntarray.hpp:69
KDL::Tree::getRootSegment
SegmentMap::const_iterator getRootSegment() const
Definition: tree.hpp:186
KDL::Joint::RotY
@ RotY
Definition: joint.hpp:47
KDL::Vector::Zero
static Vector Zero()
Definition: frames.inl:138
KDL::Segment::getJoint
const Joint & getJoint() const
Definition: segment.hpp:118
std::vector< SegmentMap::const_iterator >
std::vector::size
T size(T... args)
KDL::Joint::TransZ
@ TransZ
Definition: joint.hpp:47
KDL::Joint::twist
Twist twist(const double &qdot) const
Definition: joint.cpp:93
KDL::random
IMETHOD void random(doubleVel &F)
Definition: framevel.hpp:65
KDL::Frame::p
Vector p
origine of the Frame
Definition: frames.hpp:574
frames_io.hpp
CPPUNIT_TEST_SUITE_REGISTRATION
CPPUNIT_TEST_SUITE_REGISTRATION(KinFamTest)
chainfksolverpos_recursive.hpp
KDL::Rotation::RotY
static Rotation RotY(double angle)
The Rot... static functions give the value of the appropriate rotation matrix back.
Definition: frames.inl:610
KinFamTest::SegmentTest
void SegmentTest()
Definition: kinfamtest.cpp:71
KinFamTest::JointTest
void JointTest()
Definition: kinfamtest.cpp:22
isSubtree
bool isSubtree(const SegmentMap::const_iterator container, const SegmentMap::const_iterator contained)
Definition: kinfamtest.cpp:251
GetTreeElementChildren
#define GetTreeElementChildren(tree_element)
Definition: tree.hpp:59
KDL
Definition: kukaLWR_DHnew.cpp:25
KDL::Chain::getNrOfSegments
unsigned int getNrOfSegments() const
Definition: chain.hpp:76
KDL::Joint::pose
Frame pose(const double &q) const
Definition: joint.cpp:68
KDL::Vector
A concrete implementation of a 3 dimensional vector class.
Definition: frames.hpp:162
std::cout
KDL::Twist
represents both translational and rotational velocities.
Definition: frames.hpp:723
KDL::Tree::addChain
bool addChain(const Chain &chain, const std::string &hook_name)
Definition: tree.cpp:82
kinfamtest.hpp
KDL::Rotation::RotZ
static Rotation RotZ(double angle)
The Rot... static functions give the value of the appropriate rotation matrix back.
Definition: frames.inl:615
KDL::Joint::RotX
@ RotX
Definition: joint.hpp:47
KinFamTest::ChainTest
void ChainTest()
Definition: kinfamtest.cpp:127
KDL::Segment::pose
Frame pose(const double &q) const
Definition: segment.cpp:57
KDL::Frame
represents a frame transformation in 3D space (rotation + translation)
Definition: frames.hpp:572
KDL::ChainFkSolverPos_recursive::JntToCart
virtual int JntToCart(const JntArray &q_in, Frame &p_out, int segmentNr=-1)
Definition: chainfksolverpos_recursive.cpp:34
KDL::Joint::TransX
@ TransX
Definition: joint.hpp:47
KDL::Tree::getSubTree
bool getSubTree(const std::string &segment_name, Tree &tree) const
Definition: tree.cpp:165
KinFamTest::setUp
void setUp()
Definition: kinfamtest.cpp:14
KDL::Segment::twist
Twist twist(const double &q, const double &qdot) const
Definition: segment.cpp:62
KDL::Tree
This class encapsulates a tree kinematic interconnection structure. It is built out of segments.
Definition: tree.hpp:99
KDL::Joint::None
@ None
Definition: joint.hpp:47
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::Tree::addSegment
bool addSegment(const Segment &segment, const std::string &hook_name)
Definition: tree.cpp:54
KDL::Twist::RefPoint
Twist RefPoint(const Vector &v_base_AB) const
Definition: frames.inl:302
KinFamTest::TreeTest
void TreeTest()
Definition: kinfamtest.cpp:160
KDL::Twist::Zero
static Twist Zero()
Definition: frames.inl:290
std::endl
T endl(T... args)
KDL::Chain::addSegment
void addSegment(const Segment &segment)
Definition: chain.cpp:53
KDL::Tree::addTree
bool addTree(const Tree &tree, const std::string &hook_name)
Definition: tree.cpp:93
KDL::Tree::getChain
bool getChain(const std::string &chain_root, const std::string &chain_tip, Chain &chain) const
Definition: tree.cpp:116
KinFamTest::tearDown
void tearDown()
Definition: kinfamtest.cpp:18
KDL::Rotation::RotX
static Rotation RotX(double angle)
The Rot... static functions give the value of the appropriate rotation matrix back.
Definition: frames.inl:605
KDL::Joint::TransY
@ TransY
Definition: joint.hpp:47
KDL::Joint::getType
const JointType & getType() const
Definition: joint.hpp:159
KinFamTest
Definition: kinfamtest.hpp:13
KDL::Joint
This class encapsulates a simple joint, that is with one parameterized degree of freedom and with sca...
Definition: joint.hpp:45
KDL::ChainFkSolverPos_recursive
Definition: chainfksolverpos_recursive.hpp:36
KDL::Chain
This class encapsulates a serial kinematic interconnection structure. It is built out of segments.
Definition: chain.hpp:35
KDL::Tree::getSegment
SegmentMap::const_iterator getSegment(const std::string &segment_name) const
Definition: tree.hpp:177
KDL::Chain::getNrOfJoints
unsigned int getNrOfJoints() const
Definition: chain.hpp:71
Generated on Fri Oct 31 2025 04:37:36 for orocos_kdl by   doxygen 1.8.17