pinocchio  3.5.0
A fast and flexible implementation of Rigid Body Dynamics algorithms and their analytical derivatives
model.hpp
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 {
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 
50  template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
51  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  Model model;
59 
60  appendModel(modelA, modelB, frameInModelA, aMb, model);
61 
62  return model;
63  }
64 
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 
104  template<
105  typename Scalar,
106  int Options,
107  template<typename, int> class JointCollectionTpl,
108  typename ConfigVectorType>
109  void buildReducedModel(
110  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
111  std::vector<JointIndex> list_of_joints_to_lock,
112  const Eigen::MatrixBase<ConfigVectorType> & reference_configuration,
113  ModelTpl<Scalar, Options, JointCollectionTpl> & reduced_model);
114 
129  template<
130  typename Scalar,
131  int Options,
132  template<typename, int> class JointCollectionTpl,
133  typename ConfigVectorType>
134  ModelTpl<Scalar, Options, JointCollectionTpl> buildReducedModel(
135  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
136  const std::vector<JointIndex> & list_of_joints_to_lock,
137  const Eigen::MatrixBase<ConfigVectorType> & reference_configuration)
138  {
139  typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model;
140  Model reduced_model;
141 
142  buildReducedModel(model, list_of_joints_to_lock, reference_configuration, reduced_model);
143 
144  return reduced_model;
145  }
146 
163  template<
164  typename Scalar,
165  int Options,
166  template<typename, int> class JointCollectionTpl,
167  typename ConfigVectorType>
168  void buildReducedModel(
169  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
170  const GeometryModel & geom_model,
171  const std::vector<JointIndex> & list_of_joints_to_lock,
172  const Eigen::MatrixBase<ConfigVectorType> & reference_configuration,
173  ModelTpl<Scalar, Options, JointCollectionTpl> & reduced_model,
174  GeometryModel & reduced_geom_model);
175 
192  template<
193  typename Scalar,
194  int Options,
195  template<typename, int> class JointCollectionTpl,
196  typename GeometryModelAllocator,
197  typename ConfigVectorType>
198  void buildReducedModel(
199  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
200  const std::vector<GeometryModel, GeometryModelAllocator> & list_of_geom_models,
201  const std::vector<JointIndex> & list_of_joints_to_lock,
202  const Eigen::MatrixBase<ConfigVectorType> & reference_configuration,
203  ModelTpl<Scalar, Options, JointCollectionTpl> & reduced_model,
204  std::vector<GeometryModel, GeometryModelAllocator> & list_of_reduced_geom_models);
205 
219  template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
220  void transformJointIntoMimic(
221  const ModelTpl<Scalar, Options, JointCollectionTpl> & input_model,
222  const JointIndex & index_mimicked,
223  const JointIndex & index_mimicking,
224  const Scalar & scaling,
225  const Scalar & offset,
226  ModelTpl<Scalar, Options, JointCollectionTpl> & output_model);
227 
240  template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
241  void buildMimicModel(
242  const ModelTpl<Scalar, Options, JointCollectionTpl> & input_model,
243  const std::vector<JointIndex> & index_mimicked,
244  const std::vector<JointIndex> & index_mimicking,
245  const std::vector<Scalar> & scaling,
246  const std::vector<Scalar> & offset,
247  ModelTpl<Scalar, Options, JointCollectionTpl> & output_model);
248 
260  template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
261  JointIndex findCommonAncestor(
262  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
263  JointIndex joint1_id,
264  JointIndex joint2_id,
265  size_t & index_ancestor_in_support1,
266  size_t & index_ancestor_in_support2);
267 
268 } // namespace pinocchio
269 
270 #include "pinocchio/algorithm/model.hxx"
271 
272 #if PINOCCHIO_ENABLE_TEMPLATE_INSTANTIATION
273  #include "pinocchio/algorithm/model.txx"
274 #endif // PINOCCHIO_ENABLE_TEMPLATE_INSTANTIATION
275 
276 #endif // ifndef __pinocchio_algorithm_model_hpp__
pinocchio
Main pinocchio namespace.
Definition: treeview.dox:11
pinocchio::buildMimicModel
void buildMimicModel(const ModelTpl< Scalar, Options, JointCollectionTpl > &input_model, const std::vector< JointIndex > &index_mimicked, const std::vector< JointIndex > &index_mimicking, const std::vector< Scalar > &scaling, const std::vector< Scalar > &offset, ModelTpl< Scalar, Options, JointCollectionTpl > &output_model)
Transform joints of a model into mimic joints.
pinocchio::appendModel
void appendModel(const ModelTpl< Scalar, Options, JointCollectionTpl > &modelA, const ModelTpl< Scalar, Options, JointCollectionTpl > &modelB, const FrameIndex frameInModelA, const SE3Tpl< Scalar, Options > &aMb, ModelTpl< Scalar, Options, JointCollectionTpl > &model)
Append a child model into a parent model, after a specific frame given by its index.
pinocchio::transformJointIntoMimic
void transformJointIntoMimic(const ModelTpl< Scalar, Options, JointCollectionTpl > &input_model, const JointIndex &index_mimicked, const JointIndex &index_mimicking, const Scalar &scaling, const Scalar &offset, ModelTpl< Scalar, Options, JointCollectionTpl > &output_model)
Transform of a joint of the model into a mimic joint. Keep the type of the joint as it was previously...
pinocchio::buildReducedModel
void buildReducedModel(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, std::vector< JointIndex > list_of_joints_to_lock, const Eigen::MatrixBase< ConfigVectorType > &reference_configuration, ModelTpl< Scalar, Options, JointCollectionTpl > &reduced_model)
Build a reduced model from a given input model and a list of joint to lock.
pinocchio::findCommonAncestor
JointIndex findCommonAncestor(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, JointIndex joint1_id, JointIndex joint2_id, size_t &index_ancestor_in_support1, size_t &index_ancestor_in_support2)
Computes the common ancestor between two joints belonging to the same kinematic tree.