GCC Code Coverage Report

Directory: ./
File: unittest/joint-translation.cpp
Date: 2024-08-27 18:20:05
Exec Total Coverage
Lines: 0 78 0.0%
Branches: 0 622 0.0%
Line Branch Exec Source
1 //
2 // Copyright (c) 2015-2020 CNRS INRIA
3 // Copyright (c) 2015 Wandercraft, 86 rue de Paris 91400 Orsay, France.
4 //
5
6 #include "pinocchio/math/fwd.hpp"
7 #include "pinocchio/multibody/joint/joints.hpp"
8 #include "pinocchio/algorithm/rnea.hpp"
9 #include "pinocchio/algorithm/aba.hpp"
10 #include "pinocchio/algorithm/crba.hpp"
11 #include "pinocchio/algorithm/jacobian.hpp"
12 #include "pinocchio/algorithm/compute-all-terms.hpp"
13
14 #include <boost/test/unit_test.hpp>
15 #include <iostream>
16
17 using namespace pinocchio;
18
19 template<typename D>
20 ✗ void addJointAndBody(
21 Model & model,
22 const JointModelBase<D> & jmodel,
23 const Model::JointIndex parent_id,
24 const SE3 & joint_placement,
25 const std::string & joint_name,
26 const Inertia & Y)
27 {
28 Model::JointIndex idx;
29
30 ✗ idx = model.addJoint(parent_id, jmodel, joint_placement, joint_name);
31 ✗ model.appendBodyToJoint(idx, Y);
32 }
33
34 BOOST_AUTO_TEST_SUITE(JointTranslation)
35
36 ✗ BOOST_AUTO_TEST_CASE(spatial)
37 {
38 typedef TransformTranslationTpl<double, 0> TransformTranslation;
39 typedef SE3::Vector3 Vector3;
40
41 ✗ const Vector3 displacement(Vector3::Random());
42 ✗ SE3 Mplain, Mrand(SE3::Random());
43
44 ✗ TransformTranslation Mtrans(displacement);
45 ✗ Mplain = Mtrans;
46 ✗ BOOST_CHECK(Mplain.translation().isApprox(displacement));
47 ✗ BOOST_CHECK(Mplain.rotation().isIdentity());
48 ✗ BOOST_CHECK((Mrand * Mplain).isApprox(Mrand * Mtrans));
49
50 ✗ SE3 M(SE3::Random());
51 ✗ Motion v(Motion::Random());
52
53 ✗ MotionTranslation mp(MotionTranslation::Vector3(1., 2., 3.));
54 ✗ Motion mp_dense(mp);
55
56 ✗ BOOST_CHECK(M.act(mp).isApprox(M.act(mp_dense)));
57 ✗ BOOST_CHECK(M.actInv(mp).isApprox(M.actInv(mp_dense)));
58
59 ✗ BOOST_CHECK(v.cross(mp).isApprox(v.cross(mp_dense)));
60 }
61
62 ✗ BOOST_AUTO_TEST_CASE(vsFreeFlyer)
63 {
64 using namespace pinocchio;
65 typedef SE3::Vector3 Vector3;
66 typedef Eigen::Matrix<double, 6, 1> Vector6;
67 typedef Eigen::Matrix<double, 7, 1> VectorFF;
68 typedef SE3::Matrix3 Matrix3;
69
70 ✗ Model modelTranslation, modelFreeflyer;
71
72 ✗ Inertia inertia(1., Vector3(0.5, 0., 0.0), Matrix3::Identity());
73 ✗ SE3 pos(1);
74 ✗ pos.translation() = SE3::LinearType(1., 0., 0.);
75
76 ✗ addJointAndBody(
77 ✗ modelTranslation, JointModelTranslation(), 0, SE3::Identity(), "translation", inertia);
78 ✗ addJointAndBody(modelFreeflyer, JointModelFreeFlyer(), 0, SE3::Identity(), "free-flyer", inertia);
79
80 ✗ Data dataTranslation(modelTranslation);
81 ✗ Data dataFreeFlyer(modelFreeflyer);
82
83 ✗ Eigen::VectorXd q = Eigen::VectorXd::Ones(modelTranslation.nq);
84 ✗ VectorFF qff;
85 ✗ qff << 1, 1, 1, 0, 0, 0, 1;
86 ✗ Eigen::VectorXd v = Eigen::VectorXd::Ones(modelTranslation.nv);
87 ✗ Vector6 vff;
88 ✗ vff << 1, 1, 1, 0, 0, 0;
89 ✗ Eigen::VectorXd tauTranslation = Eigen::VectorXd::Ones(modelTranslation.nv);
90 ✗ Eigen::VectorXd tauff(6);
91 ✗ tauff << 1, 1, 1, 0, 0, 0;
92 ✗ Eigen::VectorXd aTranslation = Eigen::VectorXd::Ones(modelTranslation.nv);
93 ✗ Eigen::VectorXd aff(vff);
94
95 ✗ forwardKinematics(modelTranslation, dataTranslation, q, v);
96 ✗ forwardKinematics(modelFreeflyer, dataFreeFlyer, qff, vff);
97
98 ✗ computeAllTerms(modelTranslation, dataTranslation, q, v);
99 ✗ computeAllTerms(modelFreeflyer, dataFreeFlyer, qff, vff);
100
101 ✗ BOOST_CHECK(dataFreeFlyer.oMi[1].isApprox(dataTranslation.oMi[1]));
102 ✗ BOOST_CHECK(dataFreeFlyer.liMi[1].isApprox(dataTranslation.liMi[1]));
103 ✗ BOOST_CHECK(dataFreeFlyer.Ycrb[1].matrix().isApprox(dataTranslation.Ycrb[1].matrix()));
104 ✗ BOOST_CHECK(dataFreeFlyer.f[1].toVector().isApprox(dataTranslation.f[1].toVector()));
105
106 ✗ Eigen::VectorXd nle_expected_ff(3);
107 ✗ nle_expected_ff << dataFreeFlyer.nle[0], dataFreeFlyer.nle[1], dataFreeFlyer.nle[2];
108 ✗ BOOST_CHECK(nle_expected_ff.isApprox(dataTranslation.nle));
109 ✗ BOOST_CHECK(dataFreeFlyer.com[0].isApprox(dataTranslation.com[0]));
110
111 // InverseDynamics == rnea
112 ✗ tauTranslation = rnea(modelTranslation, dataTranslation, q, v, aTranslation);
113 ✗ tauff = rnea(modelFreeflyer, dataFreeFlyer, qff, vff, aff);
114
115 ✗ Vector3 tau_expected;
116 ✗ tau_expected << tauff(0), tauff(1), tauff(2);
117 ✗ BOOST_CHECK(tauTranslation.isApprox(tau_expected));
118
119 // ForwardDynamics == aba
120 Eigen::VectorXd aAbaTranslation =
121 ✗ aba(modelTranslation, dataTranslation, q, v, tauTranslation, Convention::WORLD);
122 Eigen::VectorXd aAbaFreeFlyer =
123 ✗ aba(modelFreeflyer, dataFreeFlyer, qff, vff, tauff, Convention::WORLD);
124 ✗ Vector3 a_expected;
125 ✗ a_expected << aAbaFreeFlyer[0], aAbaFreeFlyer[1], aAbaFreeFlyer[2];
126 ✗ BOOST_CHECK(aAbaTranslation.isApprox(a_expected));
127
128 // crba
129 ✗ crba(modelTranslation, dataTranslation, q, Convention::WORLD);
130 ✗ crba(modelFreeflyer, dataFreeFlyer, qff, Convention::WORLD);
131
132 ✗ Eigen::Matrix<double, 3, 3> M_expected(dataFreeFlyer.M.topLeftCorner<3, 3>());
133
134 ✗ BOOST_CHECK(dataTranslation.M.isApprox(M_expected));
135
136 // Jacobian
137 ✗ Eigen::Matrix<double, 6, Eigen::Dynamic> jacobian_translation;
138 ✗ jacobian_translation.resize(6, 3);
139 ✗ jacobian_translation.setZero();
140 ✗ Eigen::Matrix<double, 6, Eigen::Dynamic> jacobian_ff;
141 ✗ jacobian_ff.resize(6, 6);
142 ✗ jacobian_ff.setZero();
143 ✗ computeJointJacobians(modelTranslation, dataTranslation, q);
144 ✗ computeJointJacobians(modelFreeflyer, dataFreeFlyer, qff);
145 ✗ getJointJacobian(modelTranslation, dataTranslation, 1, LOCAL, jacobian_translation);
146 ✗ getJointJacobian(modelFreeflyer, dataFreeFlyer, 1, LOCAL, jacobian_ff);
147
148 ✗ Eigen::Matrix<double, 6, 3> jacobian_expected;
149 ✗ jacobian_expected << jacobian_ff.col(0), jacobian_ff.col(1), jacobian_ff.col(2);
150 ✗ BOOST_CHECK(jacobian_translation.isApprox(jacobian_expected));
151 }
152
153 BOOST_AUTO_TEST_SUITE_END()
154