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 // forward declaration, see below
159 bool isSubtree(const SegmentMap::const_iterator container, const SegmentMap::const_iterator contained);
160 
161 void KinFamTest::TreeTest()
162 {
163  Tree tree1;
164  Segment segment1("Segment 1", Joint("Joint 1", Joint::None));
165  Segment segment2("Segment 2", Joint("Joint 2", Joint::RotX),Frame(Vector(0.1,0.2,0.3)));
166  Segment segment3("Segment 3", Joint("Joint 3", Joint::TransZ),Frame(Rotation::RotX(1.57)));
167  Segment segment4("Segment 4", Joint("Joint 4", Joint::RotX),Frame(Vector(0.1,0.2,0.3)));
168  Segment segment5("Segment 5", Joint("Joint 5", Joint::RotX),Frame(Vector(0.1,0.2,0.3)));
169  Segment segment6("Segment 6", Joint("Joint 6", Joint::RotX),Frame(Vector(0.1,0.2,0.3)));
170  Segment segment7("Segment 7", Joint("Joint 7", Joint::RotX),Frame(Vector(0.1,0.2,0.3)));
171 
172  cout<<tree1<<endl;
173 
174  CPPUNIT_ASSERT(tree1.addSegment(segment1,"root"));
175  CPPUNIT_ASSERT(tree1.addSegment(segment2,"root"));
176  CPPUNIT_ASSERT(tree1.addSegment(segment3,"Segment 1"));
177  CPPUNIT_ASSERT(tree1.addSegment(segment4,"Segment 3"));
178  CPPUNIT_ASSERT(!tree1.addSegment(segment1,"Segment 6"));
179  CPPUNIT_ASSERT(!tree1.addSegment(segment1,"Segment 4"));
180 
181  cout<<tree1<<endl;
182 
183  Tree tree2;
184  CPPUNIT_ASSERT(tree2.addSegment(segment5,"root"));
185  CPPUNIT_ASSERT(tree2.addSegment(segment6,"root"));
186  CPPUNIT_ASSERT(tree2.addSegment(segment7,"Segment 6"));
187 
188  cout<<tree2<<endl;
189 
190  Chain chain1;
191  chain1.addSegment(Segment("Segment 8", Joint("Joint 8", Joint::RotZ),
192  Frame(Vector(0.0,0.0,0.0))));
193  chain1.addSegment(Segment("Segment 9", Joint("Joint 9", Joint::RotX),
194  Frame(Vector(0.0,0.0,0.9))));
195  chain1.addSegment(Segment("Segment 10", Joint("Joint 10", Joint::RotX),
196  Frame(Vector(0.0,0.0,1.2))));
197  chain1.addSegment(Segment("Segment 11", Joint("Joint 11", Joint::RotZ),
198  Frame(Vector(0.0,0.0,1.5))));
199  chain1.addSegment(Segment("Segment 12", Joint("Joint 12", Joint::RotX),
200  Frame(Vector(0.0,0.0,0.0))));
201  chain1.addSegment(Segment("Segment 13", Joint("Joint 13", Joint::RotZ),
202  Frame(Vector(0.0,0.0,0.4))));
203 
204 
205  CPPUNIT_ASSERT(tree2.addChain(chain1, "Segment 6"));
206  cout<<tree2<<endl;
207  CPPUNIT_ASSERT(tree1.addTree(tree2, "Segment 2"));
208  cout<<tree1<<endl;
209 
210  Chain extract_chain1;
211  CPPUNIT_ASSERT(tree1.getChain("Segment 2", "Segment 4", extract_chain1));
212  Chain extract_chain2;
213  CPPUNIT_ASSERT(tree1.getChain("Segment 4", "Segment 2", extract_chain2));
214  CPPUNIT_ASSERT(tree1.getChain("Segment 4", "Segment 2", extract_chain2));
215  CPPUNIT_ASSERT(extract_chain1.getNrOfJoints()==extract_chain2.getNrOfJoints());
216  CPPUNIT_ASSERT(extract_chain1.getNrOfSegments()==extract_chain2.getNrOfSegments());
217  ChainFkSolverPos_recursive solver1(extract_chain1);
218  ChainFkSolverPos_recursive solver2(extract_chain2);
219 
220 
221  Frame f1, f2;
222  JntArray jnt1(extract_chain2.getNrOfJoints());
223  JntArray jnt2(extract_chain2.getNrOfJoints());
224  for (int i=0; i<(int)extract_chain2.getNrOfJoints(); i++){
225  jnt1(i) = (i+1)*2;
226  jnt2((int)extract_chain2.getNrOfJoints()-i-1) = jnt1(i);
227  }
228  solver1.JntToCart(jnt1, f1);
229  solver2.JntToCart(jnt2, f2);
230  CPPUNIT_ASSERT(f1 == f2.Inverse());
231 
232  Tree subtree;
233  const std::string subroot("Segment 2");
234  CPPUNIT_ASSERT(tree1.getSubTree(subroot, subtree));
235  cout << "Tree 1:" << endl << tree2str(tree1) << endl;
236  cout << "Subtree (rooted at " << subroot << "):" << endl << tree2str(subtree) << endl;
237  CPPUNIT_ASSERT(isSubtree(tree1.getSegment(subroot), subtree.getRootSegment()));
238  CPPUNIT_ASSERT(isSubtree(subtree.getRootSegment(), tree1.getSegment(subroot)));
239 
240  Segment segment101("Segment 101", Joint("Joint 101", Joint::RotZ), Frame(Vector(0.0,0.0,0.5)));
241  Segment segment102("Segment 102", Joint("Joint 102", Joint::RotZ), Frame(Vector(0.0,0.0,1.0)));
242  subtree.addSegment(segment101, subtree.getRootSegment()->first);
243  subtree.addSegment(segment102, subtree.getSegment("Segment 5")->first);
244  cout << "Subtree (rooted at " << subroot << "):" << endl << tree2str(subtree) << endl;
245  CPPUNIT_ASSERT(!isSubtree(tree1.getSegment(subroot), subtree.getRootSegment()));
246  CPPUNIT_ASSERT(isSubtree(subtree.getRootSegment(), tree1.getSegment(subroot)));
247 }
248 
249 //Utility to check if the set of segments in contained is a subset of container.
250 //In addition, all the children of a segment in contained must be present in
251 //container as children of the same segment.
252 bool isSubtree(const SegmentMap::const_iterator container, const SegmentMap::const_iterator contained) {
253  //Check that the container and contained point to the same link
254  if(container->first != contained->first)
255  return false;
256  //Check that each child of contained is a child of container
257  std::vector<SegmentMap::const_iterator> children = GetTreeElementChildren(contained->second);
258  for(unsigned int i=0; i < children.size(); i++) {
259  //look for a child of container whose name matches the one of the current child from contained
260  std::vector<SegmentMap::const_iterator>::const_iterator it = GetTreeElementChildren(container->second).begin();
261  while(it != GetTreeElementChildren(container->second).end()) {
262  if((*it)->first == children[i]->first)
263  break; //segment found, exit the loop
264  it++;
265  }
266  if(it == GetTreeElementChildren(container->second).end())
267  return false; //child of contained not found as child of container
268  //inspect recursively all the children
269  if(!isSubtree((*it), children[i]))
270  return false;
271  }
272  return true;
273 }
chainfksolverpos_recursive.hpp
KDL::Twist::Zero
static Twist Zero()
Definition: frames.hpp:291
KDL::Tree::getSegment
SegmentMap::const_iterator getSegment(const std::string &segment_name) const
Definition: tree.hpp:177
KDL::ChainFkSolverPos_recursive::JntToCart
virtual int JntToCart(const JntArray &q_in, Frame &p_out, int segmentNr=-1)
Definition: chainfksolverpos_recursive.cpp:34
KinFamTest::JointTest
void JointTest()
Definition: kinfamtest.cpp:23
KDL::ChainFkSolverPos_recursive
Definition: chainfksolverpos_recursive.hpp:36
KinFamTest::TreeTest
void TreeTest()
Definition: kinfamtest.cpp:161
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:606
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:616
KDL::Chain::getNrOfSegments
unsigned int getNrOfSegments() const
Definition: chain.hpp:76
KDL::Tree::getSubTree
bool getSubTree(const std::string &segment_name, Tree &tree) const
Definition: tree.cpp:166
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
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
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:611
GetTreeElementChildren
#define GetTreeElementChildren(tree_element)
Definition: tree.hpp:59
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
isSubtree
bool isSubtree(const SegmentMap::const_iterator container, const SegmentMap::const_iterator contained)
Definition: kinfamtest.cpp:252
CPPUNIT_TEST_SUITE_REGISTRATION
CPPUNIT_TEST_SUITE_REGISTRATION(KinFamTest)
KDL::Frame::Identity
static Frame Identity()
Definition: frames.hpp:701
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:1215
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 Fri Mar 12 2021 03:05:43