GCC Code Coverage Report

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