9 typedef unsigned int uint;
82 CPPUNIT_ASSERT_EQUAL(f1,s.
pose(q));
89 CPPUNIT_ASSERT_EQUAL(f1,s.
pose(q));
96 CPPUNIT_ASSERT_EQUAL(f1,s.
pose(q));
103 CPPUNIT_ASSERT_EQUAL(f1,s.
pose(q));
110 CPPUNIT_ASSERT_EQUAL(f1,s.
pose(q));
117 CPPUNIT_ASSERT_EQUAL(f1,s.
pose(q));
124 CPPUNIT_ASSERT_EQUAL(f1,s.
pose(q));
150 Chain chain2 = chain1;
159 bool isSubtree(
const SegmentMap::const_iterator container,
const SegmentMap::const_iterator contained);
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"));
184 CPPUNIT_ASSERT(tree2.
addSegment(segment5,
"root"));
185 CPPUNIT_ASSERT(tree2.
addSegment(segment6,
"root"));
186 CPPUNIT_ASSERT(tree2.
addSegment(segment7,
"Segment 6"));
205 CPPUNIT_ASSERT(tree2.
addChain(chain1,
"Segment 6"));
207 CPPUNIT_ASSERT(tree1.
addTree(tree2,
"Segment 2"));
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());
222 JntArray jnt1(extract_chain2.getNrOfJoints());
223 JntArray jnt2(extract_chain2.getNrOfJoints());
224 for (
int i=0; i<(int)extract_chain2.getNrOfJoints(); i++){
226 jnt2((
int)extract_chain2.getNrOfJoints()-i-1) = jnt1(i);
228 solver1.JntToCart(jnt1, f1);
230 CPPUNIT_ASSERT(f1 == f2.
Inverse());
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;
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;
252 bool isSubtree(
const SegmentMap::const_iterator container,
const SegmentMap::const_iterator contained) {
254 if(container->first != contained->first)
258 for(
unsigned int i=0; i < children.size(); i++) {
260 std::vector<SegmentMap::const_iterator>::const_iterator it =
GetTreeElementChildren(container->second).begin();
262 if((*it)->first == children[i]->first)
SegmentMap::const_iterator getSegment(const std::string &segment_name) const
virtual int JntToCart(const JntArray &q_in, Frame &p_out, int segmentNr=-1)
This class encapsulates a simple segment, that is a "rigid body" (i.e., a frame and a rigid body...
static Rotation RotX(double angle)
The Rot... static functions give the value of the appropriate rotation matrix back.
Twist RefPoint(const Vector &v_base_AB) const
This class encapsulates a serial kinematic interconnection structure. It is built out of segments...
static Rotation RotZ(double angle)
The Rot... static functions give the value of the appropriate rotation matrix back.
unsigned int getNrOfSegments() const
bool getSubTree(const std::string &segment_name, Tree &tree) const
void addSegment(const Segment &segment)
This class represents an fixed size array containing joint values of a KDL::Chain.
Frame pose(const double &q) const
std::string tree2str(const SegmentMap::const_iterator it, const std::string &separator, const std::string &preamble, unsigned int level)
static Rotation RotY(double angle)
The Rot... static functions give the value of the appropriate rotation matrix back.
#define GetTreeElementChildren(tree_element)
Frame pose(const double &q) const
Twist twist(const double &q, const double &qdot) const
represents both translational and rotational velocities.
bool addChain(const Chain &chain, const std::string &hook_name)
A concrete implementation of a 3 dimensional vector class.
const Joint & getJoint() const
bool addTree(const Tree &tree, const std::string &hook_name)
Vector p
origine of the Frame
void addChain(const Chain &chain)
unsigned int getNrOfJoints() const
Frame Inverse() const
Gives back inverse transformation of a Frame.
bool getChain(const std::string &chain_root, const std::string &chain_tip, Chain &chain) const
bool isSubtree(const SegmentMap::const_iterator container, const SegmentMap::const_iterator contained)
CPPUNIT_TEST_SUITE_REGISTRATION(KinFamTest)
represents a frame transformation in 3D space (rotation + translation)
This class encapsulates a simple joint, that is with one parameterized degree of freedom and with sca...
bool addSegment(const Segment &segment, const std::string &hook_name)
IMETHOD void random(Vector &a)
addDelta operator for displacement rotational velocity.
This class encapsulates a tree kinematic interconnection structure. It is built out of segments...
Twist twist(const double &qdot) const
const JointType & getType() const