1 #include <kdl/kinfam/serialchain.hpp> 2 #include <kdl/frames.hpp> 3 #include <kdl/framevel.hpp> 4 #include <kdl/frames_io.hpp> 5 #include <kdl/kinfam/crs450.hpp> 6 #include <kdl/kinfam/kuka160.hpp> 7 #include <kdl/kinfam/serialchaincartpos2jnt.hpp> 8 #include <kdl/kinfam/lineartransmission.hpp> 15 Jnt2CartPos* jnt2cartpos1 = KF1->createJnt2CartPos();
16 Jnt2CartPos* jnt2cartpos2 = KF2->createJnt2CartPos();
25 jnt2cartpos1->evaluate(q);
26 jnt2cartpos1->getFrame(F1);
27 jnt2cartpos2->evaluate(q);
28 jnt2cartpos2->getFrame(F2);
29 if (!
Equal(F1,F2,1E-7)) {
30 std::cout <<
"the two kinematic families do not give the same result." << std::endl;
31 std::cout <<
"Result of first kinematic family " << std::endl;
32 std::cout << F1 << std::endl;
33 std::cout <<
"Result of second kinematic family " << std::endl;
34 std::cout << F2 << std::endl;
44 class TestForwardAndInverse {
45 KinematicFamily* family;
46 Jnt2CartPos* jnt2cartpos;
50 JointVector q_initial;
52 CartPos2Jnt* cartpos2jnt;
54 static void TestFamily(KinematicFamily* _family) {
55 JointVector q_initial(6);
62 TestForwardAndInverse testobj(_family,q_initial);
87 class TestForwardPosAndJac {
88 KinematicFamily* family;
89 Jnt2CartPos* jnt2cartpos;
95 static void TestFamily(KinematicFamily* _family) {
96 TestForwardPosAndJac testobj(_family);
114 TestForwardPosAndJac(KinematicFamily* _family) :
116 jnt2cartpos(_family->createJnt2CartPos()),
117 jnt2jac(_family->createJnt2Jac()),
118 FJ_base_ee(_family->nrOfJoints())
121 assert( jnt2jac != 0);
122 assert( jnt2cartpos != 0);
125 int test(JointVector& q) {
126 double deltaq = 1E-4;
128 if (jnt2jac->evaluate(q)!=0)
return 1;
129 jnt2jac->getJacobian(FJ_base_ee);
130 for (
int i=0; i< q.size() ;i++) {
134 if (jnt2cartpos->evaluate(q)!=0)
return 1;
135 jnt2cartpos->getFrame(F_base_ee2);
137 if (jnt2cartpos->evaluate(q)!=0)
return 1;
138 jnt2cartpos->getFrame(F_base_ee1);
141 Twist Jcol =
diff(F_base_ee1,F_base_ee2,2*deltaq);
142 if (!
Equal(Jcol,FJ_base_ee.deriv(i),epsJ)) {
143 std::cout <<
"Difference between symbolic and numeric calculation of Jacobian for column " 145 std::cout <<
"Numeric " << Jcol << std::endl;
146 std::cout <<
"Symbolic " << FJ_base_ee.deriv(i) << std::endl;
152 ~TestForwardPosAndJac() {
163 class TestCartVelAndJac {
164 KinematicFamily* family;
165 Jnt2CartVel* jnt2cartvel;
170 static void TestFamily(KinematicFamily* _family) {
171 TestCartVelAndJac testobj(_family);
186 std::cout <<
"q[1] = " << q[1] << std::endl;
187 testobj.test(q,qdot);
194 testobj.test(q,qdot);
197 TestCartVelAndJac(KinematicFamily* _family) :
199 jnt2cartvel(_family->createJnt2CartVel()),
200 jnt2jac(_family->createJnt2Jac()),
201 FJ_base_ee(_family->nrOfJoints())
204 assert( jnt2jac != 0);
205 assert( jnt2cartvel != 0);
208 int test(JointVector& q,JointVector& qdot) {
209 std::cout <<
"Testing whether Jnt2CartVel and Jnt2Jac are consistent " << std::endl;
210 std::cout <<
"q[1] = " << q[1] << std::endl;
211 double deltaq = 1E-4;
214 result = jnt2jac->evaluate(q);
216 jnt2jac->getJacobian(FJ_base_ee);
217 result = jnt2cartvel->evaluate(q,qdot);
218 jnt2cartvel->getFrameVel(F_base_ee);
221 for (
int i=0; i< q.size() ;i++) {
222 t += FJ_base_ee.deriv(i)*qdot[i];
225 std::cout <<
"Difference between the resuls"<< std::endl;
226 std::cout <<
"via the Jacobian " << t << std::endl;
227 std::cout <<
"via the jnt2cartvel transformation " << F_base_ee.
GetTwist() << std::endl;
232 ~TestCartVelAndJac() {
244 class TestCartVelAndInverse {
245 KinematicFamily* family;
246 Jnt2CartVel* jnt2cartvel;
247 CartVel2Jnt* cartvel2jnt;
251 static void TestFamily(KinematicFamily* _family) {
252 TestCartVelAndInverse testobj(_family);
267 testobj.test(q,qdot);
274 testobj.test(q,qdot);
277 TestCartVelAndInverse(KinematicFamily* _family) :
279 jnt2cartvel(_family->createJnt2CartVel()),
280 cartvel2jnt(_family->createCartVel2Jnt()),
281 qdot2(_family->nrOfJoints())
284 assert( cartvel2jnt != 0);
285 assert( jnt2cartvel != 0);
288 int test(
const JointVector& q,
const JointVector& qdot) {
289 std::cout <<
"Testing whether Jnt2CartVel and CartVel2Jnt are consistent " << std::endl;
293 result = jnt2cartvel->evaluate(q,qdot);
295 jnt2cartvel->getFrameVel(F_base_ee);
297 cartvel2jnt->setTwist(F_base_ee.
GetTwist());
298 result = cartvel2jnt->evaluate(q, qdot2);
301 for (
int i=0;i<qdot.size();++i) {
302 if (fabs(qdot[i]-qdot2[i])>epsJ) {
303 std::cout <<
" original joint velocities and calculated joint velocities do not match" << std::endl;
305 for (
int j=0;j<qdot.size();j++) {
306 std::cout <<
"qdot["<<j<<
"]="<<qdot[j]<<
" and qdot2["<<j<<
"]="<<qdot2[j] << std::endl;
308 std::cout <<
"Frame : " << F_base_ee.
GetFrame() << std::endl;
309 std::cout <<
"Twist : " << F_base_ee.
GetTwist() << std::endl;
315 ~TestCartVelAndInverse() {
331 class CRS450_exp:
public SerialChain {
333 explicit CRS450_exp(
int jointoffset=0) :
334 SerialChain(
"CRS450", 6, jointoffset,
new LinearTransmission(6) )
340 LinearTransmission* tr = (LinearTransmission*)transmission;
341 tr->setTransmission(2,2.0,1.0);
342 tr->setTransmission(3,3.0,2.0);
354 int main(
int argc,
char* argv[]) {
355 KinematicFamily* family1,*family2,*family3,*family4;
356 std::cout << std::endl <<
"Tests on CRS450 " << std::endl;
357 family1 =
new CRS450();
358 TestForwardAndInverse::TestFamily(family1);
359 TestForwardPosAndJac::TestFamily(family1);
360 TestCartVelAndJac::TestFamily(family1);
361 TestCartVelAndInverse::TestFamily(family1);
362 std::cout <<std::endl <<
"Tests on CRS450_exp " << std::endl;
363 family2 =
new CRS450_exp();
364 TestForwardAndInverse::TestFamily(family2);
365 TestForwardPosAndJac::TestFamily(family2);
366 TestCartVelAndJac::TestFamily(family2);
367 TestCartVelAndInverse::TestFamily(family2);
368 std::cout << std::endl <<
"Tests on CRS450Feath " << std::endl;
369 family3 =
new CRS450Feath();
370 TestForwardAndInverse::TestFamily(family3);
371 TestForwardPosAndJac::TestFamily(family3);
372 TestCartVelAndJac::TestFamily(family3);
373 TestCartVelAndInverse::TestFamily(family3);
374 std::cout << std::endl <<
"Comparing the kinematic families "<< std::endl;
376 std::cout << std::endl <<
"Tests on CRS450Feath->createSerialChain " << std::endl;
377 family4 = ((ZXXZXZ*)family3)->createSerialChain();
378 TestForwardAndInverse::TestFamily(family4);
379 TestForwardPosAndJac::TestFamily(family4);
380 TestCartVelAndJac::TestFamily(family4);
381 TestCartVelAndInverse::TestFamily(family4);
386 KinematicFamily* family1b,*family2b,*family3b,*family4b;
387 family1b = family1->clone();
388 family2b = family2->clone();
389 family3b = family3->clone();
390 family4b = family4->clone();
IMETHOD Frame GetFrame() const
IMETHOD Vector diff(const Vector &p_w_a, const Vector &p_w_b, double dt=1)
IMETHOD Twist GetTwist() const
void CompareFamilies(KinematicFamily *KF1, KinematicFamily *KF2)
represents both translational and rotational velocities.
IMETHOD bool Equal(const FrameAcc &r1, const FrameAcc &r2, double eps=epsilon)
A concrete implementation of a 3 dimensional vector class.
int main(int argc, char *argv[])
static Rotation Identity()
Gives back an identity rotaton matrix.
represents a frame transformation in 3D space (rotation + translation)
const double deg2rad
the value pi/180