GCC Code Coverage Report

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