kinfamtest.cpp
Go to the documentation of this file.
00001 #include "kinfamtest.hpp"
00002 #include <frames_io.hpp>
00003 #include <kinfam_io.hpp>
00004 #include <chainfksolverpos_recursive.hpp>
00005 
00006 CPPUNIT_TEST_SUITE_REGISTRATION( KinFamTest );
00007 
00008 #ifdef  __APPLE__
00009 typedef unsigned int uint;
00010 #endif
00011 
00012 using namespace KDL;
00013 using namespace std;
00014 
00015 void KinFamTest::setUp()
00016 {
00017 }
00018 
00019 void  KinFamTest::tearDown()
00020 {
00021 }
00022 
00023 void KinFamTest::JointTest()
00024 {
00025     double q;
00026     Joint j;
00027     j=Joint("Joint 1", Joint::None);
00028     CPPUNIT_ASSERT_EQUAL(Joint::None,j.getType());
00029     random(q);
00030     CPPUNIT_ASSERT_EQUAL(j.pose(q),Frame::Identity());
00031     random(q);
00032     CPPUNIT_ASSERT_EQUAL(j.twist(q),Twist::Zero());
00033     random(q);
00034     j=Joint("Joint 2", Joint::RotX);
00035     CPPUNIT_ASSERT_EQUAL(Joint::RotX,j.getType());
00036     CPPUNIT_ASSERT_EQUAL(j.pose(q),Frame(Rotation::RotX(q)));
00037     random(q);
00038     CPPUNIT_ASSERT_EQUAL(j.twist(q),Twist(Vector::Zero(),Vector(q,0,0)));
00039     random(q);
00040     j=Joint("Joint 3", Joint::RotY);
00041     CPPUNIT_ASSERT_EQUAL(Joint::RotY,j.getType());
00042     CPPUNIT_ASSERT_EQUAL(j.pose(q),Frame(Rotation::RotY(q)));
00043     random(q);
00044     CPPUNIT_ASSERT_EQUAL(j.twist(q),Twist(Vector::Zero(),Vector(0,q,0)));
00045     random(q);
00046     j=Joint("Joint 4", Joint::RotZ);
00047     CPPUNIT_ASSERT_EQUAL(Joint::RotZ,j.getType());
00048     CPPUNIT_ASSERT_EQUAL(j.pose(q),Frame(Rotation::RotZ(q)));
00049     random(q);
00050     CPPUNIT_ASSERT_EQUAL(j.twist(q),Twist(Vector::Zero(),Vector(0,0,q)));
00051     random(q);
00052     j=Joint("Joint 5", Joint::TransX);
00053     CPPUNIT_ASSERT_EQUAL(Joint::TransX,j.getType());
00054     CPPUNIT_ASSERT_EQUAL(j.pose(q),Frame(Vector(q,0,0)));
00055     random(q);
00056     CPPUNIT_ASSERT_EQUAL(j.twist(q),Twist(Vector(q,0,0),Vector::Zero()));
00057     random(q);
00058     j=Joint("Joint 6", Joint::TransY);
00059     CPPUNIT_ASSERT_EQUAL(Joint::TransY,j.getType());
00060     CPPUNIT_ASSERT_EQUAL(j.pose(q),Frame(Vector(0,q,0)));
00061     random(q);
00062     CPPUNIT_ASSERT_EQUAL(j.twist(q),Twist(Vector(0,q,0),Vector::Zero()));
00063     random(q);
00064     j=Joint("Joint 7", Joint::TransZ);
00065     CPPUNIT_ASSERT_EQUAL(Joint::TransZ,j.getType());
00066     CPPUNIT_ASSERT_EQUAL(j.pose(q),Frame(Vector(0,0,q)));
00067     random(q);
00068     CPPUNIT_ASSERT_EQUAL(j.twist(q),Twist(Vector(0,0,q),Vector::Zero()));
00069 
00070 }
00071 
00072 void KinFamTest::SegmentTest()
00073 {
00074     Segment s;
00075     double q,qdot;
00076     Frame f,f1;
00077     random(f);
00078     s = Segment("Segment 0", Joint("Joint 0", Joint::None),f);
00079     random(q);
00080     random(qdot);
00081     f1=s.getJoint().pose(q)*f;
00082     CPPUNIT_ASSERT_EQUAL(f1,s.pose(q));
00083     CPPUNIT_ASSERT_EQUAL(s.getJoint().twist(qdot).RefPoint(f1.p),s.twist(q,qdot));
00084     random(f);
00085     s = Segment("Segment 1", Joint("Joint 1", Joint::RotX),f);
00086     random(q);
00087     random(qdot);
00088     f1=s.getJoint().pose(q)*f;
00089     CPPUNIT_ASSERT_EQUAL(f1,s.pose(q));
00090     CPPUNIT_ASSERT_EQUAL(s.getJoint().twist(qdot).RefPoint(f1.p),s.twist(q,qdot));
00091     random(f);
00092     s = Segment("Segment 3", Joint("Joint 3", Joint::RotY),f);
00093     random(q);
00094     random(qdot);
00095     f1=s.getJoint().pose(q)*f;
00096     CPPUNIT_ASSERT_EQUAL(f1,s.pose(q));
00097     CPPUNIT_ASSERT_EQUAL(s.getJoint().twist(qdot).RefPoint(f1.p),s.twist(q,qdot));
00098     random(f);
00099     s = Segment("Segment 4", Joint("Joint 4", Joint::RotZ),f);
00100     random(q);
00101     random(qdot);
00102     f1=s.getJoint().pose(q)*f;
00103     CPPUNIT_ASSERT_EQUAL(f1,s.pose(q));
00104     CPPUNIT_ASSERT_EQUAL(s.getJoint().twist(qdot).RefPoint(f1.p),s.twist(q,qdot));
00105     random(f);
00106     s = Segment("Segment 5", Joint("Joint 5", Joint::TransX),f);
00107     random(q);
00108     random(qdot);
00109     f1=s.getJoint().pose(q)*f;
00110     CPPUNIT_ASSERT_EQUAL(f1,s.pose(q));
00111     CPPUNIT_ASSERT_EQUAL(s.getJoint().twist(qdot).RefPoint(f1.p),s.twist(q,qdot));
00112     random(f);
00113     s = Segment("Segment 6", Joint("Joint 6", Joint::TransY),f);
00114     random(q);
00115     random(qdot);
00116     f1=s.getJoint().pose(q)*f;
00117     CPPUNIT_ASSERT_EQUAL(f1,s.pose(q));
00118     CPPUNIT_ASSERT_EQUAL(s.getJoint().twist(qdot).RefPoint(f1.p),s.twist(q,qdot));
00119     random(f);
00120     s = Segment("Segment 7", Joint("Joint 7", Joint::TransZ),f);
00121     random(q);
00122     random(qdot);
00123     f1=s.getJoint().pose(q)*f;
00124     CPPUNIT_ASSERT_EQUAL(f1,s.pose(q));
00125     CPPUNIT_ASSERT_EQUAL(s.getJoint().twist(qdot).RefPoint(f1.p),s.twist(q,qdot));
00126 }
00127 
00128 void KinFamTest::ChainTest()
00129 {
00130     Chain chain1;
00131 
00132     chain1.addSegment(Segment("Segment 0", Joint("Joint 0", Joint::RotZ),
00133                               Frame(Vector(0.0,0.0,0.0))));
00134     chain1.addSegment(Segment("Segment 1", Joint("Joint 1", Joint::RotX),
00135                               Frame(Vector(0.0,0.0,0.9))));
00136     chain1.addSegment(Segment("Segment 2", Joint("Joint 2", Joint::RotX),
00137                               Frame(Vector(0.0,0.0,1.2))));
00138     chain1.addSegment(Segment("Segment 3", Joint("Joint 3", Joint::RotZ),
00139                               Frame(Vector(0.0,0.0,1.5))));
00140     chain1.addSegment(Segment("Segment 4", Joint("Joint 4", Joint::RotX),
00141                               Frame(Vector(0.0,0.0,0.0))));
00142     chain1.addSegment(Segment("Segment 5", Joint("Joint 5", Joint::RotZ),
00143                               Frame(Vector(0.0,0.0,0.4))));
00144     CPPUNIT_ASSERT_EQUAL(chain1.getNrOfJoints(),(uint)6);
00145     CPPUNIT_ASSERT_EQUAL(chain1.getNrOfSegments(),(uint)6);
00146     chain1.addSegment(Segment("Segment 6", Joint("Joint 6", Joint::None),Frame(Vector(0.0,0.1,0.0))));
00147     CPPUNIT_ASSERT_EQUAL(chain1.getNrOfJoints(),(uint)6);
00148     CPPUNIT_ASSERT_EQUAL(chain1.getNrOfSegments(),(uint)7);
00149 
00150     Chain chain2 = chain1;
00151     CPPUNIT_ASSERT_EQUAL(chain2.getNrOfJoints(),chain1.getNrOfJoints());
00152     CPPUNIT_ASSERT_EQUAL(chain2.getNrOfSegments(),chain1.getNrOfSegments());
00153     chain2.addChain(chain1);
00154     CPPUNIT_ASSERT_EQUAL(chain2.getNrOfJoints(),chain1.getNrOfJoints()*(uint)2);
00155     CPPUNIT_ASSERT_EQUAL(chain2.getNrOfSegments(),chain1.getNrOfSegments()*(uint)2);
00156 }
00157 
00158 void KinFamTest::TreeTest()
00159 {
00160     Tree tree1;
00161     Segment segment1("Segment 1", Joint("Joint 1", Joint::None));
00162     Segment segment2("Segment 2", Joint("Joint 2", Joint::RotX),Frame(Vector(0.1,0.2,0.3)));
00163     Segment segment3("Segment 3", Joint("Joint 3", Joint::TransZ),Frame(Rotation::RotX(1.57)));
00164     Segment segment4("Segment 4", Joint("Joint 4", Joint::RotX),Frame(Vector(0.1,0.2,0.3)));
00165     Segment segment5("Segment 5", Joint("Joint 5", Joint::RotX),Frame(Vector(0.1,0.2,0.3)));
00166     Segment segment6("Segment 6", Joint("Joint 6", Joint::RotX),Frame(Vector(0.1,0.2,0.3)));
00167     Segment segment7("Segment 7", Joint("Joint 7", Joint::RotX),Frame(Vector(0.1,0.2,0.3)));
00168 
00169     cout<<tree1<<endl;
00170 
00171     CPPUNIT_ASSERT(tree1.addSegment(segment1,"root"));
00172     CPPUNIT_ASSERT(tree1.addSegment(segment2,"root"));
00173     CPPUNIT_ASSERT(tree1.addSegment(segment3,"Segment 1"));
00174     CPPUNIT_ASSERT(tree1.addSegment(segment4,"Segment 3"));
00175     CPPUNIT_ASSERT(!tree1.addSegment(segment1,"Segment 6"));
00176     CPPUNIT_ASSERT(!tree1.addSegment(segment1,"Segment 4"));
00177 
00178     cout<<tree1<<endl;
00179 
00180     Tree tree2;
00181     CPPUNIT_ASSERT(tree2.addSegment(segment5,"root"));
00182     CPPUNIT_ASSERT(tree2.addSegment(segment6,"root"));
00183     CPPUNIT_ASSERT(tree2.addSegment(segment7,"Segment 6"));
00184 
00185     cout<<tree2<<endl;
00186 
00187     Chain chain1;
00188     chain1.addSegment(Segment("Segment 8", Joint("Joint 8", Joint::RotZ),
00189                               Frame(Vector(0.0,0.0,0.0))));
00190     chain1.addSegment(Segment("Segment 9", Joint("Joint 9", Joint::RotX),
00191                               Frame(Vector(0.0,0.0,0.9))));
00192     chain1.addSegment(Segment("Segment 10", Joint("Joint 10", Joint::RotX),
00193                               Frame(Vector(0.0,0.0,1.2))));
00194     chain1.addSegment(Segment("Segment 11", Joint("Joint 11", Joint::RotZ),
00195                               Frame(Vector(0.0,0.0,1.5))));
00196     chain1.addSegment(Segment("Segment 12", Joint("Joint 12", Joint::RotX),
00197                               Frame(Vector(0.0,0.0,0.0))));
00198     chain1.addSegment(Segment("Segment 13", Joint("Joint 13", Joint::RotZ),
00199                               Frame(Vector(0.0,0.0,0.4))));
00200 
00201 
00202     CPPUNIT_ASSERT(tree2.addChain(chain1, "Segment 6"));
00203     cout<<tree2<<endl;
00204     CPPUNIT_ASSERT(tree1.addTree(tree2, "Segment 2"));
00205     cout<<tree1<<endl;
00206 
00207     Chain extract_chain1;
00208     CPPUNIT_ASSERT(tree1.getChain("Segment 2", "Segment 4", extract_chain1));
00209     Chain extract_chain2; 
00210     CPPUNIT_ASSERT(tree1.getChain("Segment 4", "Segment 2", extract_chain2));
00211     CPPUNIT_ASSERT(tree1.getChain("Segment 4", "Segment 2", extract_chain2));
00212     CPPUNIT_ASSERT(extract_chain1.getNrOfJoints()==extract_chain2.getNrOfJoints());
00213     CPPUNIT_ASSERT(extract_chain1.getNrOfSegments()==extract_chain2.getNrOfSegments());
00214     ChainFkSolverPos_recursive solver1(extract_chain1);
00215     ChainFkSolverPos_recursive solver2(extract_chain2);
00216 
00217 
00218     Frame f1, f2;
00219     JntArray jnt1(extract_chain2.getNrOfJoints());
00220     JntArray jnt2(extract_chain2.getNrOfJoints());
00221     for (int i=0; i<(int)extract_chain2.getNrOfJoints(); i++){
00222       jnt1(i) = (i+1)*2;
00223       jnt2((int)extract_chain2.getNrOfJoints()-i-1) = jnt1(i);
00224     }
00225     solver1.JntToCart(jnt1, f1);
00226     solver2.JntToCart(jnt2, f2);
00227     CPPUNIT_ASSERT(f1 == f2.Inverse());
00228 }
00229 
00230 
00231 


orocos_kdl
Author(s):
autogenerated on Fri Jun 14 2019 19:33:22