00001 #include "solvertest.hpp"
00002 #include <frames_io.hpp>
00003 #include <framevel_io.hpp>
00004 #include <kinfam_io.hpp>
00005 #include <time.h>
00006
00007
00008 CPPUNIT_TEST_SUITE_REGISTRATION( SolverTest );
00009
00010 using namespace KDL;
00011
00012 void SolverTest::setUp()
00013 {
00014 srand( (unsigned)time( NULL ));
00015
00016 chain1.addSegment(Segment("Segment 1", Joint("Joint 1", Joint::RotZ),
00017 Frame(Vector(0.0,0.0,0.0))));
00018 chain1.addSegment(Segment("Segment 2", Joint("Joint 2", Joint::RotX),
00019 Frame(Vector(0.0,0.0,0.9))));
00020 chain1.addSegment(Segment("Segment 3", Joint("Joint 3", Joint::None),
00021 Frame(Vector(-0.4,0.0,0.0))));
00022 chain1.addSegment(Segment("Segment 4", Joint("Joint 4", Joint::RotX),
00023 Frame(Vector(0.0,0.0,1.2))));
00024 chain1.addSegment(Segment("Segment 5", Joint("Joint 5", Joint::None),
00025 Frame(Vector(0.4,0.0,0.0))));
00026 chain1.addSegment(Segment("Segment 6", Joint("Joint 6", Joint::RotZ),
00027 Frame(Vector(0.0,0.0,1.4))));
00028 chain1.addSegment(Segment("Segment 7", Joint("Joint 7", Joint::RotX),
00029 Frame(Vector(0.0,0.0,0.0))));
00030 chain1.addSegment(Segment("Segment 8", Joint("Joint 8", Joint::RotZ),
00031 Frame(Vector(0.0,0.0,0.4))));
00032 chain1.addSegment(Segment("Segment 9", Joint("Joint 9", Joint::None),
00033 Frame(Vector(0.0,0.0,0.0))));
00034
00035 chain2.addSegment(Segment("Segment 1", Joint("Joint 1", Joint::RotZ),
00036 Frame(Vector(0.0,0.0,0.5))));
00037 chain2.addSegment(Segment("Segment 2", Joint("Joint 2", Joint::RotX),
00038 Frame(Vector(0.0,0.0,0.4))));
00039 chain2.addSegment(Segment("Segment 3", Joint("Joint 3", Joint::RotX),
00040 Frame(Vector(0.0,0.0,0.3))));
00041 chain2.addSegment(Segment("Segment 4", Joint("Joint 4", Joint::RotX),
00042 Frame(Vector(0.0,0.0,0.2))));
00043 chain2.addSegment(Segment("Segment 5", Joint("Joint 5", Joint::RotZ),
00044 Frame(Vector(0.0,0.0,0.1))));
00045
00046
00047 chain3.addSegment(Segment("Segment 1", Joint("Joint 1", Joint::RotZ),
00048 Frame(Vector(0.0,0.0,0.0))));
00049 chain3.addSegment(Segment("Segment 2", Joint("Joint 2", Joint::RotX),
00050 Frame(Vector(0.0,0.0,0.9))));
00051 chain3.addSegment(Segment("Segment 3", Joint("Joint 3", Joint::RotZ),
00052 Frame(Vector(-0.4,0.0,0.0))));
00053 chain3.addSegment(Segment("Segment 4", Joint("Joint 4", Joint::RotX),
00054 Frame(Vector(0.0,0.0,1.2))));
00055 chain3.addSegment(Segment("Segment 5", Joint("Joint 5", Joint::None),
00056 Frame(Vector(0.4,0.0,0.0))));
00057 chain3.addSegment(Segment("Segment 6", Joint("Joint 6", Joint::RotZ),
00058 Frame(Vector(0.0,0.0,1.4))));
00059 chain3.addSegment(Segment("Segment 7", Joint("Joint 7", Joint::RotX),
00060 Frame(Vector(0.0,0.0,0.0))));
00061 chain3.addSegment(Segment("Segment 8", Joint("Joint 8", Joint::RotZ),
00062 Frame(Vector(0.0,0.0,0.4))));
00063 chain3.addSegment(Segment("Segment 9", Joint("Joint 9", Joint::RotY),
00064 Frame(Vector(0.0,0.0,0.0))));
00065
00066
00067 chain4.addSegment(Segment("Segment 1", Joint("Joint 1", Vector(10,0,0), Vector(1,0,1),Joint::RotAxis),
00068 Frame(Vector(0.0,0.0,0.5))));
00069 chain4.addSegment(Segment("Segment 2", Joint("Joint 2", Vector(0,5,0), Vector(1,0,0),Joint::RotAxis),
00070 Frame(Vector(0.0,0.0,0.4))));
00071 chain4.addSegment(Segment("Segment 3", Joint("Joint 3", Vector(0,0,5), Vector(1,0,4),Joint::RotAxis),
00072 Frame(Vector(0.0,0.0,0.3))));
00073 chain4.addSegment(Segment("Segment 4", Joint("Joint 4", Vector(0,0,0), Vector(1,0,0),Joint::RotAxis),
00074 Frame(Vector(0.0,0.0,0.2))));
00075 chain4.addSegment(Segment("Segment 5", Joint("Joint 5", Vector(0,0,0), Vector(0,0,1),Joint::RotAxis),
00076 Frame(Vector(0.0,0.0,0.1))));
00077
00078
00079
00080
00081 Joint rotJoint0 = Joint(Joint::RotZ, 1, 0, 0.01);
00082 Joint rotJoint1 = Joint(Joint::RotZ, 1, 0, 0.01);
00083
00084 Frame refFrame(Rotation::RPY(0.0, 0.0, 0.0), Vector(0.0, 0.0, 0.0));
00085 Frame frame1(Rotation::RPY(0.0, 0.0, 0.0), Vector(0.0, -0.4, 0.0));
00086 Frame frame2(Rotation::RPY(0.0, 0.0, 0.0), Vector(0.0, -0.4, 0.0));
00087
00088
00089 Segment segment1 = Segment(rotJoint0, frame1);
00090 Segment segment2 = Segment(rotJoint1, frame2);
00091
00092
00093 RotationalInertia rotInerSeg1(0.0, 0.0, 0.0, 0.0, 0.0, 0.0);
00094
00095
00096 RigidBodyInertia inerSegment1(0.3, Vector(0.0, -0.4, 0.0), rotInerSeg1);
00097 RigidBodyInertia inerSegment2(0.3, Vector(0.0, -0.4, 0.0), rotInerSeg1);
00098 segment1.setInertia(inerSegment1);
00099 segment2.setInertia(inerSegment2);
00100
00101
00102 chaindyn.addSegment(segment1);
00103 chaindyn.addSegment(segment2);
00104
00105
00106 motomansia10.addSegment(Segment(Joint(Joint::None),
00107 Frame::DH_Craig1989(0.0, 0.0, 0.36, 0.0)));
00108 motomansia10.addSegment(Segment(Joint(Joint::RotZ),
00109 Frame::DH_Craig1989(0.0, M_PI_2, 0.0, 0.0)));
00110 motomansia10.addSegment(Segment(Joint(Joint::RotZ),
00111 Frame::DH_Craig1989(0.0, -M_PI_2, 0.36, 0.0)));
00112 motomansia10.addSegment(Segment(Joint(Joint::RotZ),
00113 Frame::DH_Craig1989(0.0, M_PI_2, 0.0, 0.0)));
00114 motomansia10.addSegment(Segment(Joint(Joint::RotZ),
00115 Frame::DH_Craig1989(0.0, -M_PI_2, 0.36, 0.0)));
00116 motomansia10.addSegment(Segment(Joint(Joint::RotZ),
00117 Frame::DH_Craig1989(0.0, M_PI_2, 0.0, 0.0)));
00118 motomansia10.addSegment(Segment(Joint(Joint::RotZ),
00119 Frame::DH_Craig1989(0.0, -M_PI_2, 0.0, 0.0)));
00120 motomansia10.addSegment(Segment(Joint(Joint::RotZ),
00121 Frame(Rotation::Identity(),Vector(0.0,0.0,0.155))));
00122
00123
00124
00125
00126 static const double scale=1;
00127 static const double offset=0;
00128 static const double inertiamotorA=5.0;
00129 static const double inertiamotorB=3.0;
00130 static const double inertiamotorC=1.0;
00131 static const double damping=0;
00132 static const double stiffness=0;
00133
00134
00135 KDL::RigidBodyInertia inert1(15.0, KDL::Vector(0.0, -0.02, 0.0),
00136 KDL::RotationalInertia(0.1, 0.05, 0.1, 0.0, 0.0, 0.0));
00137 KDL::RigidBodyInertia inert2(5.0, KDL::Vector(0.0, -0.02, -0.1),
00138 KDL::RotationalInertia(0.01, 0.1, 0.1, 0.0, 0.0, 0.0));
00139 KDL::RigidBodyInertia inert3(5.0, KDL::Vector(0.0, -0.05, 0.02),
00140 KDL::RotationalInertia(0.05, 0.01, 0.05, 0.0, 0.0, 0.0));
00141 KDL::RigidBodyInertia inert4(3.0, KDL::Vector(0.0, 0.02, -0.15),
00142 KDL::RotationalInertia(0.1, 0.1, 0.01, 0.0, 0.0, 0.0));
00143 KDL::RigidBodyInertia inert5(3.0, KDL::Vector(0.0, -0.05, 0.01),
00144 KDL::RotationalInertia(0.02, 0.01, 0.02, 0.0, 0.0, 0.0));
00145 KDL::RigidBodyInertia inert6(3.0, KDL::Vector(0.0, -0.01, -0.1),
00146 KDL::RotationalInertia(0.1, 0.1, 0.01, 0.0, 0.0, 0.0));
00147 KDL::RigidBodyInertia inert7(1.0, KDL::Vector(0.0, 0.0, 0.05),
00148 KDL::RotationalInertia(0.01, 0.01, 0.1, 0.0, 0.0, 0.0));
00149
00150 motomansia10dyn.addSegment(Segment(Joint(Joint::RotZ, scale, offset, inertiamotorA, damping, stiffness),
00151 Frame::DH(0.0, M_PI_2, 0.36, 0.0),
00152 inert1));
00153 motomansia10dyn.addSegment(Segment(Joint(Joint::RotZ, scale, offset, inertiamotorA, damping, stiffness),
00154 Frame::DH(0.0, -M_PI_2, 0.0, 0.0),
00155 inert2));
00156 motomansia10dyn.addSegment(Segment(Joint(Joint::RotZ, scale, offset, inertiamotorB, damping, stiffness),
00157 Frame::DH(0.0, M_PI_2, 0.36, 0.0),
00158 inert3));
00159 motomansia10dyn.addSegment(Segment(Joint(Joint::RotZ, scale, offset, inertiamotorB, damping, stiffness),
00160 Frame::DH(0.0, -M_PI_2, 0.0, 0.0),
00161 inert4));
00162 motomansia10dyn.addSegment(Segment(Joint(Joint::RotZ, scale, offset, inertiamotorC, damping, stiffness),
00163 Frame::DH(0.0, M_PI_2, 0.36, 0.0),
00164 inert5));
00165 motomansia10dyn.addSegment(Segment(Joint(Joint::RotZ, scale, offset, inertiamotorC, damping, stiffness),
00166 Frame::DH(0.0, -M_PI_2, 0.0, 0.0),
00167 inert6));
00168 motomansia10dyn.addSegment(Segment(Joint(Joint::RotZ, scale, offset, inertiamotorC, damping, stiffness),
00169 Frame::DH(0.0, 0.0, 0.0, 0.0),
00170 inert7));
00171 motomansia10dyn.addSegment(Segment(Joint(Joint::None),
00172 Frame(Rotation::Identity(),Vector(0.0,0.0,0.155))));
00173 }
00174
00175 void SolverTest::tearDown()
00176 {
00177
00178
00179
00180
00181
00182 }
00183
00184 void SolverTest::UpdateChainTest()
00185 {
00186 ChainFkSolverPos_recursive fksolverpos(chain2);
00187 ChainFkSolverVel_recursive fksolvervel(chain2);
00188 ChainJntToJacSolver jacsolver1(chain2);
00189 ChainJntToJacDotSolver jacdotsolver1(chain2);
00190 ChainIkSolverVel_pinv iksolver2(chain2);
00191 ChainIkSolverVel_pinv_givens iksolver_pinv_givens2(chain2);
00192 ChainIkSolverVel_pinv_nso iksolver_pinv_nso(chain2);
00193 ChainIkSolverVel_wdls iksolver_wdls(chain2,1e-6,30);
00194 ChainIkSolverPos_NR iksolverpos(chain2,fksolverpos,iksolver2);
00195 ChainIkSolverPos_NR_JL iksolverpos2(chain2,fksolverpos,iksolver2);
00196 ChainIkSolverPos_LMA iksolverpos3(chain2);
00197 ChainDynParam dynparam(chain2, Vector::Zero());
00198 ChainIdSolver_RNE idsolver1(chain2, Vector::Zero());
00199 unsigned int nr_of_constraints = 4;
00200 ChainIdSolver_Vereshchagin idsolver2(chain2,Twist::Zero(),4);
00201
00202 JntArray q_in(chain2.getNrOfJoints());
00203 JntArray q_in2(chain2.getNrOfJoints());
00204 JntArrayVel q_in3(chain2.getNrOfJoints());
00205 for(unsigned int i=0; i<chain2.getNrOfJoints(); i++)
00206 {
00207 random(q_in(i));
00208 random(q_in2(i));
00209 }
00210 JntArray q_out(chain2.getNrOfJoints());
00211 JntArray q_out2(chain2.getNrOfJoints());
00212 Jacobian jac(chain2.getNrOfJoints());
00213 Frame T;
00214 Twist t;
00215 FrameVel T2;
00216 Wrenches wrenches(chain2.getNrOfSegments());
00217 JntSpaceInertiaMatrix m(chain2.getNrOfJoints());
00218
00219 Jacobian alpha(nr_of_constraints - 1);
00220 JntArray beta(nr_of_constraints - 1);
00221 CPPUNIT_ASSERT_EQUAL((int)SolverI::E_OUT_OF_RANGE,fksolverpos.JntToCart(q_in, T, chain2.getNrOfSegments()+1));
00222 CPPUNIT_ASSERT_EQUAL((int)SolverI::E_OUT_OF_RANGE,fksolvervel.JntToCart(q_in3, T2, chain2.getNrOfSegments()+1));
00223 CPPUNIT_ASSERT_EQUAL((int)SolverI::E_OUT_OF_RANGE, jacsolver1.JntToJac(q_in, jac, chain2.getNrOfSegments()+1));
00224 CPPUNIT_ASSERT_EQUAL((int)SolverI::E_OUT_OF_RANGE, jacdotsolver1.JntToJacDot(q_in3, t, chain2.getNrOfSegments()+1));
00225 CPPUNIT_ASSERT_EQUAL((int)SolverI::E_OUT_OF_RANGE, jacdotsolver1.JntToJacDot(q_in3, jac, chain2.getNrOfSegments()+1));
00226 chain2.addSegment(Segment("Segment 6", Joint("Joint 6", Joint::RotX),
00227 Frame(Vector(0.0,0.0,0.1))));
00228 CPPUNIT_ASSERT_EQUAL((int)SolverI::E_NOT_UP_TO_DATE, jacsolver1.JntToJac(q_in, jac, chain2.getNrOfSegments()));
00229 CPPUNIT_ASSERT_EQUAL((int)SolverI::E_NOT_UP_TO_DATE, jacdotsolver1.JntToJacDot(q_in3, jac, chain2.getNrOfSegments()));
00230 CPPUNIT_ASSERT_EQUAL((int)SolverI::E_NOT_UP_TO_DATE, jacdotsolver1.JntToJacDot(q_in3, t, chain2.getNrOfSegments()));
00231 CPPUNIT_ASSERT_EQUAL((int)SolverI::E_NOT_UP_TO_DATE, iksolver2.CartToJnt(q_in,t,q_out));
00232 CPPUNIT_ASSERT_EQUAL((int)SolverI::E_NOT_UP_TO_DATE, iksolver_pinv_givens2.CartToJnt(q_in,t,q_out));
00233 CPPUNIT_ASSERT_EQUAL((int)SolverI::E_NOT_UP_TO_DATE, iksolver_pinv_nso.CartToJnt(q_in,t,q_out));
00234 CPPUNIT_ASSERT_EQUAL((int)SolverI::E_NOT_UP_TO_DATE, iksolver_wdls.CartToJnt(q_in,t,q_out));
00235 CPPUNIT_ASSERT_EQUAL((int)SolverI::E_NOT_UP_TO_DATE, iksolverpos.CartToJnt(q_in,T,q_out));
00236 CPPUNIT_ASSERT_EQUAL((int)SolverI::E_NOT_UP_TO_DATE, iksolverpos2.CartToJnt(q_in,T,q_out));
00237 CPPUNIT_ASSERT_EQUAL((int)SolverI::E_NOT_UP_TO_DATE, iksolverpos3.CartToJnt(q_in,T,q_out));
00238 CPPUNIT_ASSERT_EQUAL((int)SolverI::E_NOT_UP_TO_DATE, idsolver1.CartToJnt(q_in,q_in2,q_out,wrenches,q_out2));
00239 CPPUNIT_ASSERT_EQUAL((int)SolverI::E_NOT_UP_TO_DATE, idsolver2.CartToJnt(q_in,q_in2,q_out,alpha,beta,wrenches,q_out2));
00240 CPPUNIT_ASSERT_EQUAL((int)SolverI::E_NOT_UP_TO_DATE, dynparam.JntToCoriolis(q_in, q_in2, q_out));
00241 CPPUNIT_ASSERT_EQUAL((int)SolverI::E_NOT_UP_TO_DATE, dynparam.JntToGravity(q_in, q_out));
00242 CPPUNIT_ASSERT_EQUAL((int)SolverI::E_NOT_UP_TO_DATE, dynparam.JntToMass(q_in, m));
00243
00244 jacsolver1.updateInternalDataStructures();
00245 jacdotsolver1.updateInternalDataStructures();
00246 iksolver2.updateInternalDataStructures();
00247 iksolver_pinv_givens2.updateInternalDataStructures();
00248 iksolver_pinv_nso.updateInternalDataStructures();
00249 iksolver_wdls.updateInternalDataStructures();
00250 iksolverpos.updateInternalDataStructures();
00251 iksolverpos2.updateInternalDataStructures();
00252 iksolverpos3.updateInternalDataStructures();
00253 idsolver1.updateInternalDataStructures();
00254 idsolver2.updateInternalDataStructures();
00255 dynparam.updateInternalDataStructures();
00256
00257 CPPUNIT_ASSERT_EQUAL((int)SolverI::E_SIZE_MISMATCH,fksolverpos.JntToCart(q_in, T, chain2.getNrOfSegments()));
00258 CPPUNIT_ASSERT_EQUAL((int)SolverI::E_SIZE_MISMATCH,fksolvervel.JntToCart(q_in3, T2, chain2.getNrOfSegments()));
00259 CPPUNIT_ASSERT_EQUAL((int)SolverI::E_SIZE_MISMATCH, jacsolver1.JntToJac(q_in, jac, chain2.getNrOfSegments()));
00260 CPPUNIT_ASSERT_EQUAL((int)SolverI::E_SIZE_MISMATCH, jacdotsolver1.JntToJacDot(q_in3, t, chain2.getNrOfSegments()));
00261 CPPUNIT_ASSERT_EQUAL((int)SolverI::E_SIZE_MISMATCH, jacdotsolver1.JntToJacDot(q_in3, jac, chain2.getNrOfSegments()));
00262 CPPUNIT_ASSERT_EQUAL((int)SolverI::E_SIZE_MISMATCH, iksolver2.CartToJnt(q_in,t,q_out));
00263 CPPUNIT_ASSERT_EQUAL((int)SolverI::E_SIZE_MISMATCH, iksolver_pinv_givens2.CartToJnt(q_in,t,q_out));
00264 CPPUNIT_ASSERT_EQUAL((int)SolverI::E_SIZE_MISMATCH, iksolver_pinv_nso.CartToJnt(q_in,t,q_out));
00265 CPPUNIT_ASSERT_EQUAL((int)SolverI::E_SIZE_MISMATCH, iksolver_wdls.CartToJnt(q_in,t,q_out));
00266 CPPUNIT_ASSERT_EQUAL((int)SolverI::E_SIZE_MISMATCH, iksolverpos.CartToJnt(q_in,T,q_out));
00267 CPPUNIT_ASSERT_EQUAL((int)SolverI::E_SIZE_MISMATCH, iksolverpos2.CartToJnt(q_in,T,q_out));
00268 CPPUNIT_ASSERT_EQUAL((int)SolverI::E_SIZE_MISMATCH, iksolverpos3.CartToJnt(q_in,T,q_out));
00269 CPPUNIT_ASSERT_EQUAL((int)SolverI::E_SIZE_MISMATCH, idsolver1.CartToJnt(q_in,q_in2,q_out,wrenches,q_out2));
00270 CPPUNIT_ASSERT_EQUAL((int)SolverI::E_SIZE_MISMATCH, idsolver2.CartToJnt(q_in,q_in2,q_out,alpha,beta,wrenches,q_out2));
00271 CPPUNIT_ASSERT_EQUAL((int)SolverI::E_SIZE_MISMATCH, dynparam.JntToCoriolis(q_in, q_in2, q_out));
00272 CPPUNIT_ASSERT_EQUAL((int)SolverI::E_SIZE_MISMATCH, dynparam.JntToGravity(q_in, q_out));
00273 CPPUNIT_ASSERT_EQUAL((int)SolverI::E_SIZE_MISMATCH, dynparam.JntToMass(q_in, m));
00274
00275 q_in.resize(chain2.getNrOfJoints());
00276 CPPUNIT_ASSERT_EQUAL((int)SolverI::E_SIZE_MISMATCH, jacsolver1.JntToJac(q_in, jac, chain2.getNrOfSegments()));
00277 CPPUNIT_ASSERT_EQUAL((int)SolverI::E_SIZE_MISMATCH, iksolver2.CartToJnt(q_in,t,q_out));
00278 CPPUNIT_ASSERT_EQUAL((int)SolverI::E_SIZE_MISMATCH, iksolver_pinv_givens2.CartToJnt(q_in,t,q_out));
00279 CPPUNIT_ASSERT_EQUAL((int)SolverI::E_SIZE_MISMATCH, iksolver_pinv_nso.CartToJnt(q_in,t,q_out));
00280 CPPUNIT_ASSERT_EQUAL((int)SolverI::E_SIZE_MISMATCH, iksolver_wdls.CartToJnt(q_in,t,q_out));
00281 CPPUNIT_ASSERT_EQUAL((int)SolverI::E_SIZE_MISMATCH, iksolverpos.CartToJnt(q_in,T,q_out));
00282 CPPUNIT_ASSERT_EQUAL((int)SolverI::E_SIZE_MISMATCH, iksolverpos2.CartToJnt(q_in,T,q_out));
00283 CPPUNIT_ASSERT_EQUAL((int)SolverI::E_SIZE_MISMATCH, iksolverpos3.CartToJnt(q_in,T,q_out));
00284 CPPUNIT_ASSERT_EQUAL((int)SolverI::E_SIZE_MISMATCH, iksolverpos2.CartToJnt(q_in,T,q_out));
00285 CPPUNIT_ASSERT_EQUAL((int)SolverI::E_SIZE_MISMATCH, iksolverpos3.CartToJnt(q_in,T,q_out));
00286 CPPUNIT_ASSERT_EQUAL((int)SolverI::E_SIZE_MISMATCH, idsolver1.CartToJnt(q_in,q_in2,q_out,wrenches,q_out2));
00287 CPPUNIT_ASSERT_EQUAL((int)SolverI::E_SIZE_MISMATCH, idsolver2.CartToJnt(q_in,q_in2,q_out,alpha,beta,wrenches,q_out2));
00288 CPPUNIT_ASSERT_EQUAL((int)SolverI::E_SIZE_MISMATCH, dynparam.JntToCoriolis(q_in, q_in2, q_out));
00289 CPPUNIT_ASSERT_EQUAL((int)SolverI::E_SIZE_MISMATCH, dynparam.JntToGravity(q_in, q_out));
00290 CPPUNIT_ASSERT_EQUAL((int)SolverI::E_SIZE_MISMATCH, dynparam.JntToMass(q_in, m));
00291 q_in2.resize(chain2.getNrOfJoints());
00292 CPPUNIT_ASSERT_EQUAL((int)SolverI::E_SIZE_MISMATCH, dynparam.JntToCoriolis(q_in, q_in2, q_out));
00293 CPPUNIT_ASSERT_EQUAL((int)SolverI::E_SIZE_MISMATCH, idsolver1.CartToJnt(q_in,q_in2,q_out,wrenches,q_out2));
00294 CPPUNIT_ASSERT_EQUAL((int)SolverI::E_SIZE_MISMATCH, idsolver2.CartToJnt(q_in,q_in2,q_out,alpha,beta,wrenches,q_out2));
00295 wrenches.resize(chain2.getNrOfSegments());
00296 CPPUNIT_ASSERT_EQUAL((int)SolverI::E_SIZE_MISMATCH, idsolver1.CartToJnt(q_in,q_in2,q_out,wrenches,q_out2));
00297 CPPUNIT_ASSERT_EQUAL((int)SolverI::E_SIZE_MISMATCH, idsolver2.CartToJnt(q_in,q_in2,q_out,alpha,beta,wrenches,q_out2));
00298 q_out2.resize(chain2.getNrOfSegments());
00299 CPPUNIT_ASSERT_EQUAL((int)SolverI::E_SIZE_MISMATCH, idsolver1.CartToJnt(q_in,q_in2,q_out,wrenches,q_out2));
00300 CPPUNIT_ASSERT_EQUAL((int)SolverI::E_SIZE_MISMATCH, idsolver2.CartToJnt(q_in,q_in2,q_out,alpha,beta,wrenches,q_out2));
00301 alpha.resize(nr_of_constraints);
00302 CPPUNIT_ASSERT_EQUAL((int)SolverI::E_SIZE_MISMATCH, idsolver2.CartToJnt(q_in,q_in2,q_out,alpha,beta,wrenches,q_out2));
00303 beta.resize(nr_of_constraints);
00304 CPPUNIT_ASSERT_EQUAL((int)SolverI::E_SIZE_MISMATCH, idsolver2.CartToJnt(q_in,q_in2,q_out,alpha,beta,wrenches,q_out2));
00305 jac.resize(chain2.getNrOfJoints());
00306 CPPUNIT_ASSERT_EQUAL((int)SolverI::E_SIZE_MISMATCH, jacdotsolver1.JntToJacDot(q_in3, jac, chain2.getNrOfSegments()));
00307 q_out.resize(chain2.getNrOfJoints());
00308 q_in3.resize(chain2.getNrOfJoints());
00309 m.resize(chain2.getNrOfJoints());
00310 CPPUNIT_ASSERT_EQUAL((int)SolverI::E_NOERROR,fksolverpos.JntToCart(q_in, T, chain2.getNrOfSegments()));
00311 CPPUNIT_ASSERT_EQUAL((int)SolverI::E_NOERROR,fksolvervel.JntToCart(q_in3, T2, chain2.getNrOfSegments()));
00312 CPPUNIT_ASSERT_EQUAL((int)SolverI::E_NOERROR, jacsolver1.JntToJac(q_in, jac, chain2.getNrOfSegments()));
00313 CPPUNIT_ASSERT_EQUAL((int)SolverI::E_NOERROR, jacdotsolver1.JntToJacDot(q_in3, jac, chain2.getNrOfSegments()));
00314 CPPUNIT_ASSERT_EQUAL((int)SolverI::E_NOERROR, jacdotsolver1.JntToJacDot(q_in3, t, chain2.getNrOfSegments()));
00315 CPPUNIT_ASSERT((int)SolverI::E_NOERROR <= iksolver2.CartToJnt(q_in,t,q_out));
00316 CPPUNIT_ASSERT((int)SolverI::E_NOERROR <= iksolver_pinv_givens2.CartToJnt(q_in,t,q_out));
00317 CPPUNIT_ASSERT((int)SolverI::E_NOERROR <= iksolver_pinv_nso.CartToJnt(q_in,t,q_out));
00318 CPPUNIT_ASSERT((int)SolverI::E_NOERROR <= iksolver_wdls.CartToJnt(q_in,t,q_out));
00319 CPPUNIT_ASSERT((int)SolverI::E_NOERROR <= iksolverpos.CartToJnt(q_in,T,q_out));
00320 CPPUNIT_ASSERT((int)SolverI::E_NOERROR <= iksolverpos2.CartToJnt(q_in,T,q_out));
00321 CPPUNIT_ASSERT((int)SolverI::E_NOERROR <= iksolverpos3.CartToJnt(q_in,T,q_out));
00322 CPPUNIT_ASSERT((int)SolverI::E_NOERROR <= iksolverpos2.CartToJnt(q_in,T,q_out));
00323 CPPUNIT_ASSERT((int)SolverI::E_NOERROR <= iksolverpos3.CartToJnt(q_in,T,q_out));
00324 CPPUNIT_ASSERT((int)SolverI::E_NOERROR <= idsolver1.CartToJnt(q_in,q_in2,q_out,wrenches,q_out2));
00325 CPPUNIT_ASSERT((int)SolverI::E_NOERROR <= idsolver2.CartToJnt(q_in,q_in2,q_out,alpha,beta,wrenches,q_out2));
00326 CPPUNIT_ASSERT((int)SolverI::E_NOERROR <= dynparam.JntToCoriolis(q_in, q_in2, q_out));
00327 CPPUNIT_ASSERT((int)SolverI::E_NOERROR <= dynparam.JntToGravity(q_in, q_out));
00328 CPPUNIT_ASSERT((int)SolverI::E_NOERROR <= dynparam.JntToMass(q_in, m));
00329 }
00330 void SolverTest::FkPosAndJacTest()
00331 {
00332 ChainFkSolverPos_recursive fksolver1(chain1);
00333 ChainJntToJacSolver jacsolver1(chain1);
00334 FkPosAndJacLocal(chain1,fksolver1,jacsolver1);
00335 ChainFkSolverPos_recursive fksolver2(chain2);
00336 ChainJntToJacSolver jacsolver2(chain2);
00337 FkPosAndJacLocal(chain2,fksolver2,jacsolver2);
00338 ChainFkSolverPos_recursive fksolver3(chain3);
00339 ChainJntToJacSolver jacsolver3(chain3);
00340 FkPosAndJacLocal(chain3,fksolver3,jacsolver3);
00341 ChainFkSolverPos_recursive fksolver4(chain4);
00342 ChainJntToJacSolver jacsolver4(chain4);
00343 FkPosAndJacLocal(chain4,fksolver4,jacsolver4);
00344 }
00345
00346 void SolverTest::FkVelAndJacTest()
00347 {
00348 ChainFkSolverVel_recursive fksolver1(chain1);
00349 ChainJntToJacSolver jacsolver1(chain1);
00350 FkVelAndJacLocal(chain1,fksolver1,jacsolver1);
00351 ChainFkSolverVel_recursive fksolver2(chain2);
00352 ChainJntToJacSolver jacsolver2(chain2);
00353 FkVelAndJacLocal(chain2,fksolver2,jacsolver2);
00354 ChainFkSolverVel_recursive fksolver3(chain3);
00355 ChainJntToJacSolver jacsolver3(chain3);
00356 FkVelAndJacLocal(chain3,fksolver3,jacsolver3);
00357 ChainFkSolverVel_recursive fksolver4(chain4);
00358 ChainJntToJacSolver jacsolver4(chain4);
00359 FkVelAndJacLocal(chain4,fksolver4,jacsolver4);
00360 }
00361
00362 void SolverTest::FkVelAndIkVelTest()
00363 {
00364
00365 std::cout<<"square problem"<<std::endl;
00366 ChainFkSolverVel_recursive fksolver1(chain1);
00367 ChainIkSolverVel_pinv iksolver1(chain1);
00368 ChainIkSolverVel_pinv_givens iksolver_pinv_givens1(chain1);
00369 std::cout<<"KDL-SVD-HouseHolder"<<std::endl;
00370 FkVelAndIkVelLocal(chain1,fksolver1,iksolver1);
00371 std::cout<<"KDL-SVD-Givens"<<std::endl;
00372 FkVelAndIkVelLocal(chain1,fksolver1,iksolver_pinv_givens1);
00373
00374
00375 std::cout<<"underdetermined problem"<<std::endl;
00376 ChainFkSolverVel_recursive fksolver2(chain2);
00377 ChainIkSolverVel_pinv iksolver2(chain2);
00378 ChainIkSolverVel_pinv_givens iksolver_pinv_givens2(chain2);
00379 std::cout<<"KDL-SVD-HouseHolder"<<std::endl;
00380 FkVelAndIkVelLocal(chain2,fksolver2,iksolver2);
00381 std::cout<<"KDL-SVD-Givens"<<std::endl;
00382 FkVelAndIkVelLocal(chain2,fksolver2,iksolver_pinv_givens2);
00383
00384
00385 std::cout<<"overdetermined problem"<<std::endl;
00386 ChainFkSolverVel_recursive fksolver3(chain3);
00387 ChainIkSolverVel_pinv iksolver3(chain3);
00388 ChainIkSolverVel_pinv_givens iksolver_pinv_givens3(chain3);
00389 std::cout<<"KDL-SVD-HouseHolder"<<std::endl;
00390 FkVelAndIkVelLocal(chain3,fksolver3,iksolver3);
00391 std::cout<<"KDL-SVD-Givens"<<std::endl;
00392 FkVelAndIkVelLocal(chain3,fksolver3,iksolver_pinv_givens3);
00393
00394
00395 std::cout<<"overdetermined problem"<<std::endl;
00396 ChainFkSolverVel_recursive fksolver4(chain4);
00397 ChainIkSolverVel_pinv iksolver4(chain4);
00398 ChainIkSolverVel_pinv_givens iksolver_pinv_givens4(chain4);
00399 std::cout<<"KDL-SVD-HouseHolder"<<std::endl;
00400 FkVelAndIkVelLocal(chain4,fksolver4,iksolver4);
00401 std::cout<<"KDL-SVD-Givens"<<std::endl;
00402 FkVelAndIkVelLocal(chain4,fksolver4,iksolver_pinv_givens4);
00403 }
00404
00405 void SolverTest::FkPosAndIkPosTest()
00406 {
00407 std::cout<<"square problem"<<std::endl;
00408 ChainFkSolverPos_recursive fksolver1(chain1);
00409 ChainIkSolverVel_pinv iksolver1v(chain1);
00410 ChainIkSolverVel_pinv_givens iksolverv_pinv_givens1(chain1);
00411 ChainIkSolverPos_NR iksolver1(chain1,fksolver1,iksolver1v);
00412 ChainIkSolverPos_NR iksolver1_givens(chain1,fksolver1,iksolverv_pinv_givens1,1000);
00413
00414 std::cout<<"KDL-SVD-HouseHolder"<<std::endl;
00415 FkPosAndIkPosLocal(chain1,fksolver1,iksolver1);
00416 std::cout<<"KDL-SVD-Givens"<<std::endl;
00417 FkPosAndIkPosLocal(chain1,fksolver1,iksolver1_givens);
00418
00419 std::cout<<"underdetermined problem"<<std::endl;
00420 ChainFkSolverPos_recursive fksolver2(chain2);
00421 ChainIkSolverVel_pinv iksolverv2(chain2);
00422 ChainIkSolverVel_pinv_givens iksolverv_pinv_givens2(chain2);
00423 ChainIkSolverPos_NR iksolver2(chain2,fksolver2,iksolverv2);
00424 ChainIkSolverPos_NR iksolver2_givens(chain2,fksolver2,iksolverv_pinv_givens2,1000);
00425
00426 std::cout<<"KDL-SVD-HouseHolder"<<std::endl;
00427 FkPosAndIkPosLocal(chain2,fksolver2,iksolver2);
00428 std::cout<<"KDL-SVD-Givens"<<std::endl;
00429 FkPosAndIkPosLocal(chain2,fksolver2,iksolver2_givens);
00430
00431 std::cout<<"overdetermined problem"<<std::endl;
00432 ChainFkSolverPos_recursive fksolver3(chain3);
00433 ChainIkSolverVel_pinv iksolverv3(chain3);
00434 ChainIkSolverVel_pinv_givens iksolverv_pinv_givens3(chain3);
00435 ChainIkSolverPos_NR iksolver3(chain3,fksolver3,iksolverv3);
00436 ChainIkSolverPos_NR iksolver3_givens(chain3,fksolver3,iksolverv_pinv_givens3,1000);
00437
00438 std::cout<<"KDL-SVD-HouseHolder"<<std::endl;
00439 FkPosAndIkPosLocal(chain3,fksolver3,iksolver3);
00440 std::cout<<"KDL-SVD-Givens"<<std::endl;
00441 FkPosAndIkPosLocal(chain3,fksolver3,iksolver3_givens);
00442
00443 std::cout<<"underdetermined problem with WGs segment constructor"<<std::endl;
00444 ChainFkSolverPos_recursive fksolver4(chain4);
00445 ChainIkSolverVel_pinv iksolverv4(chain4);
00446 ChainIkSolverVel_pinv_givens iksolverv_pinv_givens4(chain4);
00447 ChainIkSolverPos_NR iksolver4(chain4,fksolver4,iksolverv4,1000);
00448 ChainIkSolverPos_NR iksolver4_givens(chain4,fksolver4,iksolverv_pinv_givens4,1000);
00449
00450 std::cout<<"KDL-SVD-HouseHolder"<<std::endl;
00451 FkPosAndIkPosLocal(chain4,fksolver4,iksolver4);
00452 std::cout<<"KDL-SVD-Givens"<<std::endl;
00453 FkPosAndIkPosLocal(chain4,fksolver4,iksolver4_givens);
00454 }
00455
00456 void SolverTest::IkSingularValueTest()
00457 {
00458 unsigned int maxiter = 30;
00459 double eps = 1e-6 ;
00460 int maxiter_vel = 30;
00461 double eps_vel = 0.1 ;
00462 Frame F, dF, F_des,F_solved;
00463 KDL::Twist F_error ;
00464
00465 std::cout<<"KDL-IK Solver Tests for Near Zero SVs"<<std::endl;
00466
00467 ChainFkSolverPos_recursive fksolver(motomansia10);
00468 ChainIkSolverVel_pinv ikvelsolver1(motomansia10,eps_vel,maxiter_vel);
00469 ChainIkSolverPos_NR iksolver1(motomansia10,fksolver,ikvelsolver1,maxiter,eps);
00470 unsigned int nj = motomansia10.getNrOfJoints();
00471 JntArray q(nj), q_solved(nj) ;
00472
00473
00474 std::cout<<"norminal case: convergence"<<std::endl;
00475
00476 q(0) = 0. ;
00477 q(1) = 0.5 ;
00478 q(2) = 0.4 ;
00479 q(3) = -M_PI_2 ;
00480 q(4) = 0. ;
00481 q(5) = 0. ;
00482 q(6) = 0. ;
00483
00484 dF.M = KDL::Rotation::RPY(0.1, 0.1, 0.1) ;
00485 dF.p = KDL::Vector(0.01,0.01,0.01) ;
00486
00487 CPPUNIT_ASSERT_EQUAL(0, fksolver.JntToCart(q,F));
00488 F_des = F * dF ;
00489
00490 CPPUNIT_ASSERT_EQUAL((int)SolverI::E_NOERROR,
00491 iksolver1.CartToJnt(q, F_des, q_solved));
00492 CPPUNIT_ASSERT_EQUAL((int)SolverI::E_NOERROR,
00493 ikvelsolver1.getError());
00494 CPPUNIT_ASSERT_EQUAL((unsigned int)1,
00495 ikvelsolver1.getNrZeroSigmas()) ;
00496
00497 CPPUNIT_ASSERT_EQUAL(0, fksolver.JntToCart(q_solved,F_solved));
00498 F_error = KDL::diff(F_solved,F_des);
00499 CPPUNIT_ASSERT_EQUAL(F_des,F_solved);
00500
00501 std::cout<<"nonconvergence: pseudoinverse singular"<<std::endl;
00502
00503 q(0) = 0. ;
00504 q(1) = 0.2 ;
00505 q(2) = 0.4 ;
00506 q(3) = -M_PI_2 ;
00507 q(4) = 0. ;
00508 q(5) = 0. ;
00509 q(6) = 0. ;
00510
00511 dF.M = KDL::Rotation::RPY(0.1, 0.1, 0.1) ;
00512 dF.p = KDL::Vector(0.01,0.01,0.01) ;
00513
00514 CPPUNIT_ASSERT_EQUAL(0, fksolver.JntToCart(q,F));
00515 F_des = F * dF ;
00516
00517 CPPUNIT_ASSERT_EQUAL((int)SolverI::E_MAX_ITERATIONS_EXCEEDED,
00518 iksolver1.CartToJnt(q,F_des,q_solved));
00519 CPPUNIT_ASSERT_EQUAL((int)ChainIkSolverVel_pinv::E_CONVERGE_PINV_SINGULAR,
00520 ikvelsolver1.getError());
00521 CPPUNIT_ASSERT_EQUAL((unsigned int)2,
00522 ikvelsolver1.getNrZeroSigmas()) ;
00523
00524 std::cout<<"nonconvergence: large displacement, low iterations"<<std::endl;
00525
00526 q(0) = 0. ;
00527 q(1) = 0.5 ;
00528 q(2) = 0.4 ;
00529 q(3) = -M_PI_2 ;
00530 q(4) = 0. ;
00531 q(5) = 0. ;
00532 q(6) = 0. ;
00533
00534
00535 dF.M = KDL::Rotation::RPY(0.2, 0.2, 0.2) ;
00536 dF.p = KDL::Vector(-0.2,-0.2, -0.2) ;
00537
00538
00539 maxiter = 5 ;
00540 ChainIkSolverPos_NR iksolver2(motomansia10,fksolver,ikvelsolver1,maxiter,eps);
00541
00542 CPPUNIT_ASSERT_EQUAL(0, fksolver.JntToCart(q,F));
00543 F_des = F * dF ;
00544
00545 CPPUNIT_ASSERT_EQUAL((int)SolverI::E_MAX_ITERATIONS_EXCEEDED,
00546 iksolver2.CartToJnt(q,F_des,q_solved));
00547 CPPUNIT_ASSERT_EQUAL((int)SolverI::E_NOERROR,
00548 ikvelsolver1.getError());
00549 CPPUNIT_ASSERT_EQUAL((unsigned int)1,
00550 ikvelsolver1.getNrZeroSigmas()) ;
00551
00552 std::cout<<"nonconvergence: fully singular"<<std::endl;
00553
00554 q(0) = 0. ;
00555 q(1) = 0. ;
00556 q(2) = 0. ;
00557 q(3) = 0. ;
00558 q(4) = 0. ;
00559 q(5) = 0. ;
00560 q(6) = 0. ;
00561
00562 dF.M = KDL::Rotation::RPY(0.1, 0.1, 0.1) ;
00563 dF.p = KDL::Vector(0.01,0.01,0.01) ;
00564
00565 CPPUNIT_ASSERT_EQUAL((int)SolverI::E_NOERROR, fksolver.JntToCart(q,F));
00566 F_des = F * dF ;
00567
00568 CPPUNIT_ASSERT_EQUAL((int)SolverI::E_MAX_ITERATIONS_EXCEEDED,
00569 iksolver1.CartToJnt(q,F_des,q_solved));
00570 CPPUNIT_ASSERT_EQUAL((int)ChainIkSolverVel_pinv::E_CONVERGE_PINV_SINGULAR,
00571 ikvelsolver1.getError());
00572 CPPUNIT_ASSERT_EQUAL((unsigned int)3,
00573 ikvelsolver1.getNrZeroSigmas());
00574 }
00575
00576
00577 void SolverTest::IkVelSolverWDLSTest()
00578 {
00579 int maxiter = 30;
00580 double eps = 0.1 ;
00581 double lambda = 0.1 ;
00582
00583 std::cout<<"KDL-IK WDLS Vel Solver Tests for Near Zero SVs"<<std::endl;
00584
00585 KDL::ChainIkSolverVel_wdls ikvelsolver(motomansia10,eps,maxiter) ;
00586 ikvelsolver.setLambda(lambda) ;
00587 unsigned int nj = motomansia10.getNrOfJoints();
00588 JntArray q(nj), dq(nj) ;
00589
00590 KDL::Vector v05(0.05,0.05,0.05) ;
00591 KDL::Twist dx(v05,v05) ;
00592
00593 std::cout<<"smallest singular value is above threshold (no WDLS)"<<std::endl;
00594
00595 q(0) = 0. ;
00596 q(1) = 0.5 ;
00597 q(2) = 0.4 ;
00598 q(3) = -M_PI_2 ;
00599 q(4) = 0. ;
00600 q(5) = 0. ;
00601 q(6) = 0. ;
00602
00603 CPPUNIT_ASSERT_EQUAL((int)SolverI::E_NOERROR,
00604 ikvelsolver.CartToJnt(q, dx, dq)) ;
00605 CPPUNIT_ASSERT(1==ikvelsolver.getNrZeroSigmas()) ;
00606
00607
00608 std::cout<<"smallest singular value is below threshold (lambda is scaled)"<<std::endl;
00609
00610 q(1) = 0.2 ;
00611
00612 CPPUNIT_ASSERT_EQUAL((int)ChainIkSolverVel_wdls::E_CONVERGE_PINV_SINGULAR,
00613 ikvelsolver.CartToJnt(q, dx, dq)) ;
00614 CPPUNIT_ASSERT_EQUAL((unsigned int)2,ikvelsolver.getNrZeroSigmas()) ;
00615 CPPUNIT_ASSERT_EQUAL(ikvelsolver.getLambdaScaled(),
00616 sqrt(1.0-(ikvelsolver.getSigmaMin()/eps)*(ikvelsolver.getSigmaMin()/eps))*lambda) ;
00617
00618 std::cout<<"smallest singular value is zero (lambda_scaled=lambda)"<<std::endl;
00619
00620 q(1) = 0.0 ;
00621
00622 CPPUNIT_ASSERT_EQUAL((int)ChainIkSolverVel_wdls::E_CONVERGE_PINV_SINGULAR,
00623 ikvelsolver.CartToJnt(q, dx, dq)) ;
00624 CPPUNIT_ASSERT_EQUAL((unsigned int)2,ikvelsolver.getNrZeroSigmas()) ;
00625 CPPUNIT_ASSERT_EQUAL(ikvelsolver.getLambdaScaled(),lambda) ;
00626
00627
00628 q(2) = 0.0 ;
00629 q(3) = 0.0 ;
00630
00631 CPPUNIT_ASSERT_EQUAL((int)ChainIkSolverVel_wdls::E_CONVERGE_PINV_SINGULAR,
00632 ikvelsolver.CartToJnt(q, dx, dq)) ;
00633 CPPUNIT_ASSERT_EQUAL(4,(int)ikvelsolver.getNrZeroSigmas()) ;
00634 CPPUNIT_ASSERT_EQUAL(ikvelsolver.getLambdaScaled(),lambda) ;
00635 }
00636
00637
00638 void SolverTest::FkPosAndJacLocal(Chain& chain,ChainFkSolverPos& fksolverpos,ChainJntToJacSolver& jacsolver)
00639 {
00640 double deltaq = 1E-4;
00641
00642 Frame F1,F2;
00643
00644 JntArray q(chain.getNrOfJoints());
00645 Jacobian jac(chain.getNrOfJoints());
00646
00647 for(unsigned int i=0; i<chain.getNrOfJoints(); i++)
00648 {
00649 random(q(i));
00650 }
00651
00652 jacsolver.JntToJac(q,jac);
00653
00654 for (unsigned int i=0; i< q.rows() ; i++)
00655 {
00656
00657 double oldqi = q(i);
00658 q(i) = oldqi+deltaq;
00659 CPPUNIT_ASSERT_EQUAL((int)SolverI::E_NOERROR, fksolverpos.JntToCart(q,F2));
00660 q(i) = oldqi-deltaq;
00661 CPPUNIT_ASSERT_EQUAL((int)SolverI::E_NOERROR, fksolverpos.JntToCart(q,F1));
00662 q(i) = oldqi;
00663
00664 Twist Jcol1 = diff(F1,F2,2*deltaq);
00665 Twist Jcol2(Vector(jac(0,i),jac(1,i),jac(2,i)),
00666 Vector(jac(3,i),jac(4,i),jac(5,i)));
00667
00668
00669 CPPUNIT_ASSERT_EQUAL(Jcol1,Jcol2);
00670 }
00671 }
00672
00673 void SolverTest::FkVelAndJacLocal(Chain& chain, ChainFkSolverVel& fksolvervel, ChainJntToJacSolver& jacsolver)
00674 {
00675 JntArray q(chain.getNrOfJoints());
00676 JntArray qdot(chain.getNrOfJoints());
00677
00678 for(unsigned int i=0; i<chain.getNrOfJoints(); i++)
00679 {
00680 random(q(i));
00681 random(qdot(i));
00682 }
00683 JntArrayVel qvel(q,qdot);
00684 Jacobian jac(chain.getNrOfJoints());
00685
00686 FrameVel cart;
00687 Twist t;
00688
00689 jacsolver.JntToJac(qvel.q,jac);
00690 CPPUNIT_ASSERT_EQUAL((int)SolverI::E_NOERROR, fksolvervel.JntToCart(qvel,cart));
00691 MultiplyJacobian(jac,qvel.qdot,t);
00692 CPPUNIT_ASSERT_EQUAL(cart.deriv(),t);
00693 }
00694
00695 void SolverTest::FkVelAndIkVelLocal(Chain& chain, ChainFkSolverVel& fksolvervel, ChainIkSolverVel& iksolvervel)
00696 {
00697
00698 JntArray q(chain.getNrOfJoints());
00699 JntArray qdot(chain.getNrOfJoints());
00700
00701 for(unsigned int i=0; i<chain.getNrOfJoints(); i++)
00702 {
00703 random(q(i));
00704 random(qdot(i));
00705 }
00706 JntArrayVel qvel(q,qdot);
00707 JntArray qdot_solved(chain.getNrOfJoints());
00708
00709 FrameVel cart;
00710
00711 CPPUNIT_ASSERT_EQUAL((int)SolverI::E_NOERROR, fksolvervel.JntToCart(qvel,cart));
00712
00713 CPPUNIT_ASSERT_EQUAL((int)SolverI::E_NOERROR, iksolvervel.CartToJnt(qvel.q,cart.deriv(),qdot_solved));
00714
00715 qvel.deriv()=qdot_solved;
00716
00717 if(chain.getNrOfJoints()<=6)
00718 CPPUNIT_ASSERT(Equal(qvel.qdot,qdot_solved,1e-5));
00719 else
00720 {
00721 FrameVel cart_solved;
00722 CPPUNIT_ASSERT_EQUAL((int)SolverI::E_NOERROR,fksolvervel.JntToCart(qvel,cart_solved));
00723 CPPUNIT_ASSERT(Equal(cart.deriv(),cart_solved.deriv(),1e-5));
00724 }
00725 }
00726
00727
00728 void SolverTest::FkPosAndIkPosLocal(Chain& chain,ChainFkSolverPos& fksolverpos, ChainIkSolverPos& iksolverpos)
00729 {
00730 JntArray q(chain.getNrOfJoints());
00731 for(unsigned int i=0; i<chain.getNrOfJoints(); i++)
00732 {
00733 random(q(i));
00734 }
00735 JntArray q_init(chain.getNrOfJoints());
00736 double tmp;
00737 for(unsigned int i=0; i<chain.getNrOfJoints(); i++)
00738 {
00739 random(tmp);
00740 q_init(i)=q(i)+0.1*tmp;
00741 }
00742 JntArray q_solved(q);
00743
00744 Frame F1,F2;
00745
00746 CPPUNIT_ASSERT_EQUAL((int)SolverI::E_NOERROR, fksolverpos.JntToCart(q,F1));
00747 CPPUNIT_ASSERT_EQUAL((int)SolverI::E_NOERROR, iksolverpos.CartToJnt(q_init,F1,q_solved));
00748 CPPUNIT_ASSERT_EQUAL((int)SolverI::E_NOERROR, fksolverpos.JntToCart(q_solved,F2));
00749
00750 CPPUNIT_ASSERT_EQUAL(F1,F2);
00751
00752
00753 }
00754
00755
00756 void SolverTest::VereshchaginTest()
00757 {
00758
00759 Vector constrainXLinear(1.0, 0.0, 0.0);
00760 Vector constrainXAngular(0.0, 0.0, 0.0);
00761 Vector constrainYLinear(0.0, 0.0, 0.0);
00762 Vector constrainYAngular(0.0, 0.0, 0.0);
00763
00764
00765 Twist constraintForcesX(constrainXLinear, constrainXAngular);
00766 Twist constraintForcesY(constrainYLinear, constrainYAngular);
00767
00768 Jacobian alpha(1);
00769
00770 alpha.setColumn(0, constraintForcesX);
00771
00772
00773
00774 JntArray betha(1);
00775 betha(0) = 0.0;
00776
00777
00778
00779
00780 Vector linearAcc(0.0, 10, 0.0);
00781 Vector angularAcc(0.0, 0.0, 0.0);
00782 Twist twist1(linearAcc, angularAcc);
00783
00784
00785 Vector externalForce1(0.0, 0.0, 0.0);
00786 Vector externalTorque1(0.0, 0.0, 0.0);
00787 Vector externalForce2(0.0, 0.0, 0.0);
00788 Vector externalTorque2(0.0, 0.0, 0.0);
00789 Wrench externalNetForce1(externalForce1, externalTorque1);
00790 Wrench externalNetForce2(externalForce2, externalTorque2);
00791 Wrenches externalNetForce;
00792 externalNetForce.push_back(externalNetForce1);
00793 externalNetForce.push_back(externalNetForce2);
00794
00795
00796
00797
00798
00799
00800 int numberOfConstraints = 1;
00801 ChainIdSolver_Vereshchagin constraintSolver(chaindyn, twist1, numberOfConstraints);
00802
00803
00804
00805
00806
00807
00808 const int k = 1;
00809 JntArray jointPoses[k];
00810 JntArray jointRates[k];
00811 JntArray jointAccelerations[k];
00812 JntArray jointTorques[k];
00813 for (int i = 0; i < k; i++)
00814 {
00815 JntArray jointValues(chaindyn.getNrOfJoints());
00816 jointPoses[i] = jointValues;
00817 jointRates[i] = jointValues;
00818 jointAccelerations[i] = jointValues;
00819 jointTorques[i] = jointValues;
00820 }
00821
00822
00823 JntArray jointInitialPose(chaindyn.getNrOfJoints());
00824 jointInitialPose(0) = 0.0;
00825 jointInitialPose(1) = M_PI/6.0;
00826
00827
00828
00829
00830 jointPoses[0](0) = jointInitialPose(0);
00831 jointPoses[0](1) = jointInitialPose(1);
00832
00833
00834
00835
00836
00837
00838
00839
00840 double taskTimeConstant = 0.1;
00841 double simulationTime = 1*taskTimeConstant;
00842 double timeDelta = 0.01;
00843
00844 const std::string msg = "Assertion failed, check matrix and array sizes";
00845
00846 for (double t = 0.0; t <=simulationTime; t = t + timeDelta)
00847 {
00848 CPPUNIT_ASSERT_EQUAL((int)SolverI::E_NOERROR, constraintSolver.CartToJnt(jointPoses[0], jointRates[0], jointAccelerations[0], alpha, betha, externalNetForce, jointTorques[0]));
00849
00850
00851 jointRates[0](0) = jointRates[0](0) + jointAccelerations[0](0) * timeDelta;
00852 jointPoses[0](0) = jointPoses[0](0) + (jointRates[0](0) - jointAccelerations[0](0) * timeDelta / 2.0) * timeDelta;
00853 jointRates[0](1) = jointRates[0](1) + jointAccelerations[0](1) * timeDelta;
00854 jointPoses[0](1) = jointPoses[0](1) + (jointRates[0](1) - jointAccelerations[0](1) * timeDelta / 2.0) * timeDelta;
00855
00856 printf("%f %f %f %f %f %f %f %f %f\n", t, jointPoses[0](0), jointPoses[0](1), jointRates[0](0), jointRates[0](1), jointAccelerations[0](0), jointAccelerations[0](1), jointTorques[0](0), jointTorques[0](1));
00857 }
00858 }
00859
00860 void SolverTest::FkPosVectTest()
00861 {
00862 ChainFkSolverPos_recursive fksolver1(chain1);
00863 std::vector<Frame> v_out(chain1.getNrOfSegments());
00864
00865 JntArray q(chain1.getNrOfJoints());
00866 JntArray qdot(chain1.getNrOfJoints());
00867
00868 for(unsigned int i=0; i<chain1.getNrOfJoints(); i++)
00869 {
00870 random(q(i));
00871 random(qdot(i));
00872 }
00873 Frame f_out;
00874 fksolver1.JntToCart(q,v_out);
00875 fksolver1.JntToCart(q,f_out);
00876
00877 CPPUNIT_ASSERT(Equal(v_out[chain1.getNrOfSegments()-1],f_out,1e-5));
00878 }
00879
00880 void SolverTest::FkVelVectTest()
00881 {
00882 ChainFkSolverVel_recursive fksolver1(chain1);
00883 std::vector<FrameVel> v_out(chain1.getNrOfSegments());
00884
00885 JntArray q(chain1.getNrOfJoints());
00886 JntArray qdot(chain1.getNrOfJoints());
00887
00888 for(unsigned int i=0; i<chain1.getNrOfJoints(); i++)
00889 {
00890 random(q(i));
00891 random(qdot(i));
00892 }
00893 JntArrayVel qvel(q,qdot);
00894 FrameVel f_out;
00895 fksolver1.JntToCart(qvel,v_out);
00896 fksolver1.JntToCart(qvel,f_out);
00897
00898 CPPUNIT_ASSERT(Equal(v_out[chain1.getNrOfSegments()-1],f_out,1e-5));
00899 }
00900
00901 void SolverTest::FdSolverDevelopmentTest()
00902 {
00903 int ret;
00904 double eps=1.e-3;
00905
00906 std::cout<<"KDL FD Solver Development Test for Motoman SIA10"<<std::endl;
00907
00908
00909
00910
00911
00912 Vector gravity(0.0, 0.0, -9.81);
00913 ChainDynParam DynSolver = KDL::ChainDynParam(motomansia10dyn, gravity);
00914
00915 unsigned int nj = motomansia10dyn.getNrOfJoints();
00916 unsigned int ns = motomansia10dyn.getNrOfSegments();
00917
00918
00919 JntArray q(nj);
00920 JntArray qd(nj);
00921 JntArray qdd(nj);
00922
00923
00924 q(0) = 0.2;
00925 q(1) = 0.6;
00926 q(2) = 1.;
00927 q(3) = 0.5;
00928 q(4) = -1.4;
00929 q(5) = 0.3;
00930 q(6) = -0.8;
00931
00932 qd(0) = 1.;
00933 qd(1) = -2.;
00934 qd(2) = 3.;
00935 qd(3) = -4.;
00936 qd(4) = 5.;
00937 qd(5) = -6.;
00938 qd(6) = 7.;
00939
00940
00941 ChainFkSolverPos_recursive fksolver(motomansia10dyn);
00942 Frame f_out;
00943 fksolver.JntToCart(q,f_out);
00944 CPPUNIT_ASSERT(Equal(-0.547, f_out.p(0), eps));
00945 CPPUNIT_ASSERT(Equal(-0.301, f_out.p(1), eps));
00946 CPPUNIT_ASSERT(Equal(0.924, f_out.p(2), eps));
00947 CPPUNIT_ASSERT(Equal(0.503, f_out.M(0,0), eps));
00948 CPPUNIT_ASSERT(Equal(0.286, f_out.M(0,1), eps));
00949 CPPUNIT_ASSERT(Equal(-0.816, f_out.M(0,2), eps));
00950 CPPUNIT_ASSERT(Equal(-0.859, f_out.M(1,0), eps));
00951 CPPUNIT_ASSERT(Equal(0.269, f_out.M(1,1), eps));
00952 CPPUNIT_ASSERT(Equal(-0.436, f_out.M(1,2), eps));
00953 CPPUNIT_ASSERT(Equal(0.095, f_out.M(2,0), eps));
00954 CPPUNIT_ASSERT(Equal(0.920, f_out.M(2,1), eps));
00955 CPPUNIT_ASSERT(Equal(0.381, f_out.M(2,2), eps));
00956
00957
00958 ChainJntToJacSolver jacsolver(motomansia10dyn);
00959 Jacobian jac(nj);
00960 jacsolver.JntToJac(q, jac);
00961 double Jac[6][7] =
00962 {{0.301,-0.553,0.185,0.019,0.007,-0.086,0.},
00963 {-0.547,-0.112,-0.139,-0.376,-0.037,0.063,0.},
00964 {0.,-0.596,0.105,-0.342,-0.026,-0.113,0.},
00965 {0.,0.199,-0.553,0.788,-0.615,0.162,-0.816},
00966 {0.,-0.980,-0.112,-0.392,-0.536,-0.803,-0.436},
00967 {1.,0.,0.825,0.475,0.578,-0.573,0.381}};
00968 for ( int i=0; i<6; i++ ) {
00969 for ( int j=0; j<nj; j++ ) {
00970 CPPUNIT_ASSERT(Equal(jac(i,j), Jac[i][j], eps));
00971 }
00972 }
00973
00974
00975 JntArray taugrav(nj);
00976 JntArray taucor(nj);
00977 JntSpaceInertiaMatrix H(nj), Heff(nj);
00978
00979
00980 ret = DynSolver.JntToGravity(q, taugrav);
00981 if (ret < 0) std::cout << "KDL: inverse dynamics ERROR: " << ret << std::endl;
00982 CPPUNIT_ASSERT(Equal(0.000, taugrav(0), eps));
00983 CPPUNIT_ASSERT(Equal(-36.672, taugrav(1), eps));
00984 CPPUNIT_ASSERT(Equal(4.315, taugrav(2), eps));
00985 CPPUNIT_ASSERT(Equal(-11.205, taugrav(3), eps));
00986 CPPUNIT_ASSERT(Equal(0.757, taugrav(4), eps));
00987 CPPUNIT_ASSERT(Equal(1.780, taugrav(5), eps));
00988 CPPUNIT_ASSERT(Equal(0.000, taugrav(6), eps));
00989
00990 ret = DynSolver.JntToCoriolis(q, qd, taucor);
00991 if (ret < 0) std::cout << "KDL: inverse dynamics ERROR: " << ret << std::endl;
00992 CPPUNIT_ASSERT(Equal(-15.523, taucor(0), eps));
00993 CPPUNIT_ASSERT(Equal(24.250, taucor(1), eps));
00994 CPPUNIT_ASSERT(Equal(-6.862, taucor(2), eps));
00995 CPPUNIT_ASSERT(Equal(6.303, taucor(3), eps));
00996 CPPUNIT_ASSERT(Equal(0.110, taucor(4), eps));
00997 CPPUNIT_ASSERT(Equal(-4.898, taucor(5), eps));
00998 CPPUNIT_ASSERT(Equal(-0.249, taucor(6), eps));
00999
01000 ret = DynSolver.JntToMass(q, H);
01001 if (ret < 0) std::cout << "KDL: inverse dynamics ERROR: " << ret << std::endl;
01002 double Hexp[7][7] =
01003 {{6.8687,-0.4333,0.4599,0.6892,0.0638,-0.0054,0.0381},
01004 {-0.4333,8.8324,-0.5922,0.7905,0.0003,-0.0242,0.0265},
01005 {0.4599,-0.5922,3.3496,-0.0253,0.1150,-0.0243,0.0814},
01006 {0.6892,0.7905,-0.0253,3.9623,-0.0201,0.0087,-0.0291},
01007 {0.0638,0.0003,0.1150,-0.0201,1.1234,0.0029,0.0955},
01008 {-0.0054,-0.0242,-0.0243,0.0087,0.0029,1.1425,0},
01009 {0.0381,0.0265,0.0814,-0.0291,0.0955,0,1.1000}};
01010 for ( int i=0; i<nj; i++ ) {
01011 for ( int j=0; j<nj; j++ ) {
01012 CPPUNIT_ASSERT(Equal(H(i,j), Hexp[i][j], eps));
01013 }
01014 }
01015
01016
01017
01018
01019
01020
01021
01022 KDL::ChainIdSolver_RNE IdSolver = KDL::ChainIdSolver_RNE(motomansia10dyn, gravity);
01023
01024
01025 Vector f(10,-20,30) ;
01026 Vector n(3,-4,5) ;
01027 Wrench f_tool(f,n);
01028
01029 Wrenches f_ext(ns);
01030 for(int i=0;i<ns;i++){
01031 SetToZero(f_ext[i]);
01032 }
01033 f_ext[ns-1]=f_tool;
01034
01035 JntArray Tnoninertial(nj);
01036 JntArray jntarraynull(nj);
01037 SetToZero(jntarraynull);
01038 IdSolver.CartToJnt(q, qd, jntarraynull, f_ext, Tnoninertial);
01039 CPPUNIT_ASSERT(Equal(-21.252, Tnoninertial(0), eps));
01040 CPPUNIT_ASSERT(Equal(-37.933, Tnoninertial(1), eps));
01041 CPPUNIT_ASSERT(Equal(-2.497, Tnoninertial(2), eps));
01042 CPPUNIT_ASSERT(Equal(-15.289, Tnoninertial(3), eps));
01043 CPPUNIT_ASSERT(Equal(-4.646, Tnoninertial(4), eps));
01044 CPPUNIT_ASSERT(Equal(-9.201, Tnoninertial(5), eps));
01045 CPPUNIT_ASSERT(Equal(-5.249, Tnoninertial(6), eps));
01046
01047
01048 Eigen::MatrixXd H_eig(nj,nj), L(nj,nj);
01049 Eigen::VectorXd Tnon_eig(nj), D(nj), r(nj), acc_eig(nj);
01050 for(int i=0;i<nj;i++){
01051 Tnon_eig(i) = -Tnoninertial(i);
01052 for(int j=0;j<nj;j++){
01053 H_eig(i,j) = H(i,j);
01054 }
01055 }
01056 ldl_solver_eigen(H_eig, Tnon_eig, L, D, r, acc_eig);
01057 for(int i=0;i<nj;i++){
01058 qdd(i) = acc_eig(i);
01059 }
01060 CPPUNIT_ASSERT(Equal(2.998, qdd(0), eps));
01061 CPPUNIT_ASSERT(Equal(4.289, qdd(1), eps));
01062 CPPUNIT_ASSERT(Equal(0.946, qdd(2), eps));
01063 CPPUNIT_ASSERT(Equal(2.518, qdd(3), eps));
01064 CPPUNIT_ASSERT(Equal(3.530, qdd(4), eps));
01065 CPPUNIT_ASSERT(Equal(8.150, qdd(5), eps));
01066 CPPUNIT_ASSERT(Equal(4.254, qdd(6), eps));
01067 }
01068
01069 void SolverTest::FdSolverConsistencyTest()
01070 {
01071 int ret;
01072 double eps=1.e-3;
01073
01074 std::cout<<"KDL FD Solver Consistency Test for Motoman SIA10"<<std::endl;
01075
01076
01077
01078
01079 Vector gravity(0.0, 0.0, -9.81);
01080 KDL::ChainFdSolver_RNE FdSolver = KDL::ChainFdSolver_RNE(motomansia10dyn, gravity);
01081
01082 unsigned int nj = motomansia10dyn.getNrOfJoints();
01083 unsigned int ns = motomansia10dyn.getNrOfSegments();
01084
01085
01086 KDL::JntArray q(nj);
01087 KDL::JntArray qd(nj);
01088 KDL::JntArray qdd(nj);
01089 KDL::JntArray tau(nj);
01090
01091
01092 q(0) = 0.2;
01093 q(1) = 0.6;
01094 q(2) = 1.;
01095 q(3) = 0.5;
01096 q(4) = -1.4;
01097 q(5) = 0.3;
01098 q(6) = -0.8;
01099
01100 qd(0) = 1.;
01101 qd(1) = -2.;
01102 qd(2) = 3.;
01103 qd(3) = -4.;
01104 qd(4) = 5.;
01105 qd(5) = -6.;
01106 qd(6) = 7.;
01107
01108
01109 tau(0) = 50.;
01110 tau(1) = -20.;
01111 tau(2) = 10.;
01112 tau(3) = 40.;
01113 tau(4) = -60.;
01114 tau(5) = 15.;
01115 tau(6) = -10.;
01116
01117 KDL::Vector f(10,-20,30) ;
01118 KDL::Vector n(3,-4,5) ;
01119 KDL::Wrench f_tool(f,n);
01120
01121 KDL::Wrenches f_ext(ns);
01122 for(int i=0;i<ns;i++){
01123 SetToZero(f_ext[i]);
01124 }
01125 f_ext[ns-1]=f_tool;
01126
01127
01128 ret = FdSolver.CartToJnt(q, qd, tau, f_ext, qdd);
01129 if (ret < 0) std::cout << "KDL: forward dynamics ERROR: " << ret << std::endl;
01130 CPPUNIT_ASSERT(Equal(9.486, qdd(0), eps));
01131 CPPUNIT_ASSERT(Equal(1.830, qdd(1), eps));
01132 CPPUNIT_ASSERT(Equal(4.726, qdd(2), eps));
01133 CPPUNIT_ASSERT(Equal(11.665, qdd(3), eps));
01134 CPPUNIT_ASSERT(Equal(-50.108, qdd(4), eps));
01135 CPPUNIT_ASSERT(Equal(21.403, qdd(5), eps));
01136 CPPUNIT_ASSERT(Equal(-0.381, qdd(6), eps));
01137
01138
01139 KDL::ChainIdSolver_RNE IdSolver = KDL::ChainIdSolver_RNE(motomansia10dyn, gravity);
01140 KDL::JntArray torque(nj);
01141 IdSolver.CartToJnt(q, qd, qdd, f_ext, torque);
01142 for ( int i=0; i<nj; i++ )
01143 {
01144 CPPUNIT_ASSERT(Equal(torque(i), tau(i), eps));
01145 }
01146
01147 return;
01148 }
01149
01150 void SolverTest::LDLdecompTest()
01151 {
01152 std::cout<<"LDL Solver Test"<<std::endl;
01153 double eps=1.e-6;
01154
01155
01156
01157 Eigen::MatrixXd A(3,3), Aout(3,3);
01158 Eigen::VectorXd b(3);
01159 Eigen::MatrixXd L(3,3), Lout(3,3);
01160 Eigen::VectorXd d(3), dout(3);
01161 Eigen::VectorXd x(3), xout(3);
01162 Eigen::VectorXd r(3);
01163 Eigen::MatrixXd Dout(3,3);
01164
01165
01166 A << 4, 12, -16,
01167 12, 37, -43,
01168 -16, -43, 98;
01169 b << 28, 117, 98;
01170
01171 L << 1, 0, 0,
01172 3, 1, 0,
01173 -4, 5, 1;
01174 d << 4, 1, 9;
01175 x << 3, 8, 5;
01176
01177 ldl_solver_eigen(A, b, Lout, dout, r, xout);
01178
01179 for(int i=0;i<3;i++){
01180 for(int j=0;j<3;j++){
01181 CPPUNIT_ASSERT(Equal(L(i,j), Lout(i,j), eps));
01182 }
01183 }
01184
01185 Dout.setZero();
01186 for(int i=0;i<3;i++){
01187 Dout(i,i) = dout(i);
01188 }
01189
01190
01191 for(int i=0;i<3;i++){
01192 CPPUNIT_ASSERT(Equal(xout(i), x(i), eps));
01193 }
01194
01195
01196 Aout = Lout * Dout * Lout.transpose();
01197 for(int i=0;i<3;i++){
01198 for(int j=0;j<3;j++){
01199 CPPUNIT_ASSERT(Equal(A(i,j), Aout(i,j), eps));
01200 }
01201 }
01202
01203 return;
01204 }