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