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