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
