15 srand( (
unsigned)time( NULL ));
103 chaindyn.addSegment(segment1);
104 chaindyn.addSegment(segment2);
127 static const double scale=1;
128 static const double offset=0;
129 static const double inertiamotorA=5.0;
130 static const double inertiamotorB=3.0;
131 static const double inertiamotorC=1.0;
132 static const double damping=0;
133 static const double stiffness=0;
193 Vector(0.0,-0.3120511,-0.0038871),
199 Vector(0.0,-0.0015515,0.0),
205 Vector(0.0,0.5216809,0.0),
211 Vector(0.0,0.0119891,0.0),
217 Vector(0.0,0.0080787,0.0),
249 unsigned int nr_of_constraints = 4;
253 JntArray q_in(chain2.getNrOfJoints());
254 JntArray q_in2(chain2.getNrOfJoints());
256 for(
unsigned int i=0; i<chain2.getNrOfJoints(); i++)
261 JntArray q_out(chain2.getNrOfJoints());
262 JntArray q_out2(chain2.getNrOfJoints());
263 JntArray ff_tau(chain2.getNrOfJoints());
264 JntArray constraint_tau(chain2.getNrOfJoints());
265 Jacobian jac(chain2.getNrOfJoints());
269 Wrenches wrenches(chain2.getNrOfSegments());
270 JntSpaceInertiaMatrix m(chain2.getNrOfJoints());
273 Jacobian alpha(nr_of_constraints - 1);
274 JntArray beta(nr_of_constraints - 1);
332 q_in.
resize(chain2.getNrOfJoints());
349 q_in2.
resize(chain2.getNrOfJoints());
354 wrenches.
resize(chain2.getNrOfSegments());
357 q_out2.
resize(chain2.getNrOfSegments());
358 ff_tau.
resize(chain2.getNrOfSegments());
359 constraint_tau.
resize(chain2.getNrOfSegments());
362 alpha.
resize(nr_of_constraints);
364 beta.resize(nr_of_constraints);
366 jac.
resize(chain2.getNrOfJoints());
368 q_out.
resize(chain2.getNrOfJoints());
369 q_in3.
resize(chain2.getNrOfJoints());
370 m.resize(chain2.getNrOfJoints());
396 FkPosAndJacLocal(chain1,fksolver1,jacsolver1);
399 FkPosAndJacLocal(chain2,fksolver2,jacsolver2);
402 FkPosAndJacLocal(chain3,fksolver3,jacsolver3);
405 FkPosAndJacLocal(chain4,fksolver4,jacsolver4);
412 FkVelAndJacLocal(chain1,fksolver1,jacsolver1);
415 FkVelAndJacLocal(chain2,fksolver2,jacsolver2);
418 FkVelAndJacLocal(chain3,fksolver3,jacsolver3);
421 FkVelAndJacLocal(chain4,fksolver4,jacsolver4);
432 FkVelAndIkVelLocal(chain1,fksolver1,iksolver1);
434 FkVelAndIkVelLocal(chain1,fksolver1,iksolver_pinv_givens1);
442 FkVelAndIkVelLocal(chain2,fksolver2,iksolver2);
444 FkVelAndIkVelLocal(chain2,fksolver2,iksolver_pinv_givens2);
452 FkVelAndIkVelLocal(chain3,fksolver3,iksolver3);
454 FkVelAndIkVelLocal(chain3,fksolver3,iksolver_pinv_givens3);
462 FkVelAndIkVelLocal(chain4,fksolver4,iksolver4);
464 FkVelAndIkVelLocal(chain4,fksolver4,iksolver_pinv_givens4);
477 FkPosAndIkPosLocal(chain1,fksolver1,iksolver1);
479 FkPosAndIkPosLocal(chain1,fksolver1,iksolver1_givens);
489 FkPosAndIkPosLocal(chain2,fksolver2,iksolver2);
491 FkPosAndIkPosLocal(chain2,fksolver2,iksolver2_givens);
501 FkPosAndIkPosLocal(chain3,fksolver3,iksolver3);
503 FkPosAndIkPosLocal(chain3,fksolver3,iksolver3_givens);
513 FkPosAndIkPosLocal(chain4,fksolver4,iksolver4);
515 FkPosAndIkPosLocal(chain4,fksolver4,iksolver4_givens);
520 unsigned int maxiter = 30;
522 int maxiter_vel = 30;
523 double eps_vel = 0.1 ;
524 Frame F, dF, F_des,F_solved;
532 unsigned int nj = motomansia10.getNrOfJoints();
549 CPPUNIT_ASSERT_EQUAL(0, fksolver.
JntToCart(q,F));
553 iksolver1.
CartToJnt(q, F_des, q_solved));
556 CPPUNIT_ASSERT_EQUAL((
unsigned int)1,
559 CPPUNIT_ASSERT_EQUAL(0, fksolver.
JntToCart(q_solved,F_solved));
561 CPPUNIT_ASSERT_EQUAL(F_des,F_solved);
576 CPPUNIT_ASSERT_EQUAL(0, fksolver.
JntToCart(q,F));
583 CPPUNIT_ASSERT_EQUAL((
unsigned int)2,
604 CPPUNIT_ASSERT_EQUAL(0, fksolver.
JntToCart(q,F));
611 CPPUNIT_ASSERT_EQUAL((
unsigned int)1,
634 CPPUNIT_ASSERT_EQUAL((
unsigned int)3,
643 double lambda = 0.1 ;
649 unsigned int nj = motomansia10.getNrOfJoints();
670 std::cout<<
"smallest singular value is below threshold (lambda is scaled)"<<
std::endl;
702 double deltaq = 1E-4;
716 for (
unsigned int i=0; i< q.
rows() ; i++)
728 Vector(jac(3,i),jac(4,i),jac(5,i)));
731 CPPUNIT_ASSERT_EQUAL(Jcol1,Jcol2);
754 CPPUNIT_ASSERT_EQUAL(cart.
deriv(),t);
777 qvel.
deriv()=qdot_solved;
780 CPPUNIT_ASSERT(
Equal(qvel.
qdot,qdot_solved,1e-5));
802 q_init(i)=q(i)+0.1*tmp;
812 CPPUNIT_ASSERT_EQUAL(F1,F2);
842 int solver_return = 0;
845 unsigned int nj = kukaLWR.getNrOfJoints();
846 unsigned int ns = kukaLWR.getNrOfSegments();
850 CPPUNIT_ASSERT(
Equal(nj, ns));
891 f_ext[ns - 1] = f_tool;
901 int number_of_constraints = 6;
904 Jacobian alpha_unit_force(number_of_constraints);
907 Twist unit_force_x_l(
910 alpha_unit_force.
setColumn(0, unit_force_x_l);
912 Twist unit_force_y_l(
915 alpha_unit_force.
setColumn(1, unit_force_y_l);
917 Twist unit_force_z_l(
920 alpha_unit_force.
setColumn(2, unit_force_z_l);
922 Twist unit_force_x_a(
925 alpha_unit_force.
setColumn(3, unit_force_x_a);
927 Twist unit_force_y_a(
930 alpha_unit_force.
setColumn(4, unit_force_y_a);
932 Twist unit_force_z_a(
935 alpha_unit_force.
setColumn(5, unit_force_z_a);
938 JntArray beta_energy(number_of_constraints);
939 beta_energy(0) = -0.5;
940 beta_energy(1) = -0.5;
941 beta_energy(2) = 0.0;
942 beta_energy(3) = 0.0;
943 beta_energy(4) = 0.0;
944 beta_energy(5) = 0.2;
951 solver_return = vereshchaginSolver.
CartToJnt(q, qd, qdd, alpha_unit_force, beta_energy, f_ext, ff_tau, constraint_tau);
952 if (solver_return < 0)
std::cout <<
"KDL: Vereshchagin solver ERROR: " << solver_return <<
std::endl;
962 CPPUNIT_ASSERT(
Equal(beta_energy(0), xDotdot[ns].vel(0), eps));
963 CPPUNIT_ASSERT(
Equal(beta_energy(1), xDotdot[ns].vel(1), eps));
964 CPPUNIT_ASSERT(
Equal(beta_energy(2), xDotdot[ns].vel(2), eps));
965 CPPUNIT_ASSERT(
Equal(beta_energy(5), xDotdot[ns].rot(2), eps));
969 Eigen::VectorXd nu(number_of_constraints);
971 CPPUNIT_ASSERT(
Equal(nu(0), 669693.30355, eps));
972 CPPUNIT_ASSERT(
Equal(nu(1), 5930.60826, eps));
973 CPPUNIT_ASSERT(
Equal(nu(2), -639.5238, eps));
974 CPPUNIT_ASSERT(
Equal(nu(3), 0.000, eps));
975 CPPUNIT_ASSERT(
Equal(nu(4), 0.000, eps));
976 CPPUNIT_ASSERT(
Equal(nu(5), 573.90485, eps));
981 CPPUNIT_ASSERT(
Equal(total_tau(0), 2013.3541, eps));
982 CPPUNIT_ASSERT(
Equal(total_tau(1), -6073.4999, eps));
983 CPPUNIT_ASSERT(
Equal(total_tau(2), 2227.4487, eps));
984 CPPUNIT_ASSERT(
Equal(total_tau(3), 56.87456, eps));
985 CPPUNIT_ASSERT(
Equal(total_tau(4), -11.3789, eps));
986 CPPUNIT_ASSERT(
Equal(total_tau(5), -6.05957, eps));
987 CPPUNIT_ASSERT(
Equal(total_tau(6), 569.0776, eps));
992 Vector constrainXLinear(1.0, 0.0, 0.0);
993 Vector constrainXAngular(0.0, 0.0, 0.0);
994 Vector constrainYLinear(0.0, 0.0, 0.0);
995 Vector constrainYAngular(0.0, 0.0, 0.0);
998 Twist constraintForcesX(constrainXLinear, constrainXAngular);
999 Twist constraintForcesY(constrainYLinear, constrainYAngular);
1013 Vector linearAcc(0.0, 10, 0.0);
1014 Vector angularAcc(0.0, 0.0, 0.0);
1015 Twist twist1(linearAcc, angularAcc);
1018 Vector externalForce1(0.0, 0.0, 0.0);
1019 Vector externalTorque1(0.0, 0.0, 0.0);
1020 Vector externalForce2(0.0, 0.0, 0.0);
1021 Vector externalTorque2(0.0, 0.0, 0.0);
1022 Wrench externalNetForce1(externalForce1, externalTorque1);
1023 Wrench externalNetForce2(externalForce2, externalTorque2);
1025 externalNetForce.
push_back(externalNetForce1);
1026 externalNetForce.
push_back(externalNetForce2);
1033 int numberOfConstraints = 1;
1046 JntArray jointConstraintTorques[k];
1047 for (
int i = 0; i < k; i++)
1049 JntArray jointValues(chaindyn.getNrOfJoints());
1050 jointPoses[i] = jointValues;
1051 jointRates[i] = jointValues;
1052 jointAccelerations[i] = jointValues;
1053 jointFFTorques[i] = jointValues;
1054 jointConstraintTorques[i] = jointValues;
1058 JntArray jointInitialPose(chaindyn.getNrOfJoints());
1059 jointInitialPose(0) = 0.0;
1060 jointInitialPose(1) =
PI/6.0;
1065 jointPoses[0](0) = jointInitialPose(0);
1066 jointPoses[0](1) = jointInitialPose(1);
1075 double taskTimeConstant = 0.1;
1076 double simulationTime = 1*taskTimeConstant;
1077 double timeDelta = 0.01;
1079 const std::string msg =
"Assertion failed, check matrix and array sizes";
1081 for (
double t = 0.0; t <=simulationTime; t = t + timeDelta)
1083 CPPUNIT_ASSERT_EQUAL((
int)
SolverI::E_NOERROR, constraintSolver.
CartToJnt(jointPoses[0], jointRates[0], jointAccelerations[0], alpha, betha, externalNetForce, jointFFTorques[0], jointConstraintTorques[0]));
1086 jointRates[0](0) = jointRates[0](0) + jointAccelerations[0](0) * timeDelta;
1087 jointPoses[0](0) = jointPoses[0](0) + (jointRates[0](0) - jointAccelerations[0](0) * timeDelta / 2.0) * timeDelta;
1088 jointRates[0](1) = jointRates[0](1) + jointAccelerations[0](1) * timeDelta;
1089 jointPoses[0](1) = jointPoses[0](1) + (jointRates[0](1) - jointAccelerations[0](1) * timeDelta / 2.0) * timeDelta;
1090 jointFFTorques[0] = jointConstraintTorques[0];
1092 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), jointConstraintTorques[0](0), jointConstraintTorques[0](1));
1101 JntArray q(chain1.getNrOfJoints());
1102 JntArray qdot(chain1.getNrOfJoints());
1104 for(
unsigned int i=0; i<chain1.getNrOfJoints(); i++)
1113 CPPUNIT_ASSERT(
Equal(v_out[chain1.getNrOfSegments()-1],f_out,1e-5));
1121 JntArray q(chain1.getNrOfJoints());
1122 JntArray qdot(chain1.getNrOfJoints());
1124 for(
unsigned int i=0; i<chain1.getNrOfJoints(); i++)
1134 CPPUNIT_ASSERT(
Equal(v_out[chain1.getNrOfSegments()-1],f_out,1e-5));
1148 Vector gravity(0.0, 0.0, -9.81);
1151 unsigned int nj = motomansia10dyn.getNrOfJoints();
1152 unsigned int ns = motomansia10dyn.getNrOfSegments();
1180 CPPUNIT_ASSERT(
Equal(-0.547, f_out.
p(0), eps));
1181 CPPUNIT_ASSERT(
Equal(-0.301, f_out.
p(1), eps));
1182 CPPUNIT_ASSERT(
Equal(0.924, f_out.
p(2), eps));
1183 CPPUNIT_ASSERT(
Equal(0.503, f_out.
M(0,0), eps));
1184 CPPUNIT_ASSERT(
Equal(0.286, f_out.
M(0,1), eps));
1185 CPPUNIT_ASSERT(
Equal(-0.816, f_out.
M(0,2), eps));
1186 CPPUNIT_ASSERT(
Equal(-0.859, f_out.
M(1,0), eps));
1187 CPPUNIT_ASSERT(
Equal(0.269, f_out.
M(1,1), eps));
1188 CPPUNIT_ASSERT(
Equal(-0.436, f_out.
M(1,2), eps));
1189 CPPUNIT_ASSERT(
Equal(0.095, f_out.
M(2,0), eps));
1190 CPPUNIT_ASSERT(
Equal(0.920, f_out.
M(2,1), eps));
1191 CPPUNIT_ASSERT(
Equal(0.381, f_out.
M(2,2), eps));
1198 {{0.301,-0.553,0.185,0.019,0.007,-0.086,0.},
1199 {-0.547,-0.112,-0.139,-0.376,-0.037,0.063,0.},
1200 {0.,-0.596,0.105,-0.342,-0.026,-0.113,0.},
1201 {0.,0.199,-0.553,0.788,-0.615,0.162,-0.816},
1202 {0.,-0.980,-0.112,-0.392,-0.536,-0.803,-0.436},
1203 {1.,0.,0.825,0.475,0.578,-0.573,0.381}};
1204 for (
unsigned int i=0; i<6; i++ ) {
1205 for (
unsigned int j=0; j<nj; j++ ) {
1206 CPPUNIT_ASSERT(
Equal(jac(i,j), Jac[i][j], eps));
1213 JntSpaceInertiaMatrix H(nj), Heff(nj);
1218 CPPUNIT_ASSERT(
Equal(0.000, taugrav(0), eps));
1219 CPPUNIT_ASSERT(
Equal(-36.672, taugrav(1), eps));
1220 CPPUNIT_ASSERT(
Equal(4.315, taugrav(2), eps));
1221 CPPUNIT_ASSERT(
Equal(-11.205, taugrav(3), eps));
1222 CPPUNIT_ASSERT(
Equal(0.757, taugrav(4), eps));
1223 CPPUNIT_ASSERT(
Equal(1.780, taugrav(5), eps));
1224 CPPUNIT_ASSERT(
Equal(0.000, taugrav(6), eps));
1228 CPPUNIT_ASSERT(
Equal(-15.523, taucor(0), eps));
1229 CPPUNIT_ASSERT(
Equal(24.250, taucor(1), eps));
1230 CPPUNIT_ASSERT(
Equal(-6.862, taucor(2), eps));
1231 CPPUNIT_ASSERT(
Equal(6.303, taucor(3), eps));
1232 CPPUNIT_ASSERT(
Equal(0.110, taucor(4), eps));
1233 CPPUNIT_ASSERT(
Equal(-4.898, taucor(5), eps));
1234 CPPUNIT_ASSERT(
Equal(-0.249, taucor(6), eps));
1239 {{6.8687,-0.4333,0.4599,0.6892,0.0638,-0.0054,0.0381},
1240 {-0.4333,8.8324,-0.5922,0.7905,0.0003,-0.0242,0.0265},
1241 {0.4599,-0.5922,3.3496,-0.0253,0.1150,-0.0243,0.0814},
1242 {0.6892,0.7905,-0.0253,3.9623,-0.0201,0.0087,-0.0291},
1243 {0.0638,0.0003,0.1150,-0.0201,1.1234,0.0029,0.0955},
1244 {-0.0054,-0.0242,-0.0243,0.0087,0.0029,1.1425,0},
1245 {0.0381,0.0265,0.0814,-0.0291,0.0955,0,1.1000}};
1246 for (
unsigned int i=0; i<nj; i++ ) {
1247 for (
unsigned int j=0; j<nj; j++ ) {
1248 CPPUNIT_ASSERT(
Equal(H(i,j), Hexp[i][j], eps));
1266 for(
unsigned int i=0;i<ns;i++){
1274 IdSolver.
CartToJnt(q, qd, jntarraynull, f_ext, Tnoninertial);
1275 CPPUNIT_ASSERT(
Equal(-21.252, Tnoninertial(0), eps));
1276 CPPUNIT_ASSERT(
Equal(-37.933, Tnoninertial(1), eps));
1277 CPPUNIT_ASSERT(
Equal(-2.497, Tnoninertial(2), eps));
1278 CPPUNIT_ASSERT(
Equal(-15.289, Tnoninertial(3), eps));
1279 CPPUNIT_ASSERT(
Equal(-4.646, Tnoninertial(4), eps));
1280 CPPUNIT_ASSERT(
Equal(-9.201, Tnoninertial(5), eps));
1281 CPPUNIT_ASSERT(
Equal(-5.249, Tnoninertial(6), eps));
1284 Eigen::MatrixXd H_eig(nj,nj), L(nj,nj);
1285 Eigen::VectorXd Tnon_eig(nj), D(nj), r(nj), acc_eig(nj);
1286 for(
unsigned int i=0;i<nj;i++){
1287 Tnon_eig(i) = -Tnoninertial(i);
1288 for(
unsigned int j=0;j<nj;j++){
1289 H_eig(i,j) = H(i,j);
1293 for(
unsigned int i=0;i<nj;i++){
1294 qdd(i) = acc_eig(i);
1296 CPPUNIT_ASSERT(
Equal(2.998, qdd(0), eps));
1297 CPPUNIT_ASSERT(
Equal(4.289, qdd(1), eps));
1298 CPPUNIT_ASSERT(
Equal(0.946, qdd(2), eps));
1299 CPPUNIT_ASSERT(
Equal(2.518, qdd(3), eps));
1300 CPPUNIT_ASSERT(
Equal(3.530, qdd(4), eps));
1301 CPPUNIT_ASSERT(
Equal(8.150, qdd(5), eps));
1302 CPPUNIT_ASSERT(
Equal(4.254, qdd(6), eps));
1315 Vector gravity(0.0, 0.0, -9.81);
1318 unsigned int nj = motomansia10dyn.getNrOfJoints();
1319 unsigned int ns = motomansia10dyn.getNrOfSegments();
1358 for(
unsigned int i=0;i<ns;i++){
1364 ret = FdSolver.
CartToJnt(q, qd, tau, f_ext, qdd);
1366 CPPUNIT_ASSERT(
Equal(9.486, qdd(0), eps));
1367 CPPUNIT_ASSERT(
Equal(1.830, qdd(1), eps));
1368 CPPUNIT_ASSERT(
Equal(4.726, qdd(2), eps));
1369 CPPUNIT_ASSERT(
Equal(11.665, qdd(3), eps));
1370 CPPUNIT_ASSERT(
Equal(-50.108, qdd(4), eps));
1371 CPPUNIT_ASSERT(
Equal(21.403, qdd(5), eps));
1372 CPPUNIT_ASSERT(
Equal(-0.381, qdd(6), eps));
1377 IdSolver.
CartToJnt(q, qd, qdd, f_ext, torque);
1378 for (
unsigned int i=0; i<nj; i++ )
1380 CPPUNIT_ASSERT(
Equal(torque(i), tau(i), eps));
1393 Eigen::MatrixXd A(3,3), Aout(3,3);
1394 Eigen::VectorXd b(3);
1395 Eigen::MatrixXd L(3,3), Lout(3,3);
1396 Eigen::VectorXd d(3), dout(3);
1397 Eigen::VectorXd x(3), xout(3);
1398 Eigen::VectorXd r(3);
1399 Eigen::MatrixXd Dout(3,3);
1415 for(
int i=0;i<3;i++){
1416 for(
int j=0;j<3;j++){
1417 CPPUNIT_ASSERT(
Equal(L(i,j), Lout(i,j), eps));
1422 for(
int i=0;i<3;i++){
1423 Dout(i,i) = dout(i);
1427 for(
int i=0;i<3;i++){
1428 CPPUNIT_ASSERT(
Equal(xout(i), x(i), eps));
1432 Aout = Lout * Dout * Lout.transpose();
1433 for(
int i=0;i<3;i++){
1434 for(
int j=0;j<3;j++){
1435 CPPUNIT_ASSERT(
Equal(A(i,j), Aout(i,j), eps));
1447 Frame end_effector_pose;
1453 std::cout <<
"KDL FD (inverse-inertia version) and Vereshchagin Solvers Consistency Test for KUKA LWR 4 robot" <<
std::endl;
1457 unsigned int nj = kukaLWR.getNrOfJoints();
1458 unsigned int ns = kukaLWR.getNrOfSegments();
1462 CPPUNIT_ASSERT(
Equal(nj, ns));
1502 for(
unsigned int i=0 ;i<ns; i++)
1504 f_ext[ns - 1] = f_tool;
1508 Vector gravity(0.0, 0.0, -9.81);
1512 ret = FdSolver.
CartToJnt(q, qd, ff_tau, f_ext, qdd);
1523 int numberOfConstraints = 6;
1524 Jacobian alpha(numberOfConstraints);
1528 JntArray beta(numberOfConstraints);
1533 Vector linearAcc(0.0, 0.0, 9.81);
Vector angularAcc(0.0, 0.0, 0.0);
1534 Twist root_Acc(linearAcc, angularAcc);
1542 fksolverpos.
JntToCart(q, end_effector_pose, kukaLWR.getNrOfSegments());
1543 f_ext[ns - 1] = end_effector_pose.
M * f_tool;
1546 ret = constraintSolver.
CartToJnt(q, qd, q_dd_Ver, alpha, beta, f_ext, ff_tau, constraint_tau);
1552 CPPUNIT_ASSERT(
Equal(q_dd_Ver(0), qdd(0), eps));
1553 CPPUNIT_ASSERT(
Equal(q_dd_Ver(1), qdd(1), eps));
1554 CPPUNIT_ASSERT(
Equal(q_dd_Ver(2), qdd(2), eps));
1555 CPPUNIT_ASSERT(
Equal(q_dd_Ver(3), qdd(3), eps));
1556 CPPUNIT_ASSERT(
Equal(q_dd_Ver(4), qdd(4), eps));
1557 CPPUNIT_ASSERT(
Equal(q_dd_Ver(5), qdd(5), eps));
1558 CPPUNIT_ASSERT(
Equal(q_dd_Ver(6), qdd(6), eps));
1581 double eps_wrench = 0.5, eps_torque = 0.3;
1583 unsigned int nj = kukaLWR.getNrOfJoints();
1584 unsigned int ns = kukaLWR.getNrOfSegments();
1585 CPPUNIT_ASSERT(
Equal(nj, ns));
1601 Frame end_effector_pose;
1602 Frame desired_end_eff_pose;
1606 Eigen::Matrix<double, 6, 1> end_eff_force;
1607 Eigen::Matrix<double, 6, 1> end_eff_pos_error;
1608 Eigen::Matrix<double, 6, 1> end_eff_vel_error;
1611 Vector linearAcc(0.0, 0.0, -9.81);
Vector angularAcc(0.0, 0.0, 0.0);
1622 int numberOfConstraints = 6;
1623 Jacobian alpha(numberOfConstraints);
1624 JntArray beta(numberOfConstraints);
1627 Twist vereshchagin_root_Acc(-linearAcc, angularAcc);
1631 double sample_frequency = 1000.0;
1632 double estimation_gain = 45.0;
1633 double filter_constant = 0.5;
1678 wrench_reference.
push_back(
Wrench(
Vector(dis_force(gen), dis_force(gen), dis_force(gen)),
Vector(dis_moment(gen), 0.0, dis_moment(gen))));
1685 double k_p = 1500.0;
1689 double simulationTime = 0.4;
1690 double timeDelta = 1.0 / sample_frequency;
1693 for (
unsigned int i = 0; i < jnt_pos.
size(); i++)
1697 qd(0) = dis_jnt_vel(gen);
1698 qd(1) = dis_jnt_vel(gen);
1699 qd(2) = dis_jnt_vel(gen);
1700 qd(3) = dis_jnt_vel(gen);
1701 qd(4) = dis_jnt_vel(gen);
1702 qd(5) = dis_jnt_vel(gen);
1703 qd(6) = dis_jnt_vel(gen);
1705 end_eff_force.setZero();
1706 end_eff_pos_error.setZero();
1707 end_eff_vel_error.setZero();
1708 f_ext_base = f_ext_zero;
1715 fksolverpos.
JntToCart(q, end_effector_pose);
1716 desired_end_eff_pose.
p(0) = end_effector_pose.
p(0) + 0.02;
1717 desired_end_eff_pose.
p(1) = end_effector_pose.
p(1) + 0.02;
1718 desired_end_eff_pose.
p(2) = end_effector_pose.
p(2) + 0.02;
1719 desired_end_eff_twist.
p.
v(0) = 0.0;
1720 desired_end_eff_twist.
p.
v(1) = 0.0;
1721 desired_end_eff_twist.
p.
v(2) = 0.0;
1723 for (
double t = 0.0; t <= simulationTime; t = t + timeDelta)
1725 ret = jacobian_solver.
JntToJac(q, jacobian_end_eff);
1732 ret = fksolverpos.
JntToCart(q, end_effector_pose);
1739 jnt_position_velocity.
q = q;
1740 jnt_position_velocity.
qdot = qd;
1741 ret = fksolvervel.
JntToCart(jnt_position_velocity, end_eff_twist);
1748 end_eff_pos_error(0) = end_effector_pose.
p(0) - desired_end_eff_pose.
p(0);
1749 end_eff_pos_error(1) = end_effector_pose.
p(1) - desired_end_eff_pose.
p(1);
1750 end_eff_pos_error(2) = end_effector_pose.
p(2) - desired_end_eff_pose.
p(2);
1752 end_eff_vel_error(0) = end_eff_twist.
p.
v(0) - desired_end_eff_twist.
p.
v(0);
1753 end_eff_vel_error(1) = end_eff_twist.
p.
v(1) - desired_end_eff_twist.
p.
v(1);
1754 end_eff_vel_error(2) = end_eff_twist.
p.
v(2) - desired_end_eff_twist.
p.
v(2);
1756 end_eff_force = -end_eff_pos_error * k_p - end_eff_vel_error * k_d;
1759 ret = IdSolver.
CartToJnt(q, jnt_array_zero, jnt_array_zero, f_ext_zero, gravity_torque);
1767 command_torque.
data = jacobian_end_eff.
data.transpose() * end_eff_force;
1768 command_torque.
data += gravity_torque.
data;
1771 if (t > 0.2) f_ext_base[ns - 1] = end_effector_pose.
M * wrench_reference[i];
1774 ret = constraintSolver.
CartToJnt(q, qd, qdd, alpha, beta, f_ext_base, command_torque, constraint_tau);
1786 for (
unsigned int j = 0; j < nj; j++)
1789 if (q(j) < 0.0) q(j) += 360 *
deg2rad;
1793 extwrench_estimator.
JntToExtWrench(q, qd, command_torque, f_tool_estimated);
1797 Eigen::Matrix<double, 6, 1> wrench;
1798 wrench(0) = f_ext_base[ns - 1](0);
1799 wrench(1) = f_ext_base[ns - 1](1);
1800 wrench(2) = f_ext_base[ns - 1](2);
1801 wrench(3) = f_ext_base[ns - 1](3);
1802 wrench(4) = f_ext_base[ns - 1](4);
1803 wrench(5) = f_ext_base[ns - 1](5);
1804 ext_torque_reference.
data = jacobian_end_eff.
data.transpose() * wrench;
1812 CPPUNIT_ASSERT(
Equal(f_tool_estimated(0), wrench_reference[i](0), eps_wrench));
1813 CPPUNIT_ASSERT(
Equal(f_tool_estimated(1), wrench_reference[i](1), eps_wrench));
1814 CPPUNIT_ASSERT(
Equal(f_tool_estimated(2), wrench_reference[i](2), eps_wrench));
1815 CPPUNIT_ASSERT(
Equal(f_tool_estimated(3), wrench_reference[i](3), eps_wrench));
1816 CPPUNIT_ASSERT(
Equal(f_tool_estimated(4), wrench_reference[i](4), eps_wrench));
1817 CPPUNIT_ASSERT(
Equal(f_tool_estimated(5), wrench_reference[i](5), eps_wrench));
1819 CPPUNIT_ASSERT(
Equal(ext_torque_estimated(0), ext_torque_reference(0), eps_torque));
1820 CPPUNIT_ASSERT(
Equal(ext_torque_estimated(1), ext_torque_reference(1), eps_torque));
1821 CPPUNIT_ASSERT(
Equal(ext_torque_estimated(2), ext_torque_reference(2), eps_torque));
1822 CPPUNIT_ASSERT(
Equal(ext_torque_estimated(3), ext_torque_reference(3), eps_torque));
1823 CPPUNIT_ASSERT(
Equal(ext_torque_estimated(4), ext_torque_reference(4), eps_torque));
1824 CPPUNIT_ASSERT(
Equal(ext_torque_estimated(5), ext_torque_reference(5), eps_torque));
1825 CPPUNIT_ASSERT(
Equal(ext_torque_estimated(6), ext_torque_reference(6), eps_torque));