GCC Code Coverage Report

Directory: ./
File: unittest/utils/model-generator.hpp
Date: 2025-04-30 16:14:33
Exec Total Coverage
Lines: 140 142 98.6%
Branches: 221 447 49.4%
Line Branch Exec Source
1 //
2 // Copyright (c) 2015-2020 CNRS
3 // Copyright (c) 2018-2025 INRIA
4 //
5
6 #include "pinocchio/multibody/model.hpp"
7 #include "pinocchio/algorithm/model.hpp"
8 #include "pinocchio/multibody/sample-models.hpp"
9 #include <iostream>
10
11 namespace pinocchio
12 {
13
14 template<typename D>
15 286 void addJointAndBody(
16 Model & model,
17 const JointModelBase<D> & jmodel,
18 const Model::JointIndex parent_id,
19 const SE3 & joint_placement,
20 const std::string & name,
21 const Inertia & Y)
22 {
23 Model::JointIndex idx;
24 typedef typename D::TangentVector_t TV;
25 typedef typename D::ConfigVector_t CV;
26
27
17/34
✓ Branch 1 taken 143 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 143 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 143 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 143 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 143 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 143 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 143 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 143 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 143 times.
✗ Branch 26 not taken.
✓ Branch 28 taken 143 times.
✗ Branch 29 not taken.
✓ Branch 31 taken 143 times.
✗ Branch 32 not taken.
✓ Branch 34 taken 143 times.
✗ Branch 35 not taken.
✓ Branch 37 taken 143 times.
✗ Branch 38 not taken.
✓ Branch 40 taken 143 times.
✗ Branch 41 not taken.
✓ Branch 43 taken 143 times.
✗ Branch 44 not taken.
✓ Branch 46 taken 143 times.
✗ Branch 47 not taken.
✓ Branch 49 taken 143 times.
✗ Branch 50 not taken.
 1144 idx = model.addJoint(
28 parent_id, jmodel, joint_placement, name + "_joint", TV::Zero(),
29
2/4
✓ Branch 1 taken 143 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 143 times.
✗ Branch 5 not taken.
 858 1e3 * (TV::Random() + TV::Constant(1)), 1e3 * (CV::Random() - CV::Constant(1)),
30
1/2
✓ Branch 1 taken 143 times.
✗ Branch 2 not taken.
 286 1e3 * (CV::Random() + CV::Constant(1)));
31
32
1/2
✓ Branch 2 taken 143 times.
✗ Branch 3 not taken.
 286 model.appendBodyToJoint(idx, Y, SE3::Identity());
33 286 }
34
35 13 void buildAllJointsModel(Model & model)
36 {
37
4/8
✓ Branch 2 taken 13 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 13 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 13 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 13 times.
✗ Branch 13 not taken.
 65 addJointAndBody(
38
2/4
✓ Branch 1 taken 13 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 13 times.
✗ Branch 5 not taken.
 52 model, JointModelFreeFlyer(), model.getJointId("universe"), SE3::Identity(), "freeflyer",
39 13 Inertia::Random());
40
4/8
✓ Branch 2 taken 13 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 13 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 13 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 13 times.
✗ Branch 13 not taken.
 65 addJointAndBody(
41
2/4
✓ Branch 1 taken 13 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 13 times.
✗ Branch 5 not taken.
 52 model, JointModelSpherical(), model.getJointId("freeflyer_joint"), SE3::Identity(),
42 13 "spherical", Inertia::Random());
43
4/8
✓ Branch 2 taken 13 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 13 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 13 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 13 times.
✗ Branch 13 not taken.
 65 addJointAndBody(
44
2/4
✓ Branch 1 taken 13 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 13 times.
✗ Branch 5 not taken.
 52 model, JointModelPlanar(), model.getJointId("spherical_joint"), SE3::Identity(), "planar",
45 13 Inertia::Random());
46
4/8
✓ Branch 2 taken 13 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 13 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 13 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 13 times.
✗ Branch 13 not taken.
 65 addJointAndBody(
47
2/4
✓ Branch 1 taken 13 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 13 times.
✗ Branch 5 not taken.
 52 model, JointModelRX(), model.getJointId("planar_joint"), SE3::Identity(), "rx",
48 13 Inertia::Random());
49
4/8
✓ Branch 2 taken 13 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 13 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 13 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 13 times.
✗ Branch 13 not taken.
 65 addJointAndBody(
50
2/4
✓ Branch 1 taken 13 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 13 times.
✗ Branch 5 not taken.
 52 model, JointModelPX(), model.getJointId("rx_joint"), SE3::Identity(), "px",
51 13 Inertia::Random());
52
4/8
✓ Branch 2 taken 13 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 13 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 13 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 13 times.
✗ Branch 13 not taken.
 65 addJointAndBody(
53
2/4
✓ Branch 1 taken 13 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 13 times.
✗ Branch 5 not taken.
 52 model, JointModelHX(1.0), model.getJointId("px_joint"), SE3::Identity(), "hx",
54 13 Inertia::Random());
55
4/8
✓ Branch 2 taken 13 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 13 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 13 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 13 times.
✗ Branch 13 not taken.
 65 addJointAndBody(
56
2/4
✓ Branch 1 taken 13 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 13 times.
✗ Branch 5 not taken.
 26 model, JointModelPrismaticUnaligned(SE3::Vector3(1, 0, 0)), model.getJointId("hx_joint"),
57
1/2
✓ Branch 2 taken 13 times.
✗ Branch 3 not taken.
 39 SE3::Identity(), "pu", Inertia::Random());
58
4/8
✓ Branch 2 taken 13 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 13 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 13 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 13 times.
✗ Branch 13 not taken.
 65 addJointAndBody(
59
2/4
✓ Branch 1 taken 13 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 13 times.
✗ Branch 5 not taken.
 26 model, JointModelRevoluteUnaligned(SE3::Vector3(0, 0, 1)), model.getJointId("pu_joint"),
60
1/2
✓ Branch 2 taken 13 times.
✗ Branch 3 not taken.
 39 SE3::Identity(), "ru", Inertia::Random());
61
4/8
✓ Branch 2 taken 13 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 13 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 13 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 13 times.
✗ Branch 13 not taken.
 65 addJointAndBody(
62
2/4
✓ Branch 1 taken 13 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 13 times.
✗ Branch 5 not taken.
 26 model, JointModelHelicalUnaligned(SE3::Vector3(0, 0, 1), 1.0), model.getJointId("ru_joint"),
63
1/2
✓ Branch 2 taken 13 times.
✗ Branch 3 not taken.
 39 SE3::Identity(), "hu", Inertia::Random());
64
4/8
✓ Branch 2 taken 13 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 13 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 13 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 13 times.
✗ Branch 13 not taken.
 65 addJointAndBody(
65
2/4
✓ Branch 1 taken 13 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 13 times.
✗ Branch 5 not taken.
 52 model, JointModelSphericalZYX(), model.getJointId("hu_joint"), SE3::Identity(),
66 13 "sphericalZYX", Inertia::Random());
67
4/8
✓ Branch 2 taken 13 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 13 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 13 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 13 times.
✗ Branch 13 not taken.
 65 addJointAndBody(
68
2/4
✓ Branch 1 taken 13 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 13 times.
✗ Branch 5 not taken.
 52 model, JointModelTranslation(), model.getJointId("sphericalZYX_joint"), SE3::Identity(),
69 13 "translation", Inertia::Random());
70 13 }
71
72 24 void toFull(
73 const Model & model_full,
74 const Model & model_mimic,
75 const std::vector<pinocchio::JointIndex> & mimicking_ids,
76 const std::vector<double> & ratio,
77 const std::vector<double> & offset,
78 const Eigen::VectorXd & q,
79 Eigen::VectorXd & q_full)
80 {
81
2/2
✓ Branch 0 taken 648 times.
✓ Branch 1 taken 24 times.
 672 for (int n = 1; n < model_full.njoints; n++)
82 {
83 648 double joint_ratio = 1.0;
84 648 double joint_offset = 0.0;
85
1/2
✓ Branch 3 taken 648 times.
✗ Branch 4 not taken.
 648 auto it = std::find(mimicking_ids.begin(), mimicking_ids.end(), n);
86
2/2
✓ Branch 2 taken 32 times.
✓ Branch 3 taken 616 times.
 648 if (it != mimicking_ids.end()) // If n was found
87 {
88
1/2
✓ Branch 2 taken 32 times.
✗ Branch 3 not taken.
 32 joint_ratio = ratio[size_t(std::distance(mimicking_ids.begin(), it))];
89
1/2
✓ Branch 2 taken 32 times.
✗ Branch 3 not taken.
 32 joint_offset = offset[size_t(std::distance(mimicking_ids.begin(), it))];
90 }
91
1/2
✓ Branch 2 taken 648 times.
✗ Branch 3 not taken.
 648 model_full.joints[size_t(n)].JointMappedConfigSelector(q_full) =
92
2/4
✓ Branch 2 taken 648 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 648 times.
✗ Branch 6 not taken.
 648 joint_ratio * model_mimic.joints[size_t(n)].JointMappedConfigSelector(q)
93
5/10
✓ Branch 2 taken 648 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 648 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 648 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 648 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 648 times.
✗ Branch 15 not taken.
 1944 + joint_offset * Eigen::VectorXd::Ones(model_full.joints[size_t(n)].nq());
94 }
95 24 }
96
97 54 void mimicTransformMatrix(
98 const Model & model_full,
99 const Model & model_mimic,
100 const std::vector<pinocchio::JointIndex> & /*mimicked_ids*/,
101 const std::vector<pinocchio::JointIndex> & mimicking_ids,
102 const std::vector<double> & ratios,
103 Eigen::MatrixXd & G)
104 {
105 // Initialize G as a zero matrix
106 54 G.resize(model_full.nv, model_mimic.nv);
107 54 G.setZero();
108 // Set ones on the pseudo-diagonal
109
2/2
✓ Branch 0 taken 1458 times.
✓ Branch 1 taken 54 times.
 1512 for (int j = 1; j < model_full.njoints; ++j)
110 {
111
3/4
✓ Branch 4 taken 1458 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1386 times.
✓ Branch 8 taken 72 times.
 1458 if (std::find(mimicking_ids.begin(), mimicking_ids.end(), j) == mimicking_ids.end())
112
2/4
✓ Branch 1 taken 1386 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1386 times.
✗ Branch 5 not taken.
 4158 G.block(
113
3/6
✓ Branch 2 taken 1386 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 1386 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 1386 times.
✗ Branch 10 not taken.
 1386 model_full.joints[size_t(j)].idx_v(), model_mimic.joints[size_t(j)].idx_v(),
114 2772 model_full.joints[size_t(j)].nv(), model_mimic.joints[size_t(j)].nv())
115
1/2
✓ Branch 1 taken 1386 times.
✗ Branch 2 not taken.
 1386 .setIdentity();
116 }
117
118 // Set specific values for mimicked-mimicking joint pairs
119
2/2
✓ Branch 1 taken 72 times.
✓ Branch 2 taken 54 times.
 126 for (size_t i = 0; i < mimicking_ids.size(); ++i)
120 {
121 72 G(model_full.idx_vs[mimicking_ids[i]], model_mimic.idx_vs[mimicking_ids[i]]) = ratios[i];
122 }
123 54 }
124
125 class MimicTestCases
126 {
127 public:
128 static const int N_CASES = 6;
129
130 pinocchio::Model model_mimic;
131 pinocchio::Model model_full;
132 std::vector<pinocchio::JointIndex> mimicked_ids;
133 std::vector<pinocchio::JointIndex> mimicking_ids;
134 Eigen::MatrixXd G;
135 std::vector<double> ratios;
136 std::vector<double> offsets;
137
138 54 MimicTestCases(int case_i, bool verbose = false)
139 54 : mimicked_ids()
140 54 , mimicking_ids()
141 54 , ratios()
142
2/4
✓ Branch 2 taken 54 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 54 times.
✗ Branch 6 not taken.
 162 , offsets()
143 {
144
1/2
✓ Branch 1 taken 54 times.
✗ Branch 2 not taken.
 54 pinocchio::buildModels::humanoidRandom(model_full);
145
2/4
✓ Branch 1 taken 54 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 54 times.
✗ Branch 5 not taken.
 54 model_full.lowerPositionLimit.head<3>().fill(-1.);
146
2/4
✓ Branch 1 taken 54 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 54 times.
✗ Branch 5 not taken.
 54 model_full.upperPositionLimit.head<3>().fill(1.);
147
148 // Select mimic joints based on test case
149
2/2
✓ Branch 0 taken 6 times.
✓ Branch 1 taken 48 times.
 54 if (verbose)
150 {
151
1/2
✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
 6 std::cout << "Mimic test case : ";
152 }
153
6/7
✓ Branch 0 taken 9 times.
✓ Branch 1 taken 9 times.
✓ Branch 2 taken 9 times.
✓ Branch 3 taken 9 times.
✓ Branch 4 taken 9 times.
✓ Branch 5 taken 9 times.
✗ Branch 6 not taken.
 54 switch (case_i)
154 {
155 9 case 0:
156
2/2
✓ Branch 0 taken 1 times.
✓ Branch 1 taken 8 times.
 9 if (verbose)
157 {
158
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 std::cout << "Mimicked/mimicking parent/child";
159 }
160
3/6
✓ Branch 2 taken 9 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 9 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 9 times.
✗ Branch 9 not taken.
 9 mimicked_ids.push_back(model_full.getJointId("rleg1_joint"));
161
3/6
✓ Branch 2 taken 9 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 9 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 9 times.
✗ Branch 9 not taken.
 9 mimicking_ids.push_back(model_full.getJointId("rleg2_joint"));
162
1/2
✓ Branch 1 taken 9 times.
✗ Branch 2 not taken.
 9 ratios.push_back(2.5);
163
1/2
✓ Branch 1 taken 9 times.
✗ Branch 2 not taken.
 9 offsets.push_back(0.75);
164 9 break;
165 9 case 1:
166
2/2
✓ Branch 0 taken 1 times.
✓ Branch 1 taken 8 times.
 9 if (verbose)
167 {
168
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 std::cout << "Spaced mimicked/mimicking";
169 }
170
3/6
✓ Branch 2 taken 9 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 9 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 9 times.
✗ Branch 9 not taken.
 9 mimicked_ids.push_back(model_full.getJointId("rleg1_joint"));
171
3/6
✓ Branch 2 taken 9 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 9 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 9 times.
✗ Branch 9 not taken.
 9 mimicking_ids.push_back(model_full.getJointId("rleg4_joint"));
172
1/2
✓ Branch 1 taken 9 times.
✗ Branch 2 not taken.
 9 ratios.push_back(2.5);
173
1/2
✓ Branch 1 taken 9 times.
✗ Branch 2 not taken.
 9 offsets.push_back(0.75);
174 9 break;
175 9 case 2:
176
2/2
✓ Branch 0 taken 1 times.
✓ Branch 1 taken 8 times.
 9 if (verbose)
177 {
178
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 std::cout << "Parallel mimic";
179 }
180
3/6
✓ Branch 2 taken 9 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 9 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 9 times.
✗ Branch 9 not taken.
 9 mimicked_ids.push_back(model_full.getJointId("lleg1_joint"));
181
3/6
✓ Branch 2 taken 9 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 9 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 9 times.
✗ Branch 9 not taken.
 9 mimicking_ids.push_back(model_full.getJointId("rleg1_joint"));
182
1/2
✓ Branch 1 taken 9 times.
✗ Branch 2 not taken.
 9 ratios.push_back(2.5);
183
1/2
✓ Branch 1 taken 9 times.
✗ Branch 2 not taken.
 9 offsets.push_back(0.75);
184 9 break;
185 9 case 3:
186
2/2
✓ Branch 0 taken 1 times.
✓ Branch 1 taken 8 times.
 9 if (verbose)
187 {
188
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 std::cout << "Double mimic, not same mimicked";
189 }
190
3/6
✓ Branch 2 taken 9 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 9 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 9 times.
✗ Branch 9 not taken.
 9 mimicked_ids.push_back(model_full.getJointId("lleg1_joint"));
191
3/6
✓ Branch 2 taken 9 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 9 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 9 times.
✗ Branch 9 not taken.
 9 mimicking_ids.push_back(model_full.getJointId("rleg1_joint"));
192
1/2
✓ Branch 1 taken 9 times.
✗ Branch 2 not taken.
 9 ratios.push_back(2.5);
193
1/2
✓ Branch 1 taken 9 times.
✗ Branch 2 not taken.
 9 offsets.push_back(0.75);
194
195
3/6
✓ Branch 2 taken 9 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 9 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 9 times.
✗ Branch 9 not taken.
 9 mimicked_ids.push_back(model_full.getJointId("rarm1_joint"));
196
3/6
✓ Branch 2 taken 9 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 9 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 9 times.
✗ Branch 9 not taken.
 9 mimicking_ids.push_back(model_full.getJointId("larm1_joint"));
197
1/2
✓ Branch 1 taken 9 times.
✗ Branch 2 not taken.
 9 ratios.push_back(3.2);
198
1/2
✓ Branch 1 taken 9 times.
✗ Branch 2 not taken.
 9 offsets.push_back(8);
199 9 break;
200 9 case 4:
201
2/2
✓ Branch 0 taken 1 times.
✓ Branch 1 taken 8 times.
 9 if (verbose)
202 {
203
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 std::cout << "Double mimic, same mimicked";
204 }
205
3/6
✓ Branch 2 taken 9 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 9 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 9 times.
✗ Branch 9 not taken.
 9 mimicked_ids.push_back(model_full.getJointId("lleg1_joint"));
206
3/6
✓ Branch 2 taken 9 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 9 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 9 times.
✗ Branch 9 not taken.
 9 mimicking_ids.push_back(model_full.getJointId("rleg1_joint"));
207
1/2
✓ Branch 1 taken 9 times.
✗ Branch 2 not taken.
 9 ratios.push_back(2.5);
208
1/2
✓ Branch 1 taken 9 times.
✗ Branch 2 not taken.
 9 offsets.push_back(0.75);
209
210
3/6
✓ Branch 2 taken 9 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 9 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 9 times.
✗ Branch 9 not taken.
 9 mimicked_ids.push_back(model_full.getJointId("lleg1_joint"));
211
3/6
✓ Branch 2 taken 9 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 9 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 9 times.
✗ Branch 9 not taken.
 9 mimicking_ids.push_back(model_full.getJointId("lleg2_joint"));
212
1/2
✓ Branch 1 taken 9 times.
✗ Branch 2 not taken.
 9 ratios.push_back(3.2);
213
1/2
✓ Branch 1 taken 9 times.
✗ Branch 2 not taken.
 9 offsets.push_back(8);
214 9 break;
215 9 case 5:
216
2/2
✓ Branch 0 taken 1 times.
✓ Branch 1 taken 8 times.
 9 if (verbose)
217 {
218
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 std::cout << "Mimicking terminal joint";
219 }
220
3/6
✓ Branch 2 taken 9 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 9 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 9 times.
✗ Branch 9 not taken.
 9 mimicked_ids.push_back(model_full.getJointId("larm5_joint"));
221
3/6
✓ Branch 2 taken 9 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 9 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 9 times.
✗ Branch 9 not taken.
 9 mimicking_ids.push_back(model_full.getJointId("larm6_joint"));
222
1/2
✓ Branch 1 taken 9 times.
✗ Branch 2 not taken.
 9 ratios.push_back(2.5);
223
1/2
✓ Branch 1 taken 9 times.
✗ Branch 2 not taken.
 9 offsets.push_back(0.75);
224
225 9 break;
226 ✗ default:
227 ✗ PINOCCHIO_THROW_PRETTY(
228 std::invalid_argument,
229 "No mimic test case number " << case_i << ". Max number " << N_CASES - 1);
230 }
231
232
2/2
✓ Branch 0 taken 6 times.
✓ Branch 1 taken 48 times.
 54 if (verbose)
233 {
234
1/2
✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
 6 std::cout << std::endl;
235 }
236
237
1/2
✓ Branch 1 taken 54 times.
✗ Branch 2 not taken.
 54 buildMimicModel(model_full, mimicked_ids, mimicking_ids, ratios, offsets, model_mimic);
238
1/2
✓ Branch 1 taken 54 times.
✗ Branch 2 not taken.
 54 mimicTransformMatrix(model_full, model_mimic, mimicked_ids, mimicking_ids, ratios, G);
239 54 }
240
241 24 void toFull(const Eigen::VectorXd & q, Eigen::VectorXd & q_full) const
242 {
243 24 pinocchio::toFull(model_full, model_mimic, mimicking_ids, ratios, offsets, q, q_full);
244 24 }
245 };
246 } // namespace pinocchio
247