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 |
|
|
*/ |
23 |
|
|
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl> |
24 |
|
|
void |
25 |
|
|
appendModel(const ModelTpl<Scalar,Options,JointCollectionTpl> & modelA, |
26 |
|
|
const ModelTpl<Scalar,Options,JointCollectionTpl> & modelB, |
27 |
|
|
const FrameIndex frameInModelA, |
28 |
|
|
const SE3Tpl<Scalar,Options> & aMb, |
29 |
|
|
ModelTpl<Scalar,Options,JointCollectionTpl> & model); |
30 |
|
|
|
31 |
|
|
/** |
32 |
|
|
* \brief Append a child model into a parent model, after a specific frame given by its index. |
33 |
|
|
* |
34 |
|
|
* \param[in] modelA the parent model. |
35 |
|
|
* \param[in] modelB the child model. |
36 |
|
|
* \param[in] frameInModelA index of the frame of modelA where to append modelB. |
37 |
|
|
* \param[in] aMb pose of modelB universe joint (index 0) in frameInModelA. |
38 |
|
|
* |
39 |
|
|
* \return A new model containing the fusion of modelA and modelB. |
40 |
|
|
* |
41 |
|
|
*/ |
42 |
|
|
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl> |
43 |
|
|
ModelTpl<Scalar,Options,JointCollectionTpl> |
44 |
|
2 |
appendModel(const ModelTpl<Scalar,Options,JointCollectionTpl> & modelA, |
45 |
|
|
const ModelTpl<Scalar,Options,JointCollectionTpl> & modelB, |
46 |
|
|
const FrameIndex frameInModelA, |
47 |
|
|
const SE3Tpl<Scalar,Options> & aMb) |
48 |
|
|
{ |
49 |
|
|
typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model; |
50 |
|
2 |
Model model; |
51 |
|
|
|
52 |
✓✗ |
2 |
appendModel(modelA,modelB,frameInModelA,aMb,model); |
53 |
|
|
|
54 |
|
2 |
return model; |
55 |
|
|
} |
56 |
|
|
|
57 |
|
|
/** |
58 |
|
|
* \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); |
59 |
|
|
* |
60 |
|
|
* \param[in] geomModelA the parent geometry model. |
61 |
|
|
* \param[in] geomModelB the child geometry model. |
62 |
|
|
* \param[out] geomModel the resulting geometry model. |
63 |
|
|
*/ |
64 |
|
|
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl> |
65 |
|
|
void |
66 |
|
|
appendModel(const ModelTpl<Scalar,Options,JointCollectionTpl> & modelA, |
67 |
|
|
const ModelTpl<Scalar,Options,JointCollectionTpl> & modelB, |
68 |
|
|
const GeometryModel & geomModelA, |
69 |
|
|
const GeometryModel & geomModelB, |
70 |
|
|
const FrameIndex frameInModelA, |
71 |
|
|
const SE3Tpl<Scalar,Options> & aMb, |
72 |
|
|
ModelTpl<Scalar,Options,JointCollectionTpl> & model, |
73 |
|
|
GeometryModel & geomModel); |
74 |
|
|
|
75 |
|
|
/** |
76 |
|
|
* |
77 |
|
|
* \brief Build a reduced model from a given input model and a list of joint to lock. |
78 |
|
|
* |
79 |
|
|
* \param[in] model the input model to reduce. |
80 |
|
|
* \param[in] list_of_joints_to_lock list of joints to lock in the input model. |
81 |
|
|
* \param[in] reference_configuration reference configuration. |
82 |
|
|
* \param[out] reduced_model the reduced model. |
83 |
|
|
* |
84 |
|
|
* \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. |
85 |
|
|
* |
86 |
|
|
* \todo At the moment, the joint and geometry order is kept while the frames |
87 |
|
|
* are re-ordered in a hard to predict way. Their order could be kept. |
88 |
|
|
* |
89 |
|
|
*/ |
90 |
|
|
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType> |
91 |
|
|
void |
92 |
|
|
buildReducedModel(const ModelTpl<Scalar,Options,JointCollectionTpl> & model, |
93 |
|
|
std::vector<JointIndex> list_of_joints_to_lock, |
94 |
|
|
const Eigen::MatrixBase<ConfigVectorType> & reference_configuration, |
95 |
|
|
ModelTpl<Scalar,Options,JointCollectionTpl> & reduced_model); |
96 |
|
|
|
97 |
|
|
/** |
98 |
|
|
* |
99 |
|
|
* \brief Build a reduced model from a given input model and a list of joint to lock. |
100 |
|
|
* |
101 |
|
|
* \param[in] model the input model to reduce. |
102 |
|
|
* \param[in] list_of_joints_to_lock list of joints to lock in the input model. |
103 |
|
|
* \param[in] reference_configuration reference configuration. |
104 |
|
|
* |
105 |
|
|
* \returns A reduce model of the input model. |
106 |
|
|
* |
107 |
|
|
* \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. |
108 |
|
|
* |
109 |
|
|
*/ |
110 |
|
|
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType> |
111 |
|
|
ModelTpl<Scalar,Options,JointCollectionTpl> |
112 |
|
8 |
buildReducedModel(const ModelTpl<Scalar,Options,JointCollectionTpl> & model, |
113 |
|
|
const std::vector<JointIndex> & list_of_joints_to_lock, |
114 |
|
|
const Eigen::MatrixBase<ConfigVectorType> & reference_configuration) |
115 |
|
|
{ |
116 |
|
|
typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model; |
117 |
|
8 |
Model reduced_model; |
118 |
|
|
|
119 |
✓✗✓✗
|
8 |
buildReducedModel(model,list_of_joints_to_lock,reference_configuration,reduced_model); |
120 |
|
|
|
121 |
|
8 |
return reduced_model; |
122 |
|
|
} |
123 |
|
|
|
124 |
|
|
/** |
125 |
|
|
* |
126 |
|
|
* \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. |
127 |
|
|
* |
128 |
|
|
* \param[in] model the input model to reduce. |
129 |
|
|
* \param[in] geom_model the input geometry model to reduce. |
130 |
|
|
* \param[in] list_of_joints_to_lock list of joints to lock in the input model. |
131 |
|
|
* \param[in] reference_configuration reference configuration. |
132 |
|
|
* \param[out] reduced_model the reduced model. |
133 |
|
|
* \param[out] reduced_geom_model the reduced geometry model. |
134 |
|
|
* |
135 |
|
|
* \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. |
136 |
|
|
* |
137 |
|
|
*/ |
138 |
|
|
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType> |
139 |
|
|
void |
140 |
|
|
buildReducedModel(const ModelTpl<Scalar,Options,JointCollectionTpl> & model, |
141 |
|
|
const GeometryModel & geom_model, |
142 |
|
|
const std::vector<JointIndex> & list_of_joints_to_lock, |
143 |
|
|
const Eigen::MatrixBase<ConfigVectorType> & reference_configuration, |
144 |
|
|
ModelTpl<Scalar,Options,JointCollectionTpl> & reduced_model, |
145 |
|
|
GeometryModel & reduced_geom_model); |
146 |
|
|
|
147 |
|
|
/** |
148 |
|
|
* |
149 |
|
|
* \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. |
150 |
|
|
* |
151 |
|
|
* \param[in] model the input model to reduce. |
152 |
|
|
* \param[in] list_of_geom_models the input geometry model to reduce (example: visual_model, collision_model). |
153 |
|
|
* \param[in] list_of_joints_to_lock list of joints to lock in the input model. |
154 |
|
|
* \param[in] reference_configuration reference configuration. |
155 |
|
|
* \param[out] reduced_model the reduced model. |
156 |
|
|
* \param[out] list_of_reduced_geom_model the list of reduced geometry models. |
157 |
|
|
* |
158 |
|
|
* \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. |
159 |
|
|
* |
160 |
|
|
*/ |
161 |
|
|
template <typename Scalar, int Options, |
162 |
|
|
template <typename, int> class JointCollectionTpl, |
163 |
|
|
typename GeometryModelAllocator, |
164 |
|
|
typename ConfigVectorType> |
165 |
|
|
void buildReducedModel( |
166 |
|
|
const ModelTpl<Scalar, Options, JointCollectionTpl> &model, |
167 |
|
|
const std::vector<GeometryModel,GeometryModelAllocator> &list_of_geom_models, |
168 |
|
|
const std::vector<JointIndex> &list_of_joints_to_lock, |
169 |
|
|
const Eigen::MatrixBase<ConfigVectorType> &reference_configuration, |
170 |
|
|
ModelTpl<Scalar, Options, JointCollectionTpl> &reduced_model, |
171 |
|
|
std::vector<GeometryModel,GeometryModelAllocator> &list_of_reduced_geom_models); |
172 |
|
|
|
173 |
|
|
} // namespace pinocchio |
174 |
|
|
|
175 |
|
|
#include "pinocchio/algorithm/model.hxx" |
176 |
|
|
|
177 |
|
|
#endif // ifndef __pinocchio_algorithm_model_hpp__ |