Directory: | ./ |
---|---|
File: | include/pinocchio/algorithm/model.hpp |
Date: | 2024-08-27 18:20:05 |
Exec | Total | Coverage | |
---|---|---|---|
Lines: | 8 | 8 | 100.0% |
Branches: | 3 | 6 | 50.0% |
Line | Branch | Exec | Source |
---|---|---|---|
1 | // | ||
2 | // Copyright (c) 2019 CNRS INRIA | ||
3 | // | ||
4 | |||
5 | #ifndef __pinocchio_algorithm_model_hpp__ | ||
6 | #define __pinocchio_algorithm_model_hpp__ | ||
7 | |||
8 | #include "pinocchio/multibody/model.hpp" | ||
9 | #include "pinocchio/multibody/geometry.hpp" | ||
10 | |||
11 | namespace pinocchio | ||
12 | { | ||
13 | /** | ||
14 | * \brief Append a child model into a parent model, after a specific frame given by its index. | ||
15 | * | ||
16 | * \param[in] modelA the parent model. | ||
17 | * \param[in] modelB the child model. | ||
18 | * \param[in] frameInModelA index of the frame of modelA where to append modelB. | ||
19 | * \param[in] aMb pose of modelB universe joint (index 0) in frameInModelA. | ||
20 | * \param[out] model the resulting model. | ||
21 | * | ||
22 | * The order of the joints in the output models are | ||
23 | * - joints of modelA up to the parent of FrameInModelA, | ||
24 | * - all the descendents of parent of FrameInModelA, | ||
25 | * - the remaining joints of modelA. | ||
26 | */ | ||
27 | template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl> | ||
28 | void appendModel( | ||
29 | const ModelTpl<Scalar, Options, JointCollectionTpl> & modelA, | ||
30 | const ModelTpl<Scalar, Options, JointCollectionTpl> & modelB, | ||
31 | const FrameIndex frameInModelA, | ||
32 | const SE3Tpl<Scalar, Options> & aMb, | ||
33 | ModelTpl<Scalar, Options, JointCollectionTpl> & model); | ||
34 | |||
35 | /** | ||
36 | * \brief Append a child model into a parent model, after a specific frame given by its index. | ||
37 | * | ||
38 | * \param[in] modelA the parent model. | ||
39 | * \param[in] modelB the child model. | ||
40 | * \param[in] frameInModelA index of the frame of modelA where to append modelB. | ||
41 | * \param[in] aMb pose of modelB universe joint (index 0) in frameInModelA. | ||
42 | * | ||
43 | * \return A new model containing the fusion of modelA and modelB. | ||
44 | * | ||
45 | * The order of the joints in the output models are | ||
46 | * - joints of modelA up to the parent of FrameInModelA, | ||
47 | * - all the descendents of parent of FrameInModelA, | ||
48 | * - the remaining joints of modelA. | ||
49 | */ | ||
50 | template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl> | ||
51 | 2 | ModelTpl<Scalar, Options, JointCollectionTpl> appendModel( | |
52 | const ModelTpl<Scalar, Options, JointCollectionTpl> & modelA, | ||
53 | const ModelTpl<Scalar, Options, JointCollectionTpl> & modelB, | ||
54 | const FrameIndex frameInModelA, | ||
55 | const SE3Tpl<Scalar, Options> & aMb) | ||
56 | { | ||
57 | typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model; | ||
58 | 2 | Model model; | |
59 | |||
60 |
1/2✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
|
2 | appendModel(modelA, modelB, frameInModelA, aMb, model); |
61 | |||
62 | 2 | return model; | |
63 | } | ||
64 | |||
65 | /** | ||
66 | * \brief Append a child model into a parent model, after a specific frame given by its index. | ||
67 | * | ||
68 | * \param[in] modelA the parent model. | ||
69 | * \param[in] modelB the child model. | ||
70 | * \param[in] geomModelA the parent geometry model. | ||
71 | * \param[in] geomModelB the child geometry model. | ||
72 | * \param[in] frameInModelA index of the frame of modelA where to append modelB. | ||
73 | * \param[in] aMb pose of modelB universe joint (index 0) in frameInModelA. | ||
74 | * \param[out] model the resulting model. | ||
75 | * \param[out] geomModel the resulting geometry model. | ||
76 | */ | ||
77 | template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl> | ||
78 | void appendModel( | ||
79 | const ModelTpl<Scalar, Options, JointCollectionTpl> & modelA, | ||
80 | const ModelTpl<Scalar, Options, JointCollectionTpl> & modelB, | ||
81 | const GeometryModel & geomModelA, | ||
82 | const GeometryModel & geomModelB, | ||
83 | const FrameIndex frameInModelA, | ||
84 | const SE3Tpl<Scalar, Options> & aMb, | ||
85 | ModelTpl<Scalar, Options, JointCollectionTpl> & model, | ||
86 | GeometryModel & geomModel); | ||
87 | |||
88 | /** | ||
89 | * | ||
90 | * \brief Build a reduced model from a given input model and a list of joint to lock. | ||
91 | * | ||
92 | * \param[in] model the input model to reduce. | ||
93 | * \param[in] list_of_joints_to_lock list of joints to lock in the input model. | ||
94 | * \param[in] reference_configuration reference configuration. | ||
95 | * \param[out] reduced_model the reduced model. | ||
96 | * | ||
97 | * \remarks All the joints that have been set to be fixed in the new reduced_model now appear in | ||
98 | * the kinematic tree as a Frame as FIXED_JOINT. | ||
99 | * | ||
100 | * \todo At the moment, the joint and geometry order is kept while the frames | ||
101 | * are re-ordered in a hard to predict way. Their order could be kept. | ||
102 | * | ||
103 | */ | ||
104 | template< | ||
105 | typename Scalar, | ||
106 | int Options, | ||
107 | template<typename, int> | ||
108 | class JointCollectionTpl, | ||
109 | typename ConfigVectorType> | ||
110 | void buildReducedModel( | ||
111 | const ModelTpl<Scalar, Options, JointCollectionTpl> & model, | ||
112 | std::vector<JointIndex> list_of_joints_to_lock, | ||
113 | const Eigen::MatrixBase<ConfigVectorType> & reference_configuration, | ||
114 | ModelTpl<Scalar, Options, JointCollectionTpl> & reduced_model); | ||
115 | |||
116 | /** | ||
117 | * | ||
118 | * \brief Build a reduced model from a given input model and a list of joint to lock. | ||
119 | * | ||
120 | * \param[in] model the input model to reduce. | ||
121 | * \param[in] list_of_joints_to_lock list of joints to lock in the input model. | ||
122 | * \param[in] reference_configuration reference configuration. | ||
123 | * | ||
124 | * \returns A reduce model of the input model. | ||
125 | * | ||
126 | * \remarks All the joints that have been set to be fixed in the new reduced_model now appear in | ||
127 | * the kinematic tree as a Frame as FIXED_JOINT. | ||
128 | * | ||
129 | */ | ||
130 | template< | ||
131 | typename Scalar, | ||
132 | int Options, | ||
133 | template<typename, int> | ||
134 | class JointCollectionTpl, | ||
135 | typename ConfigVectorType> | ||
136 | 8 | ModelTpl<Scalar, Options, JointCollectionTpl> buildReducedModel( | |
137 | const ModelTpl<Scalar, Options, JointCollectionTpl> & model, | ||
138 | const std::vector<JointIndex> & list_of_joints_to_lock, | ||
139 | const Eigen::MatrixBase<ConfigVectorType> & reference_configuration) | ||
140 | { | ||
141 | typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model; | ||
142 | 8 | Model reduced_model; | |
143 | |||
144 |
2/4✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 8 times.
✗ Branch 5 not taken.
|
8 | buildReducedModel(model, list_of_joints_to_lock, reference_configuration, reduced_model); |
145 | |||
146 | 8 | return reduced_model; | |
147 | } | ||
148 | |||
149 | /** | ||
150 | * | ||
151 | * \brief Build a reduced model and a rededuced geometry model from a given input model, a given | ||
152 | * input geometry model and a list of joint to lock. | ||
153 | * | ||
154 | * \param[in] model the input model to reduce. | ||
155 | * \param[in] geom_model the input geometry model to reduce. | ||
156 | * \param[in] list_of_joints_to_lock list of joints to lock in the input model. | ||
157 | * \param[in] reference_configuration reference configuration. | ||
158 | * \param[out] reduced_model the reduced model. | ||
159 | * \param[out] reduced_geom_model the reduced geometry model. | ||
160 | * | ||
161 | * \remarks All the joints that have been set to be fixed in the new reduced_model now appear in | ||
162 | * the kinematic tree as a Frame as FIXED_JOINT. | ||
163 | * | ||
164 | */ | ||
165 | template< | ||
166 | typename Scalar, | ||
167 | int Options, | ||
168 | template<typename, int> | ||
169 | class JointCollectionTpl, | ||
170 | typename ConfigVectorType> | ||
171 | void buildReducedModel( | ||
172 | const ModelTpl<Scalar, Options, JointCollectionTpl> & model, | ||
173 | const GeometryModel & geom_model, | ||
174 | const std::vector<JointIndex> & list_of_joints_to_lock, | ||
175 | const Eigen::MatrixBase<ConfigVectorType> & reference_configuration, | ||
176 | ModelTpl<Scalar, Options, JointCollectionTpl> & reduced_model, | ||
177 | GeometryModel & reduced_geom_model); | ||
178 | |||
179 | /** | ||
180 | * | ||
181 | * \brief Build a reduced model and a rededuced geometry model from a given input model, a given | ||
182 | * input geometry model and a list of joint to lock. | ||
183 | * | ||
184 | * \param[in] model the input model to reduce. | ||
185 | * \param[in] list_of_geom_models the input geometry model to reduce (example: visual_model, | ||
186 | * collision_model). \param[in] list_of_joints_to_lock list of joints to lock in the input model. | ||
187 | * \param[in] reference_configuration reference configuration. | ||
188 | * \param[out] reduced_model the reduced model. | ||
189 | * \param[out] list_of_reduced_geom_model the list of reduced geometry models. | ||
190 | * | ||
191 | * \remarks All the joints that have been set to be fixed in the new reduced_model now appear in | ||
192 | * the kinematic tree as a Frame as FIXED_JOINT. | ||
193 | * | ||
194 | */ | ||
195 | template< | ||
196 | typename Scalar, | ||
197 | int Options, | ||
198 | template<typename, int> | ||
199 | class JointCollectionTpl, | ||
200 | typename GeometryModelAllocator, | ||
201 | typename ConfigVectorType> | ||
202 | void buildReducedModel( | ||
203 | const ModelTpl<Scalar, Options, JointCollectionTpl> & model, | ||
204 | const std::vector<GeometryModel, GeometryModelAllocator> & list_of_geom_models, | ||
205 | const std::vector<JointIndex> & list_of_joints_to_lock, | ||
206 | const Eigen::MatrixBase<ConfigVectorType> & reference_configuration, | ||
207 | ModelTpl<Scalar, Options, JointCollectionTpl> & reduced_model, | ||
208 | std::vector<GeometryModel, GeometryModelAllocator> & list_of_reduced_geom_models); | ||
209 | |||
210 | /** | ||
211 | * | ||
212 | * \brief Computes the common ancestor between two joints belonging to the same kinematic tree. | ||
213 | * | ||
214 | * \param[in] model the input model. | ||
215 | * \param[in] joint1_id index of the first joint. | ||
216 | * \param[in] joint2_id index of the second joint. | ||
217 | * \param[out] index_ancestor_in_support1 index of the ancestor within model.support[joint1_id]. | ||
218 | * \param[out] index_ancestor_in_support2 index of the ancestor within model.support[joint2_id]. | ||
219 | * | ||
220 | */ | ||
221 | template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl> | ||
222 | JointIndex findCommonAncestor( | ||
223 | const ModelTpl<Scalar, Options, JointCollectionTpl> & model, | ||
224 | JointIndex joint1_id, | ||
225 | JointIndex joint2_id, | ||
226 | size_t & index_ancestor_in_support1, | ||
227 | size_t & index_ancestor_in_support2); | ||
228 | |||
229 | } // namespace pinocchio | ||
230 | |||
231 | #include "pinocchio/algorithm/model.hxx" | ||
232 | |||
233 | #if PINOCCHIO_ENABLE_TEMPLATE_INSTANTIATION | ||
234 | #include "pinocchio/algorithm/model.txx" | ||
235 | #endif // PINOCCHIO_ENABLE_TEMPLATE_INSTANTIATION | ||
236 | |||
237 | #endif // ifndef __pinocchio_algorithm_model_hpp__ | ||
238 |