GCC Code Coverage Report

Directory: ./
File: include/pinocchio/multibody/sample-models.hxx
Date: 2025-04-30 16:14:33
Exec Total Coverage
Lines: 224 225 99.6%
Branches: 551 1106 49.8%
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 #ifndef __pinocchio_multibody_sample_models_hxx__
7 #define __pinocchio_multibody_sample_models_hxx__
8
9 namespace pinocchio
10 {
11 namespace buildModels
12 {
13 namespace details
14 {
15 template<
16 typename Scalar,
17 int Options,
18 template<typename, int> class JointCollectionTpl,
19 typename JointModel>
20 25116 static JointIndex addJointAndBody(
21 ModelTpl<Scalar, Options, JointCollectionTpl> & model,
22 const JointModelBase<JointModel> & joint,
23 const std::string & parent_name,
24 const std::string & name,
25 const typename ModelTpl<Scalar, Options, JointCollectionTpl>::SE3 & placement =
26 ModelTpl<Scalar, Options, JointCollectionTpl>::SE3::Random(),
27 bool setRandomLimits = true)
28 {
29 typedef typename JointModel::ConfigVector_t CV;
30 typedef typename JointModel::TangentVector_t TV;
31
32
6/12
✓ Branch 1 taken 12558 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 12558 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 12558 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 12558 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 12558 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 12558 times.
✗ Branch 17 not taken.
 25116 CV qmin = CV::Constant(joint.nq(), -3.14), qmax = CV::Constant(joint.nq(), 3.14);
33
6/12
✓ Branch 1 taken 12558 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 12558 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 12558 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 12558 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 12558 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 12558 times.
✗ Branch 17 not taken.
 25116 TV vmax = TV::Constant(joint.nv(), 10), taumax = TV::Constant(joint.nv(), 10);
34
35 JointIndex idx;
36
37
1/2
✓ Branch 0 taken 12558 times.
✗ Branch 1 not taken.
 25116 if (setRandomLimits)
38
12/24
✓ Branch 1 taken 12558 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 12558 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 12558 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 12558 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 12558 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 12558 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 12558 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 12558 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 12558 times.
✗ Branch 26 not taken.
✓ Branch 28 taken 12558 times.
✗ Branch 29 not taken.
✓ Branch 31 taken 12558 times.
✗ Branch 32 not taken.
✓ Branch 34 taken 12558 times.
✗ Branch 35 not taken.
 100464 idx = model.addJoint(
39 model.getJointId(parent_name), joint, placement, name + "_joint",
40
4/8
✓ Branch 1 taken 12558 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 12558 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 12558 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 12558 times.
✗ Branch 11 not taken.
 25116 TV::Random(joint.nv(), 1) + TV::Constant(joint.nv(), 1, 1), // effort
41
4/8
✓ Branch 1 taken 12558 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 12558 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 12558 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 12558 times.
✗ Branch 11 not taken.
 25116 TV::Random(joint.nv(), 1) + TV::Constant(joint.nv(), 1, 1), // vel
42
4/8
✓ Branch 1 taken 12558 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 12558 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 12558 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 12558 times.
✗ Branch 11 not taken.
 25116 CV::Random(joint.nq(), 1) - CV::Constant(joint.nq(), 1, 1), // qmin
43
4/8
✓ Branch 1 taken 12558 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 12558 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 12558 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 12558 times.
✗ Branch 11 not taken.
 25116 CV::Random(joint.nq(), 1) + CV::Constant(joint.nq(), 1, 1) // qmax
44 );
45 else
46 ✗ idx = model.addJoint(
47 model.getJointId(parent_name), joint, placement, name + "_joint", taumax, vmax, qmin,
48 qmax);
49
50
1/2
✓ Branch 1 taken 12558 times.
✗ Branch 2 not taken.
 25116 model.addJointFrame(idx);
51
52
3/6
✓ Branch 1 taken 12558 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 12558 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 12558 times.
✗ Branch 8 not taken.
 25116 model.appendBodyToJoint(idx, Inertia::Random(), SE3::Identity());
53
3/6
✓ Branch 1 taken 12558 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 12558 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 12558 times.
✗ Branch 8 not taken.
 25116 model.addBodyFrame(name + "_body", idx);
54 25116 return idx;
55 }
56
57 template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
58 203 static void addManipulator(
59 ModelTpl<Scalar, Options, JointCollectionTpl> & model,
60 const bool mimic = false,
61 typename ModelTpl<Scalar, Options, JointCollectionTpl>::JointIndex root_joint_idx = 0,
62 const typename ModelTpl<Scalar, Options, JointCollectionTpl>::SE3 & Mroot =
63 ModelTpl<Scalar, Options, JointCollectionTpl>::SE3::Identity(),
64 const std::string & pre = "")
65 {
66 typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model;
67 typedef typename Model::JointIndex JointIndex;
68 typedef typename Model::SE3 SE3;
69 typedef typename Model::Inertia Inertia;
70
71 typedef JointCollectionTpl<Scalar, Options> JC;
72
6/12
✓ Branch 0 taken 22 times.
✓ Branch 1 taken 181 times.
✓ Branch 3 taken 22 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 22 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 22 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 22 times.
✗ Branch 13 not taken.
✗ Branch 15 not taken.
✗ Branch 16 not taken.
 203 static const SE3 Marm(SE3::Matrix3::Identity(), SE3::Vector3::UnitZ());
73
4/8
✓ Branch 0 taken 22 times.
✓ Branch 1 taken 181 times.
✓ Branch 3 taken 22 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 22 times.
✗ Branch 7 not taken.
✗ Branch 9 not taken.
✗ Branch 10 not taken.
 203 static const SE3 Id4 = SE3::Identity();
74
7/14
✓ Branch 0 taken 22 times.
✓ Branch 1 taken 181 times.
✓ Branch 3 taken 22 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 22 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 22 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 22 times.
✗ Branch 13 not taken.
✓ Branch 15 taken 22 times.
✗ Branch 16 not taken.
✗ Branch 18 not taken.
✗ Branch 19 not taken.
 247 static const Inertia Ijoint(
75
2/4
✓ Branch 1 taken 22 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 22 times.
✗ Branch 5 not taken.
 66 .1, Inertia::Vector3::Zero(), Inertia::Matrix3::Identity() * .01);
76
5/10
✓ Branch 0 taken 22 times.
✓ Branch 1 taken 181 times.
✓ Branch 3 taken 22 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 22 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 22 times.
✗ Branch 10 not taken.
✗ Branch 12 not taken.
✗ Branch 13 not taken.
 225 static const Inertia Iarm(
77
2/4
✓ Branch 1 taken 22 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 22 times.
✗ Branch 5 not taken.
 44 1., typename Inertia::Vector3(0, 0, .5), Inertia::Matrix3::Identity());
78 static const Scalar qmin = -3.14, qmax = 3.14;
79 static const Scalar vmax = 10., taumax = 10.;
80
81 JointIndex joint_id;
82
83 203 const std::string & root_joint_name = model.names[root_joint_idx];
84
2/4
✓ Branch 1 taken 203 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 203 times.
✗ Branch 5 not taken.
 406 joint_id = addJointAndBody(
85
1/2
✓ Branch 1 taken 203 times.
✗ Branch 2 not taken.
 406 model, typename JC::JointModelRX(), root_joint_name, pre + "shoulder1", Mroot);
86
1/2
✓ Branch 2 taken 203 times.
✗ Branch 3 not taken.
 203 model.inertias[joint_id] = Ijoint;
87 203 const JointIndex root_joint_id = joint_id;
88
89
2/4
✓ Branch 1 taken 203 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 203 times.
✗ Branch 5 not taken.
 406 joint_id = addJointAndBody(
90
1/2
✓ Branch 2 taken 203 times.
✗ Branch 3 not taken.
 406 model, typename JC::JointModelRY(), model.names[joint_id], pre + "shoulder2", Id4);
91
1/2
✓ Branch 2 taken 203 times.
✗ Branch 3 not taken.
 203 model.inertias[joint_id] = Ijoint;
92
93
2/4
✓ Branch 1 taken 203 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 203 times.
✗ Branch 5 not taken.
 406 joint_id = addJointAndBody(
94
1/2
✓ Branch 2 taken 203 times.
✗ Branch 3 not taken.
 406 model, typename JC::JointModelRZ(), model.names[joint_id], pre + "shoulder3", Id4);
95
1/2
✓ Branch 2 taken 203 times.
✗ Branch 3 not taken.
 203 model.inertias[joint_id] = Iarm;
96
3/6
✓ Branch 1 taken 203 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 203 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 203 times.
✗ Branch 8 not taken.
 203 model.addBodyFrame(pre + "upperarm_body", joint_id);
97
98
2/4
✓ Branch 1 taken 203 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 203 times.
✗ Branch 5 not taken.
 406 joint_id = addJointAndBody(
99
1/2
✓ Branch 2 taken 203 times.
✗ Branch 3 not taken.
 406 model, typename JC::JointModelRY(), model.names[joint_id], pre + "elbow", Marm);
100
1/2
✓ Branch 2 taken 203 times.
✗ Branch 3 not taken.
 203 model.inertias[joint_id] = Iarm;
101
3/6
✓ Branch 1 taken 203 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 203 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 203 times.
✗ Branch 8 not taken.
 203 model.addBodyFrame(pre + "lowerarm_body", joint_id);
102
3/6
✓ Branch 1 taken 203 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 203 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 203 times.
✗ Branch 8 not taken.
 203 model.addBodyFrame(pre + "elbow_body", joint_id);
103
104
2/4
✓ Branch 1 taken 203 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 203 times.
✗ Branch 5 not taken.
 406 joint_id = addJointAndBody(
105
1/2
✓ Branch 2 taken 203 times.
✗ Branch 3 not taken.
 406 model, typename JC::JointModelRX(), model.names[joint_id], pre + "wrist1", Marm);
106
1/2
✓ Branch 2 taken 203 times.
✗ Branch 3 not taken.
 203 model.inertias[joint_id] = Ijoint;
107
108
2/2
✓ Branch 0 taken 6 times.
✓ Branch 1 taken 197 times.
 203 if (mimic)
109 {
110 // Scalar multiplier = JC::JointModelRX::ConfigVector_t::Random(1)(0);
111 // Scalar offset = JC::JointModelRX::ConfigVector_t::Random(1)(0);
112
113 6 Scalar multiplier = 2.5;
114 6 Scalar offset = 0.75;
115
2/4
✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6 times.
✗ Branch 5 not taken.
 18 joint_id = addJointAndBody(
116 model,
117
1/2
✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
 12 typename JC::JointModelMimic(
118
1/2
✓ Branch 3 taken 6 times.
✗ Branch 4 not taken.
 12 typename JC::JointModelRY(), model.joints[joint_id].derived(), multiplier, offset),
119 6 model.names[joint_id], pre + "wrist1_mimic", Id4);
120 }
121 else
122 {
123
2/4
✓ Branch 1 taken 197 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 197 times.
✗ Branch 5 not taken.
 591 joint_id = addJointAndBody(
124
1/2
✓ Branch 2 taken 197 times.
✗ Branch 3 not taken.
 394 model, typename JC::JointModelRY(), model.names[joint_id], pre + "wrist2", Id4);
125 }
126
127
1/2
✓ Branch 2 taken 203 times.
✗ Branch 3 not taken.
 203 model.inertias[joint_id] = Iarm;
128
3/6
✓ Branch 1 taken 203 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 203 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 203 times.
✗ Branch 8 not taken.
 203 model.addBodyFrame(pre + "effector_body", joint_id);
129
2/2
✓ Branch 0 taken 6 times.
✓ Branch 1 taken 197 times.
 203 const int nq = mimic ? 5 : 6;
130
131 203 const JointModel & base_joint = model.joints[root_joint_id];
132
1/2
✓ Branch 1 taken 203 times.
✗ Branch 2 not taken.
 203 const int idx_q = base_joint.idx_q();
133
1/2
✓ Branch 1 taken 203 times.
✗ Branch 2 not taken.
 203 const int idx_v = base_joint.idx_v();
134
135
2/4
✓ Branch 1 taken 203 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 203 times.
✗ Branch 5 not taken.
 203 model.lowerPositionLimit.segment(idx_q, nq).fill(qmin);
136
2/4
✓ Branch 1 taken 203 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 203 times.
✗ Branch 5 not taken.
 203 model.upperPositionLimit.segment(idx_q, nq).fill(qmax);
137
2/4
✓ Branch 1 taken 203 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 203 times.
✗ Branch 5 not taken.
 203 model.velocityLimit.segment(idx_v, nq).fill(vmax);
138
2/4
✓ Branch 1 taken 203 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 203 times.
✗ Branch 5 not taken.
 203 model.effortLimit.segment(idx_v, nq).fill(taumax);
139 203 }
140
141 #ifdef PINOCCHIO_WITH_HPP_FCL
142 /* Add a 6DOF manipulator shoulder-elbow-wrist geometries to an existing model.
143 * <model> is the the kinematic chain, constant.
144 * <geom> is the geometry model where the new geoms are added.
145 * <pre> is the prefix (string) before every name in the model.
146 */
147 template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
148 59 static void addManipulatorGeometries(
149 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
150 GeometryModel & geom,
151 const std::string & pre = "")
152 {
153 typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model;
154 typedef typename Model::FrameIndex FrameIndex;
155 typedef typename Model::SE3 SE3;
156
157
1/2
✓ Branch 1 taken 59 times.
✗ Branch 2 not taken.
 59 const Eigen::Vector4d meshColor(1., 1., 0.78, 1.0);
158
159 FrameIndex parentFrame;
160
161
2/4
✓ Branch 1 taken 59 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 59 times.
✗ Branch 5 not taken.
 59 parentFrame = model.getBodyId(pre + "shoulder1_body");
162
7/14
✓ Branch 1 taken 59 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 59 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 59 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 59 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 59 times.
✗ Branch 16 not taken.
✓ Branch 19 taken 59 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 59 times.
✗ Branch 23 not taken.
 295 GeometryObject shoulderBall(
163
1/2
✓ Branch 2 taken 59 times.
✗ Branch 3 not taken.
 59 pre + "shoulder_object", model.frames[parentFrame].parentJoint, parentFrame,
164
3/6
✓ Branch 1 taken 59 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 59 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 59 times.
✗ Branch 8 not taken.
 118 SE3::Identity(), std::shared_ptr<fcl::Sphere>(new fcl::Sphere(0.05)), "SPHERE",
165 Eigen::Vector3d::Ones(), false, meshColor);
166
1/2
✓ Branch 1 taken 59 times.
✗ Branch 2 not taken.
 59 geom.addGeometryObject(shoulderBall);
167
168
2/4
✓ Branch 1 taken 59 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 59 times.
✗ Branch 5 not taken.
 59 parentFrame = model.getBodyId(pre + "elbow_body");
169
7/14
✓ Branch 1 taken 59 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 59 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 59 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 59 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 59 times.
✗ Branch 16 not taken.
✓ Branch 19 taken 59 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 59 times.
✗ Branch 23 not taken.
 295 GeometryObject elbowBall(
170
1/2
✓ Branch 2 taken 59 times.
✗ Branch 3 not taken.
 59 pre + "elbow_object", model.frames[parentFrame].parentJoint, parentFrame, SE3::Identity(),
171
3/6
✓ Branch 1 taken 59 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 59 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 59 times.
✗ Branch 8 not taken.
 118 std::shared_ptr<fcl::Sphere>(new fcl::Sphere(0.05)), "SPHERE", Eigen::Vector3d::Ones(),
172 false, meshColor);
173
1/2
✓ Branch 1 taken 59 times.
✗ Branch 2 not taken.
 59 geom.addGeometryObject(elbowBall);
174
175
2/4
✓ Branch 1 taken 59 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 59 times.
✗ Branch 5 not taken.
 59 parentFrame = model.getBodyId(pre + "wrist1_body");
176
7/14
✓ Branch 1 taken 59 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 59 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 59 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 59 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 59 times.
✗ Branch 16 not taken.
✓ Branch 19 taken 59 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 59 times.
✗ Branch 23 not taken.
 295 GeometryObject wristBall(
177
1/2
✓ Branch 2 taken 59 times.
✗ Branch 3 not taken.
 59 pre + "wrist_object", model.frames[parentFrame].parentJoint, parentFrame, SE3::Identity(),
178
3/6
✓ Branch 1 taken 59 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 59 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 59 times.
✗ Branch 8 not taken.
 118 std::shared_ptr<fcl::Sphere>(new fcl::Sphere(0.05)), "SPHERE", Eigen::Vector3d::Ones(),
179 false, meshColor);
180
1/2
✓ Branch 1 taken 59 times.
✗ Branch 2 not taken.
 59 geom.addGeometryObject(wristBall);
181
182
2/4
✓ Branch 1 taken 59 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 59 times.
✗ Branch 5 not taken.
 59 parentFrame = model.getBodyId(pre + "upperarm_body");
183
6/12
✓ Branch 1 taken 59 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 59 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 59 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 59 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 59 times.
✗ Branch 16 not taken.
✓ Branch 19 taken 59 times.
✗ Branch 20 not taken.
 295 GeometryObject upperArm(
184
1/2
✓ Branch 2 taken 59 times.
✗ Branch 3 not taken.
 59 pre + "upperarm_object", model.frames[parentFrame].parentJoint, parentFrame,
185
3/6
✓ Branch 1 taken 59 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 59 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 59 times.
✗ Branch 8 not taken.
 118 SE3(SE3::Matrix3::Identity(), typename SE3::Vector3(0, 0, 0.5)),
186
3/6
✓ Branch 1 taken 59 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 59 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 59 times.
✗ Branch 8 not taken.
 118 std::shared_ptr<fcl::Capsule>(new fcl::Capsule(0.05, .8)), "CAPSULE",
187 Eigen::Vector3d::Ones(), false, meshColor);
188
1/2
✓ Branch 1 taken 59 times.
✗ Branch 2 not taken.
 59 geom.addGeometryObject(upperArm);
189
190
2/4
✓ Branch 1 taken 59 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 59 times.
✗ Branch 5 not taken.
 59 parentFrame = model.getBodyId(pre + "lowerarm_body");
191
6/12
✓ Branch 1 taken 59 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 59 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 59 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 59 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 59 times.
✗ Branch 16 not taken.
✓ Branch 19 taken 59 times.
✗ Branch 20 not taken.
 295 GeometryObject lowerArm(
192
1/2
✓ Branch 2 taken 59 times.
✗ Branch 3 not taken.
 59 pre + "lowerarm_object", model.frames[parentFrame].parentJoint, parentFrame,
193
3/6
✓ Branch 1 taken 59 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 59 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 59 times.
✗ Branch 8 not taken.
 118 SE3(SE3::Matrix3::Identity(), typename SE3::Vector3(0, 0, 0.5)),
194
3/6
✓ Branch 1 taken 59 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 59 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 59 times.
✗ Branch 8 not taken.
 118 std::shared_ptr<fcl::Capsule>(new fcl::Capsule(0.05, .8)), "CAPSULE",
195 Eigen::Vector3d::Ones(), false, meshColor);
196
1/2
✓ Branch 1 taken 59 times.
✗ Branch 2 not taken.
 59 geom.addGeometryObject(lowerArm);
197
198
2/4
✓ Branch 1 taken 59 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 59 times.
✗ Branch 5 not taken.
 59 parentFrame = model.getBodyId(pre + "effector_body");
199
6/12
✓ Branch 1 taken 59 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 59 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 59 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 59 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 59 times.
✗ Branch 16 not taken.
✓ Branch 19 taken 59 times.
✗ Branch 20 not taken.
 295 GeometryObject effectorArm(
200
1/2
✓ Branch 2 taken 59 times.
✗ Branch 3 not taken.
 59 pre + "effector_object", model.frames[parentFrame].parentJoint, parentFrame,
201
3/6
✓ Branch 1 taken 59 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 59 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 59 times.
✗ Branch 8 not taken.
 118 SE3(SE3::Matrix3::Identity(), typename SE3::Vector3(0, 0, 0.1)),
202
3/6
✓ Branch 1 taken 59 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 59 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 59 times.
✗ Branch 8 not taken.
 118 std::shared_ptr<fcl::Capsule>(new fcl::Capsule(0.05, .2)), "CAPSULE",
203 Eigen::Vector3d::Ones(), false, meshColor);
204
1/2
✓ Branch 1 taken 59 times.
✗ Branch 2 not taken.
 59 geom.addGeometryObject(effectorArm);
205 59 }
206 #endif
207
208 template<typename Vector3Like>
209 static typename Eigen::AngleAxis<typename Vector3Like::Scalar>::Matrix3
210 270 rotate(const typename Vector3Like::Scalar angle, const Eigen::MatrixBase<Vector3Like> & axis)
211 {
212 typedef typename Vector3Like::Scalar Scalar;
213 typedef Eigen::AngleAxis<Scalar> AngleAxis;
214
215
1/2
✓ Branch 2 taken 270 times.
✗ Branch 3 not taken.
 540 return AngleAxis(angle, axis).toRotationMatrix();
216 }
217
218 } // namespace details
219
220 template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
221 23 void manipulator(ModelTpl<Scalar, Options, JointCollectionTpl> & model, const bool mimic)
222 {
223
3/6
✓ Branch 2 taken 23 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 23 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 23 times.
✗ Branch 9 not taken.
 23 details::addManipulator(model, mimic);
224 23 }
225
226 #ifdef PINOCCHIO_WITH_HPP_FCL
227 template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
228 3 void manipulatorGeometries(
229 const ModelTpl<Scalar, Options, JointCollectionTpl> & model, GeometryModel & geom)
230 {
231
2/4
✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 3 times.
✗ Branch 6 not taken.
 3 details::addManipulatorGeometries(model, geom);
232 3 }
233 #endif
234
235 template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
236 void
237 420 humanoidRandom(ModelTpl<Scalar, Options, JointCollectionTpl> & model, bool usingFF, bool mimic)
238 {
239 typedef JointCollectionTpl<Scalar, Options> JC;
240 typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model;
241 typedef typename Model::SE3 SE3;
242 using details::addJointAndBody;
243
4/8
✓ Branch 0 taken 68 times.
✓ Branch 1 taken 352 times.
✓ Branch 3 taken 68 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 68 times.
✗ Branch 7 not taken.
✗ Branch 9 not taken.
✗ Branch 10 not taken.
 420 static const SE3 Id = SE3::Identity();
244
245 // root
246
2/2
✓ Branch 0 taken 5 times.
✓ Branch 1 taken 415 times.
 420 if (!usingFF)
247 {
248
3/6
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 5 times.
✗ Branch 8 not taken.
 5 typename JC::JointModelComposite jff((typename JC::JointModelTranslation()));
249
3/6
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 5 times.
✗ Branch 8 not taken.
 5 jff.addJoint(typename JC::JointModelSphericalZYX());
250
3/6
✓ Branch 2 taken 5 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 5 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 5 times.
✗ Branch 10 not taken.
 5 addJointAndBody(model, jff, "universe", "root", Id);
251 5 }
252 else
253 {
254
4/8
✓ Branch 2 taken 415 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 415 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 415 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 415 times.
✗ Branch 13 not taken.
 415 addJointAndBody(model, typename JC::JointModelFreeFlyer(), "universe", "root", Id);
255
1/2
✓ Branch 2 taken 415 times.
✗ Branch 3 not taken.
 415 model.lowerPositionLimit.template segment<4>(3).fill(-1.);
256
1/2
✓ Branch 2 taken 415 times.
✗ Branch 3 not taken.
 415 model.upperPositionLimit.template segment<4>(3).fill(1.);
257 }
258
259 // lleg
260
4/8
✓ Branch 3 taken 420 times.
✗ Branch 4 not taken.
✓ Branch 7 taken 420 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 420 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 420 times.
✗ Branch 14 not taken.
 420 addJointAndBody(model, typename JC::JointModelRX(), "root_joint", "lleg1");
261
4/8
✓ Branch 3 taken 420 times.
✗ Branch 4 not taken.
✓ Branch 7 taken 420 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 420 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 420 times.
✗ Branch 14 not taken.
 420 addJointAndBody(model, typename JC::JointModelRY(), "lleg1_joint", "lleg2");
262
4/8
✓ Branch 3 taken 420 times.
✗ Branch 4 not taken.
✓ Branch 7 taken 420 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 420 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 420 times.
✗ Branch 14 not taken.
 420 addJointAndBody(model, typename JC::JointModelRZ(), "lleg2_joint", "lleg3");
263
4/8
✓ Branch 3 taken 420 times.
✗ Branch 4 not taken.
✓ Branch 7 taken 420 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 420 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 420 times.
✗ Branch 14 not taken.
 420 addJointAndBody(model, typename JC::JointModelRY(), "lleg3_joint", "lleg4");
264
4/8
✓ Branch 3 taken 420 times.
✗ Branch 4 not taken.
✓ Branch 7 taken 420 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 420 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 420 times.
✗ Branch 14 not taken.
 420 addJointAndBody(model, typename JC::JointModelRY(), "lleg4_joint", "lleg5");
265
4/8
✓ Branch 3 taken 420 times.
✗ Branch 4 not taken.
✓ Branch 7 taken 420 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 420 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 420 times.
✗ Branch 14 not taken.
 420 addJointAndBody(model, typename JC::JointModelRX(), "lleg5_joint", "lleg6");
266
267 // rleg
268
4/8
✓ Branch 3 taken 420 times.
✗ Branch 4 not taken.
✓ Branch 7 taken 420 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 420 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 420 times.
✗ Branch 14 not taken.
 420 addJointAndBody(model, typename JC::JointModelRX(), "root_joint", "rleg1");
269
4/8
✓ Branch 3 taken 420 times.
✗ Branch 4 not taken.
✓ Branch 7 taken 420 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 420 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 420 times.
✗ Branch 14 not taken.
 420 addJointAndBody(model, typename JC::JointModelRY(), "rleg1_joint", "rleg2");
270
4/8
✓ Branch 3 taken 420 times.
✗ Branch 4 not taken.
✓ Branch 7 taken 420 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 420 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 420 times.
✗ Branch 14 not taken.
 420 addJointAndBody(model, typename JC::JointModelRZ(), "rleg2_joint", "rleg3");
271
4/8
✓ Branch 3 taken 420 times.
✗ Branch 4 not taken.
✓ Branch 7 taken 420 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 420 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 420 times.
✗ Branch 14 not taken.
 420 addJointAndBody(model, typename JC::JointModelRY(), "rleg3_joint", "rleg4");
272
4/8
✓ Branch 3 taken 420 times.
✗ Branch 4 not taken.
✓ Branch 7 taken 420 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 420 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 420 times.
✗ Branch 14 not taken.
 420 addJointAndBody(model, typename JC::JointModelRY(), "rleg4_joint", "rleg5");
273
4/8
✓ Branch 3 taken 420 times.
✗ Branch 4 not taken.
✓ Branch 7 taken 420 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 420 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 420 times.
✗ Branch 14 not taken.
 420 addJointAndBody(model, typename JC::JointModelRX(), "rleg5_joint", "rleg6");
274
275 // trunc
276
4/8
✓ Branch 3 taken 420 times.
✗ Branch 4 not taken.
✓ Branch 7 taken 420 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 420 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 420 times.
✗ Branch 14 not taken.
 420 addJointAndBody(model, typename JC::JointModelRY(), "root_joint", "torso1");
277
4/8
✓ Branch 3 taken 420 times.
✗ Branch 4 not taken.
✓ Branch 7 taken 420 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 420 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 420 times.
✗ Branch 14 not taken.
 420 addJointAndBody(model, typename JC::JointModelRZ(), "torso1_joint", "chest");
278
279 // rarm
280
4/8
✓ Branch 3 taken 420 times.
✗ Branch 4 not taken.
✓ Branch 7 taken 420 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 420 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 420 times.
✗ Branch 14 not taken.
 420 addJointAndBody(model, typename JC::JointModelRX(), "chest_joint", "rarm1");
281
4/8
✓ Branch 3 taken 420 times.
✗ Branch 4 not taken.
✓ Branch 7 taken 420 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 420 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 420 times.
✗ Branch 14 not taken.
 420 addJointAndBody(model, typename JC::JointModelRY(), "rarm1_joint", "rarm2");
282
4/8
✓ Branch 3 taken 420 times.
✗ Branch 4 not taken.
✓ Branch 7 taken 420 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 420 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 420 times.
✗ Branch 14 not taken.
 420 addJointAndBody(model, typename JC::JointModelRZ(), "rarm2_joint", "rarm3");
283
4/8
✓ Branch 3 taken 420 times.
✗ Branch 4 not taken.
✓ Branch 7 taken 420 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 420 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 420 times.
✗ Branch 14 not taken.
 420 addJointAndBody(model, typename JC::JointModelRY(), "rarm3_joint", "rarm4");
284
4/8
✓ Branch 3 taken 420 times.
✗ Branch 4 not taken.
✓ Branch 7 taken 420 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 420 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 420 times.
✗ Branch 14 not taken.
 420 addJointAndBody(model, typename JC::JointModelRY(), "rarm4_joint", "rarm5");
285
4/8
✓ Branch 3 taken 420 times.
✗ Branch 4 not taken.
✓ Branch 7 taken 420 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 420 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 420 times.
✗ Branch 14 not taken.
 420 addJointAndBody(model, typename JC::JointModelRX(), "rarm5_joint", "rarm6");
286
287 // larm
288
4/8
✓ Branch 3 taken 420 times.
✗ Branch 4 not taken.
✓ Branch 7 taken 420 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 420 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 420 times.
✗ Branch 14 not taken.
 420 addJointAndBody(model, typename JC::JointModelRX(), "chest_joint", "larm1");
289
4/8
✓ Branch 3 taken 420 times.
✗ Branch 4 not taken.
✓ Branch 7 taken 420 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 420 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 420 times.
✗ Branch 14 not taken.
 420 addJointAndBody(model, typename JC::JointModelRY(), "larm1_joint", "larm2");
290
4/8
✓ Branch 3 taken 420 times.
✗ Branch 4 not taken.
✓ Branch 7 taken 420 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 420 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 420 times.
✗ Branch 14 not taken.
 420 addJointAndBody(model, typename JC::JointModelRZ(), "larm2_joint", "larm3");
291
4/8
✓ Branch 3 taken 420 times.
✗ Branch 4 not taken.
✓ Branch 7 taken 420 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 420 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 420 times.
✗ Branch 14 not taken.
 420 addJointAndBody(model, typename JC::JointModelRY(), "larm3_joint", "larm4");
292
4/8
✓ Branch 3 taken 420 times.
✗ Branch 4 not taken.
✓ Branch 7 taken 420 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 420 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 420 times.
✗ Branch 14 not taken.
 420 Index joint_id = addJointAndBody(model, typename JC::JointModelRY(), "larm4_joint", "larm5");
293
294
2/2
✓ Branch 0 taken 9 times.
✓ Branch 1 taken 411 times.
 420 if (mimic)
295 {
296 9 Scalar multiplier = 2.5;
297 9 Scalar offset = 0.75;
298
4/8
✓ Branch 1 taken 9 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 9 times.
✗ Branch 6 not taken.
✓ Branch 9 taken 9 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 9 times.
✗ Branch 13 not taken.
 27 addJointAndBody(
299 model,
300
1/2
✓ Branch 1 taken 9 times.
✗ Branch 2 not taken.
 18 typename JC::JointModelMimic(
301
1/2
✓ Branch 3 taken 9 times.
✗ Branch 4 not taken.
 18 typename JC::JointModelRX(), model.joints[joint_id].derived(), multiplier, offset),
302 "larm5_joint", "larm6");
303 }
304 else
305 {
306
4/8
✓ Branch 3 taken 411 times.
✗ Branch 4 not taken.
✓ Branch 7 taken 411 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 411 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 411 times.
✗ Branch 14 not taken.
 411 addJointAndBody(model, typename JC::JointModelRX(), "larm5_joint", "larm6");
307 }
308 420 }
309
310 template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
311 45 void humanoid(ModelTpl<Scalar, Options, JointCollectionTpl> & model, bool usingFF)
312 {
313 typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model;
314 typedef JointCollectionTpl<Scalar, Options> JC;
315 typedef typename Model::SE3 SE3;
316 typedef typename Model::Inertia Inertia;
317
318 typedef typename JC::JointModelRX::ConfigVector_t CV;
319 typedef typename JC::JointModelRX::TangentVector_t TV;
320
321 typename Model::JointIndex idx, chest, ffidx;
322
323
3/4
✓ Branch 0 taken 10 times.
✓ Branch 1 taken 35 times.
✓ Branch 3 taken 10 times.
✗ Branch 4 not taken.
 45 static const Scalar pi = PI<Scalar>();
324
325
3/6
✓ Branch 1 taken 45 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 45 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 45 times.
✗ Branch 8 not taken.
 45 SE3 Marm(SE3::Matrix3::Identity(), SE3::Vector3::UnitZ());
326
1/2
✓ Branch 1 taken 45 times.
✗ Branch 2 not taken.
 45 SE3 I4 = SE3::Identity();
327
6/12
✓ Branch 1 taken 45 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 45 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 45 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 45 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 45 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 45 times.
✗ Branch 17 not taken.
 45 Inertia Ijoint(.1, Inertia::Vector3::Zero(), Inertia::Matrix3::Identity() * .01);
328
4/8
✓ Branch 1 taken 45 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 45 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 45 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 45 times.
✗ Branch 11 not taken.
 45 Inertia Iarm(1., typename Inertia::Vector3(0, 0, .5), Inertia::Matrix3::Identity());
329
4/8
✓ Branch 1 taken 45 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 45 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 45 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 45 times.
✗ Branch 11 not taken.
 45 CV qmin = CV::Constant(-3.14), qmax = CV::Constant(3.14);
330
4/8
✓ Branch 1 taken 45 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 45 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 45 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 45 times.
✗ Branch 11 not taken.
 45 TV vmax = TV::Constant(10), taumax = TV::Constant(10);
331
332 /* --- Free flyer --- */
333
2/2
✓ Branch 0 taken 44 times.
✓ Branch 1 taken 1 times.
 45 if (usingFF)
334 {
335 44 ffidx =
336
5/10
✓ Branch 2 taken 44 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 44 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 44 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 44 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 44 times.
✗ Branch 15 not taken.
 44 model.addJoint(0, typename JC::JointModelFreeFlyer(), SE3::Identity(), "root_joint");
337
2/4
✓ Branch 1 taken 44 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 44 times.
✗ Branch 5 not taken.
 44 model.lowerPositionLimit.template segment<4>(3).fill(-1.);
338
2/4
✓ Branch 1 taken 44 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 44 times.
✗ Branch 5 not taken.
 44 model.upperPositionLimit.template segment<4>(3).fill(1.);
339 }
340 else
341 {
342
3/6
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
 1 typename JC::JointModelComposite jff((typename JC::JointModelTranslation()));
343
3/6
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
 1 jff.addJoint(typename JC::JointModelSphericalZYX());
344
4/8
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
 1 ffidx = model.addJoint(0, jff, SE3::Identity(), "root_joint");
345 1 }
346
2/4
✓ Branch 1 taken 45 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 45 times.
✗ Branch 5 not taken.
 45 model.appendBodyToJoint(ffidx, Ijoint);
347
1/2
✓ Branch 1 taken 45 times.
✗ Branch 2 not taken.
 45 model.addJointFrame(ffidx);
348
349 /* --- Lower limbs --- */
350
351
2/4
✓ Branch 2 taken 45 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 45 times.
✗ Branch 6 not taken.
 135 details::addManipulator(
352 model, false, ffidx,
353
4/8
✓ Branch 1 taken 45 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 45 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 45 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 45 times.
✗ Branch 11 not taken.
 90 SE3(details::rotate(pi, SE3::Vector3::UnitX()), typename SE3::Vector3(0, -0.2, -.1)),
354 "rleg_");
355
2/4
✓ Branch 2 taken 45 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 45 times.
✗ Branch 6 not taken.
 135 details::addManipulator(
356 model, false, ffidx,
357
4/8
✓ Branch 1 taken 45 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 45 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 45 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 45 times.
✗ Branch 11 not taken.
 90 SE3(details::rotate(pi, SE3::Vector3::UnitX()), typename SE3::Vector3(0, 0.2, -.1)),
358 "lleg_");
359
360
2/4
✓ Branch 1 taken 45 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 45 times.
✗ Branch 6 not taken.
 90 model.jointPlacements[7].rotation() =
361
1/2
✓ Branch 1 taken 45 times.
✗ Branch 2 not taken.
 45 details::rotate(pi / 2, SE3::Vector3::UnitY()); // rotate right foot
362
2/4
✓ Branch 1 taken 45 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 45 times.
✗ Branch 6 not taken.
 90 model.jointPlacements[13].rotation() =
363
1/2
✓ Branch 1 taken 45 times.
✗ Branch 2 not taken.
 45 details::rotate(pi / 2, SE3::Vector3::UnitY()); // rotate left foot
364
365 /* --- Chest --- */
366
7/14
✓ Branch 1 taken 45 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 45 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 45 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 45 times.
✗ Branch 11 not taken.
✓ Branch 14 taken 45 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 45 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 45 times.
✗ Branch 21 not taken.
 135 idx = model.addJoint(
367
1/2
✓ Branch 1 taken 45 times.
✗ Branch 2 not taken.
 90 ffidx, typename JC::JointModelRX(), I4, "chest1_joint", taumax, vmax, qmin, qmax);
368
2/4
✓ Branch 1 taken 45 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 45 times.
✗ Branch 5 not taken.
 45 model.appendBodyToJoint(idx, Ijoint);
369
1/2
✓ Branch 1 taken 45 times.
✗ Branch 2 not taken.
 45 model.addJointFrame(idx);
370
3/6
✓ Branch 1 taken 45 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 45 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 45 times.
✗ Branch 9 not taken.
 45 model.addBodyFrame("chest1_body", idx);
371
372
7/14
✓ Branch 1 taken 45 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 45 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 45 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 45 times.
✗ Branch 11 not taken.
✓ Branch 14 taken 45 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 45 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 45 times.
✗ Branch 21 not taken.
 135 idx = model.addJoint(
373
1/2
✓ Branch 1 taken 45 times.
✗ Branch 2 not taken.
 90 idx, typename JC::JointModelRY(), I4, "chest2_joint", taumax, vmax, qmin, qmax);
374
2/4
✓ Branch 1 taken 45 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 45 times.
✗ Branch 5 not taken.
 45 model.appendBodyToJoint(idx, Iarm);
375
1/2
✓ Branch 1 taken 45 times.
✗ Branch 2 not taken.
 45 model.addJointFrame(idx);
376
3/6
✓ Branch 1 taken 45 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 45 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 45 times.
✗ Branch 9 not taken.
 45 model.addBodyFrame("chest2_body", idx);
377
378 45 chest = idx;
379
380 /* --- Head --- */
381
7/14
✓ Branch 1 taken 45 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 45 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 45 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 45 times.
✗ Branch 11 not taken.
✓ Branch 14 taken 45 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 45 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 45 times.
✗ Branch 21 not taken.
 135 idx = model.addJoint(
382
4/8
✓ Branch 1 taken 45 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 45 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 45 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 45 times.
✗ Branch 11 not taken.
 90 idx, typename JC::JointModelRX(), SE3(SE3::Matrix3::Identity(), SE3::Vector3::UnitZ()),
383 "head1_joint", taumax, vmax, qmin, qmax);
384
2/4
✓ Branch 1 taken 45 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 45 times.
✗ Branch 5 not taken.
 45 model.appendBodyToJoint(idx, Ijoint);
385
1/2
✓ Branch 1 taken 45 times.
✗ Branch 2 not taken.
 45 model.addJointFrame(idx);
386
3/6
✓ Branch 1 taken 45 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 45 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 45 times.
✗ Branch 9 not taken.
 45 model.addBodyFrame("head1_body", idx);
387
388
7/14
✓ Branch 1 taken 45 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 45 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 45 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 45 times.
✗ Branch 11 not taken.
✓ Branch 14 taken 45 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 45 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 45 times.
✗ Branch 21 not taken.
 135 idx = model.addJoint(
389
1/2
✓ Branch 1 taken 45 times.
✗ Branch 2 not taken.
 90 idx, typename JC::JointModelRY(), I4, "head2_joint", taumax, vmax, qmin, qmax);
390
2/4
✓ Branch 1 taken 45 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 45 times.
✗ Branch 5 not taken.
 45 model.appendBodyToJoint(idx, Iarm);
391
1/2
✓ Branch 1 taken 45 times.
✗ Branch 2 not taken.
 45 model.addJointFrame(idx);
392
3/6
✓ Branch 1 taken 45 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 45 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 45 times.
✗ Branch 9 not taken.
 45 model.addBodyFrame("head2_body", idx);
393
394 /* --- Upper Limbs --- */
395
2/4
✓ Branch 2 taken 45 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 45 times.
✗ Branch 6 not taken.
 135 details::addManipulator(
396 model, false, chest,
397
4/8
✓ Branch 1 taken 45 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 45 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 45 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 45 times.
✗ Branch 11 not taken.
 90 SE3(details::rotate(pi, SE3::Vector3::UnitX()), typename SE3::Vector3(0, -0.3, 1.)),
398 "rarm_");
399
2/4
✓ Branch 2 taken 45 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 45 times.
✗ Branch 6 not taken.
 135 details::addManipulator(
400 model, false, chest,
401
4/8
✓ Branch 1 taken 45 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 45 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 45 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 45 times.
✗ Branch 11 not taken.
 90 SE3(details::rotate(pi, SE3::Vector3::UnitX()), typename SE3::Vector3(0, 0.3, 1.)),
402 "larm_");
403 45 }
404
405 #ifdef PINOCCHIO_WITH_HPP_FCL
406 template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
407 14 void humanoidGeometries(
408 const ModelTpl<Scalar, Options, JointCollectionTpl> & model, GeometryModel & geom)
409 {
410 typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model;
411 typedef typename Model::FrameIndex FrameIndex;
412 typedef typename Model::SE3 SE3;
413
414
2/4
✓ Branch 2 taken 14 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 14 times.
✗ Branch 6 not taken.
 14 details::addManipulatorGeometries(model, geom, "rleg_");
415
2/4
✓ Branch 2 taken 14 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 14 times.
✗ Branch 6 not taken.
 14 details::addManipulatorGeometries(model, geom, "lleg_");
416
2/4
✓ Branch 2 taken 14 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 14 times.
✗ Branch 6 not taken.
 14 details::addManipulatorGeometries(model, geom, "rarm_");
417
2/4
✓ Branch 2 taken 14 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 14 times.
✗ Branch 6 not taken.
 14 details::addManipulatorGeometries(model, geom, "larm_");
418
419 FrameIndex parentFrame;
420
421
1/2
✓ Branch 1 taken 14 times.
✗ Branch 2 not taken.
 14 const Eigen::Vector4d meshColor(1., 1., 0.78, 1.0);
422
423
2/4
✓ Branch 2 taken 14 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 14 times.
✗ Branch 6 not taken.
 14 parentFrame = model.getBodyId("chest1_body");
424
8/16
✓ Branch 1 taken 14 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 14 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 14 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 14 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 14 times.
✗ Branch 16 not taken.
✓ Branch 19 taken 14 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 14 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 14 times.
✗ Branch 26 not taken.
 70 GeometryObject chestBall(
425 14 "chest_object", model.frames[parentFrame].parentJoint, parentFrame, SE3::Identity(),
426
3/6
✓ Branch 1 taken 14 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 14 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 14 times.
✗ Branch 8 not taken.
 28 std::shared_ptr<fcl::Sphere>(new fcl::Sphere(0.05)), "SPHERE", Eigen::Vector3d::Ones(),
427 false, meshColor);
428
1/2
✓ Branch 1 taken 14 times.
✗ Branch 2 not taken.
 14 geom.addGeometryObject(chestBall);
429
430
2/4
✓ Branch 2 taken 14 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 14 times.
✗ Branch 6 not taken.
 14 parentFrame = model.getBodyId("head2_body");
431
7/14
✓ Branch 1 taken 14 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 14 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 14 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 14 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 14 times.
✗ Branch 16 not taken.
✓ Branch 19 taken 14 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 14 times.
✗ Branch 23 not taken.
 84 GeometryObject headBall(
432 14 "head_object", model.frames[parentFrame].parentJoint, parentFrame,
433
3/6
✓ Branch 1 taken 14 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 14 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 14 times.
✗ Branch 8 not taken.
 28 SE3(SE3::Matrix3::Identity(), typename SE3::Vector3(0, 0, 0.5)),
434
3/6
✓ Branch 1 taken 14 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 14 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 14 times.
✗ Branch 8 not taken.
 28 std::shared_ptr<fcl::Sphere>(new fcl::Sphere(0.25)), "SPHERE", Eigen::Vector3d::Ones(),
435 false, meshColor);
436
1/2
✓ Branch 1 taken 14 times.
✗ Branch 2 not taken.
 14 geom.addGeometryObject(headBall);
437
438
2/4
✓ Branch 2 taken 14 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 14 times.
✗ Branch 6 not taken.
 14 parentFrame = model.getBodyId("chest2_body");
439
7/14
✓ Branch 1 taken 14 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 14 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 14 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 14 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 14 times.
✗ Branch 16 not taken.
✓ Branch 19 taken 14 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 14 times.
✗ Branch 23 not taken.
 84 GeometryObject chestArm(
440 14 "chest2_object", model.frames[parentFrame].parentJoint, parentFrame,
441
3/6
✓ Branch 1 taken 14 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 14 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 14 times.
✗ Branch 8 not taken.
 28 SE3(SE3::Matrix3::Identity(), typename SE3::Vector3(0, 0, 0.5)),
442
3/6
✓ Branch 1 taken 14 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 14 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 14 times.
✗ Branch 8 not taken.
 28 std::shared_ptr<fcl::Capsule>(new fcl::Capsule(0.05, .8)), "SPHERE",
443 Eigen::Vector3d::Ones(), false, meshColor);
444
1/2
✓ Branch 1 taken 14 times.
✗ Branch 2 not taken.
 14 geom.addGeometryObject(chestArm);
445 14 }
446 #endif
447
448 } // namespace buildModels
449
450 } // namespace pinocchio
451
452 #endif // ifndef __pinocchio_multibody_sample_models_hxx__
453