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 |