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 |
29 |
|
|
appendModel(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 |
|
|
ModelTpl<Scalar,Options,JointCollectionTpl> |
52 |
|
2 |
appendModel(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 |
✓✗ |
2 |
appendModel(modelA,modelB,frameInModelA,aMb,model); |
61 |
|
|
|
62 |
|
2 |
return model; |
63 |
|
|
} |
64 |
|
|
|
65 |
|
|
/** |
66 |
|
|
* \copydoc pinocchio::appendModel(const ModelTpl<Scalar,Options,JointCollectionTpl>&, const ModelTpl<Scalar,Options,JointCollectionTpl> & modelB, FrameIndex frameInModelA, const SE3Tpl<Scalar, Options>& aMb, ModelTpl<Scalar,Options,JointCollectionTpl>& model); |
67 |
|
|
* |
68 |
|
|
* \param[in] geomModelA the parent geometry model. |
69 |
|
|
* \param[in] geomModelB the child geometry model. |
70 |
|
|
* \param[out] geomModel the resulting geometry model. |
71 |
|
|
*/ |
72 |
|
|
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl> |
73 |
|
|
void |
74 |
|
|
appendModel(const ModelTpl<Scalar,Options,JointCollectionTpl> & modelA, |
75 |
|
|
const ModelTpl<Scalar,Options,JointCollectionTpl> & modelB, |
76 |
|
|
const GeometryModel & geomModelA, |
77 |
|
|
const GeometryModel & geomModelB, |
78 |
|
|
const FrameIndex frameInModelA, |
79 |
|
|
const SE3Tpl<Scalar,Options> & aMb, |
80 |
|
|
ModelTpl<Scalar,Options,JointCollectionTpl> & model, |
81 |
|
|
GeometryModel & geomModel); |
82 |
|
|
|
83 |
|
|
/** |
84 |
|
|
* |
85 |
|
|
* \brief Build a reduced model from a given input model and a list of joint to lock. |
86 |
|
|
* |
87 |
|
|
* \param[in] model the input model to reduce. |
88 |
|
|
* \param[in] list_of_joints_to_lock list of joints to lock in the input model. |
89 |
|
|
* \param[in] reference_configuration reference configuration. |
90 |
|
|
* \param[out] reduced_model the reduced model. |
91 |
|
|
* |
92 |
|
|
* \remarks All the joints that have been set to be fixed in the new reduced_model now appear in the kinematic tree as a Frame as FIXED_JOINT. |
93 |
|
|
* |
94 |
|
|
* \todo At the moment, the joint and geometry order is kept while the frames |
95 |
|
|
* are re-ordered in a hard to predict way. Their order could be kept. |
96 |
|
|
* |
97 |
|
|
*/ |
98 |
|
|
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType> |
99 |
|
|
void |
100 |
|
|
buildReducedModel(const ModelTpl<Scalar,Options,JointCollectionTpl> & model, |
101 |
|
|
std::vector<JointIndex> list_of_joints_to_lock, |
102 |
|
|
const Eigen::MatrixBase<ConfigVectorType> & reference_configuration, |
103 |
|
|
ModelTpl<Scalar,Options,JointCollectionTpl> & reduced_model); |
104 |
|
|
|
105 |
|
|
/** |
106 |
|
|
* |
107 |
|
|
* \brief Build a reduced model from a given input model and a list of joint to lock. |
108 |
|
|
* |
109 |
|
|
* \param[in] model the input model to reduce. |
110 |
|
|
* \param[in] list_of_joints_to_lock list of joints to lock in the input model. |
111 |
|
|
* \param[in] reference_configuration reference configuration. |
112 |
|
|
* |
113 |
|
|
* \returns A reduce model of the input model. |
114 |
|
|
* |
115 |
|
|
* \remarks All the joints that have been set to be fixed in the new reduced_model now appear in the kinematic tree as a Frame as FIXED_JOINT. |
116 |
|
|
* |
117 |
|
|
*/ |
118 |
|
|
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType> |
119 |
|
|
ModelTpl<Scalar,Options,JointCollectionTpl> |
120 |
|
8 |
buildReducedModel(const ModelTpl<Scalar,Options,JointCollectionTpl> & model, |
121 |
|
|
const std::vector<JointIndex> & list_of_joints_to_lock, |
122 |
|
|
const Eigen::MatrixBase<ConfigVectorType> & reference_configuration) |
123 |
|
|
{ |
124 |
|
|
typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model; |
125 |
|
8 |
Model reduced_model; |
126 |
|
|
|
127 |
✓✗✓✗
|
8 |
buildReducedModel(model,list_of_joints_to_lock,reference_configuration,reduced_model); |
128 |
|
|
|
129 |
|
8 |
return reduced_model; |
130 |
|
|
} |
131 |
|
|
|
132 |
|
|
/** |
133 |
|
|
* |
134 |
|
|
* \brief Build a reduced model and a rededuced geometry model from a given input model, a given input geometry model and a list of joint to lock. |
135 |
|
|
* |
136 |
|
|
* \param[in] model the input model to reduce. |
137 |
|
|
* \param[in] geom_model the input geometry model to reduce. |
138 |
|
|
* \param[in] list_of_joints_to_lock list of joints to lock in the input model. |
139 |
|
|
* \param[in] reference_configuration reference configuration. |
140 |
|
|
* \param[out] reduced_model the reduced model. |
141 |
|
|
* \param[out] reduced_geom_model the reduced geometry model. |
142 |
|
|
* |
143 |
|
|
* \remarks All the joints that have been set to be fixed in the new reduced_model now appear in the kinematic tree as a Frame as FIXED_JOINT. |
144 |
|
|
* |
145 |
|
|
*/ |
146 |
|
|
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType> |
147 |
|
|
void |
148 |
|
|
buildReducedModel(const ModelTpl<Scalar,Options,JointCollectionTpl> & model, |
149 |
|
|
const GeometryModel & geom_model, |
150 |
|
|
const std::vector<JointIndex> & list_of_joints_to_lock, |
151 |
|
|
const Eigen::MatrixBase<ConfigVectorType> & reference_configuration, |
152 |
|
|
ModelTpl<Scalar,Options,JointCollectionTpl> & reduced_model, |
153 |
|
|
GeometryModel & reduced_geom_model); |
154 |
|
|
|
155 |
|
|
/** |
156 |
|
|
* |
157 |
|
|
* \brief Build a reduced model and a rededuced geometry model from a given input model, a given input geometry model and a list of joint to lock. |
158 |
|
|
* |
159 |
|
|
* \param[in] model the input model to reduce. |
160 |
|
|
* \param[in] list_of_geom_models the input geometry model to reduce (example: visual_model, collision_model). |
161 |
|
|
* \param[in] list_of_joints_to_lock list of joints to lock in the input model. |
162 |
|
|
* \param[in] reference_configuration reference configuration. |
163 |
|
|
* \param[out] reduced_model the reduced model. |
164 |
|
|
* \param[out] list_of_reduced_geom_model the list of reduced geometry models. |
165 |
|
|
* |
166 |
|
|
* \remarks All the joints that have been set to be fixed in the new reduced_model now appear in the kinematic tree as a Frame as FIXED_JOINT. |
167 |
|
|
* |
168 |
|
|
*/ |
169 |
|
|
template <typename Scalar, int Options, |
170 |
|
|
template <typename, int> class JointCollectionTpl, |
171 |
|
|
typename GeometryModelAllocator, |
172 |
|
|
typename ConfigVectorType> |
173 |
|
|
void buildReducedModel( |
174 |
|
|
const ModelTpl<Scalar, Options, JointCollectionTpl> &model, |
175 |
|
|
const std::vector<GeometryModel,GeometryModelAllocator> &list_of_geom_models, |
176 |
|
|
const std::vector<JointIndex> &list_of_joints_to_lock, |
177 |
|
|
const Eigen::MatrixBase<ConfigVectorType> &reference_configuration, |
178 |
|
|
ModelTpl<Scalar, Options, JointCollectionTpl> &reduced_model, |
179 |
|
|
std::vector<GeometryModel,GeometryModelAllocator> &list_of_reduced_geom_models); |
180 |
|
|
|
181 |
|
|
} // namespace pinocchio |
182 |
|
|
|
183 |
|
|
#include "pinocchio/algorithm/model.hxx" |
184 |
|
|
|
185 |
|
|
#endif // ifndef __pinocchio_algorithm_model_hpp__ |