Line |
Branch |
Exec |
Source |
1 |
|
|
// |
2 |
|
|
// Copyright (c) 2015-2020 CNRS INRIA |
3 |
|
|
// |
4 |
|
|
|
5 |
|
|
#include "pinocchio/multibody/model.hpp" |
6 |
|
|
|
7 |
|
|
namespace pinocchio |
8 |
|
|
{ |
9 |
|
|
|
10 |
|
|
template<typename D> |
11 |
|
✗ |
void addJointAndBody( |
12 |
|
|
Model & model, |
13 |
|
|
const JointModelBase<D> & jmodel, |
14 |
|
|
const Model::JointIndex parent_id, |
15 |
|
|
const SE3 & joint_placement, |
16 |
|
|
const std::string & name, |
17 |
|
|
const Inertia & Y) |
18 |
|
|
{ |
19 |
|
|
Model::JointIndex idx; |
20 |
|
|
typedef typename D::TangentVector_t TV; |
21 |
|
|
typedef typename D::ConfigVector_t CV; |
22 |
|
|
|
23 |
|
✗ |
idx = model.addJoint( |
24 |
|
|
parent_id, jmodel, joint_placement, name + "_joint", TV::Zero(), |
25 |
|
✗ |
1e3 * (TV::Random() + TV::Constant(1)), 1e3 * (CV::Random() - CV::Constant(1)), |
26 |
|
✗ |
1e3 * (CV::Random() + CV::Constant(1))); |
27 |
|
|
|
28 |
|
✗ |
model.appendBodyToJoint(idx, Y, SE3::Identity()); |
29 |
|
|
} |
30 |
|
|
|
31 |
|
✗ |
void buildAllJointsModel(Model & model) |
32 |
|
|
{ |
33 |
|
✗ |
addJointAndBody( |
34 |
|
✗ |
model, JointModelFreeFlyer(), model.getJointId("universe"), SE3::Identity(), "freeflyer", |
35 |
|
✗ |
Inertia::Random()); |
36 |
|
✗ |
addJointAndBody( |
37 |
|
✗ |
model, JointModelSpherical(), model.getJointId("freeflyer_joint"), SE3::Identity(), |
38 |
|
✗ |
"spherical", Inertia::Random()); |
39 |
|
✗ |
addJointAndBody( |
40 |
|
✗ |
model, JointModelPlanar(), model.getJointId("spherical_joint"), SE3::Identity(), "planar", |
41 |
|
✗ |
Inertia::Random()); |
42 |
|
✗ |
addJointAndBody( |
43 |
|
✗ |
model, JointModelRX(), model.getJointId("planar_joint"), SE3::Identity(), "rx", |
44 |
|
✗ |
Inertia::Random()); |
45 |
|
✗ |
addJointAndBody( |
46 |
|
✗ |
model, JointModelPX(), model.getJointId("rx_joint"), SE3::Identity(), "px", |
47 |
|
✗ |
Inertia::Random()); |
48 |
|
✗ |
addJointAndBody( |
49 |
|
✗ |
model, JointModelHX(1.0), model.getJointId("px_joint"), SE3::Identity(), "hx", |
50 |
|
✗ |
Inertia::Random()); |
51 |
|
✗ |
addJointAndBody( |
52 |
|
✗ |
model, JointModelPrismaticUnaligned(SE3::Vector3(1, 0, 0)), model.getJointId("hx_joint"), |
53 |
|
✗ |
SE3::Identity(), "pu", Inertia::Random()); |
54 |
|
✗ |
addJointAndBody( |
55 |
|
✗ |
model, JointModelRevoluteUnaligned(SE3::Vector3(0, 0, 1)), model.getJointId("pu_joint"), |
56 |
|
✗ |
SE3::Identity(), "ru", Inertia::Random()); |
57 |
|
✗ |
addJointAndBody( |
58 |
|
✗ |
model, JointModelHelicalUnaligned(SE3::Vector3(0, 0, 1), 1.0), model.getJointId("ru_joint"), |
59 |
|
✗ |
SE3::Identity(), "hu", Inertia::Random()); |
60 |
|
✗ |
addJointAndBody( |
61 |
|
✗ |
model, JointModelSphericalZYX(), model.getJointId("hu_joint"), SE3::Identity(), |
62 |
|
✗ |
"sphericalZYX", Inertia::Random()); |
63 |
|
✗ |
addJointAndBody( |
64 |
|
✗ |
model, JointModelTranslation(), model.getJointId("sphericalZYX_joint"), SE3::Identity(), |
65 |
|
✗ |
"translation", Inertia::Random()); |
66 |
|
|
} |
67 |
|
|
|
68 |
|
|
} // namespace pinocchio |
69 |
|
|
|