9 typedef unsigned int uint;
81 CPPUNIT_ASSERT_EQUAL(f1,s.
pose(q));
88 CPPUNIT_ASSERT_EQUAL(f1,s.
pose(q));
95 CPPUNIT_ASSERT_EQUAL(f1,s.
pose(q));
102 CPPUNIT_ASSERT_EQUAL(f1,s.
pose(q));
109 CPPUNIT_ASSERT_EQUAL(f1,s.
pose(q));
116 CPPUNIT_ASSERT_EQUAL(f1,s.
pose(q));
123 CPPUNIT_ASSERT_EQUAL(f1,s.
pose(q));
149 Chain chain2 = chain1;
158 bool isSubtree(
const SegmentMap::const_iterator container,
const SegmentMap::const_iterator contained);
171 std::cout<<tree1<<std::endl;
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"));
180 std::cout<<tree1<<std::endl;
183 CPPUNIT_ASSERT(tree2.
addSegment(segment5,
"root"));
184 CPPUNIT_ASSERT(tree2.
addSegment(segment6,
"root"));
185 CPPUNIT_ASSERT(tree2.
addSegment(segment7,
"Segment 6"));
187 std::cout<<tree2<<std::endl;
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;
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());
221 JntArray jnt1(extract_chain2.getNrOfJoints());
222 JntArray jnt2(extract_chain2.getNrOfJoints());
223 for (
int i=0; i<(int)extract_chain2.getNrOfJoints(); i++){
225 jnt2((
int)extract_chain2.getNrOfJoints()-i-1) = jnt1(i);
227 solver1.JntToCart(jnt1, f1);
229 CPPUNIT_ASSERT(f1 == f2.
Inverse());
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;
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;
251 bool isSubtree(
const SegmentMap::const_iterator container,
const SegmentMap::const_iterator contained) {
253 if(container->first != contained->first)
257 for(
unsigned int i=0; i < children.size(); i++) {
259 std::vector<SegmentMap::const_iterator>::const_iterator it =
GetTreeElementChildren(container->second).begin();
261 if((*it)->first == children[i]->first)
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 ine...
This class encapsulates a serial kinematic interconnection structure. It is built out of segments...
unsigned int getNrOfSegments() const
void addSegment(const Segment &segment)
This class represents an fixed size array containing joint values of a KDL::Chain.
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)
const Joint & getJoint() const
represents both translational and rotational velocities.
bool addChain(const Chain &chain, const std::string &hook_name)
unsigned int getNrOfJoints() const
A concrete implementation of a 3 dimensional vector class.
Frame Inverse() const
Gives back inverse transformation of a Frame.
Frame pose(const double &q) const
bool addTree(const Tree &tree, const std::string &hook_name)
Vector p
origine of the Frame
void addChain(const Chain &chain)
static Rotation RotZ(double angle)
The Rot... static functions give the value of the appropriate rotation matrix back.
bool getChain(const std::string &chain_root, const std::string &chain_tip, Chain &chain) const
Twist twist(const double &qdot) const
const JointType & getType() const
static Rotation RotX(double angle)
The Rot... static functions give the value of the appropriate rotation matrix back.
bool getSubTree(const std::string &segment_name, Tree &tree) 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)
Twist twist(const double &q, const double &qdot) const
Twist RefPoint(const Vector &v_base_AB) const
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...
Frame pose(const double &q) const
SegmentMap::const_iterator getSegment(const std::string &segment_name) const