$darkmode
pinocchio 4.0.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 #pragma once
5 
6 // IWYU pragma: begin_keep
7 #include <Eigen/Core>
8 //
9 #include <cassert>
10 #include <cstddef>
11 #include <stdexcept>
12 #include <algorithm>
13 #include <type_traits>
14 #include <utility>
15 #include <iterator>
16 #include <string>
17 #include <vector>
18 
19 #include <boost/fusion/container/vector.hpp>
20 #include <boost/variant/get.hpp>
21 
22 #include "pinocchio/macros.hpp"
23 #include "pinocchio/fwd.hpp"
24 
25 #include "pinocchio/spatial.hpp"
26 
27 #include "pinocchio/multibody.hpp"
28 #include "pinocchio/geometry.hpp"
29 #include "pinocchio/multibody/joint.hpp"
30 
31 #include "pinocchio/algorithm/joint-configuration.hpp"
32 // IWYU pragma: end_keep
33 
34 namespace pinocchio
35 {
50  template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
51  void 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  ModelTpl<Scalar, Options, JointCollectionTpl> & model);
57 
73  template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
74  ModelTpl<Scalar, Options, JointCollectionTpl> appendModel(
75  const ModelTpl<Scalar, Options, JointCollectionTpl> & modelA,
76  const ModelTpl<Scalar, Options, JointCollectionTpl> & modelB,
77  const FrameIndex frameInModelA,
78  const SE3Tpl<Scalar, Options> & aMb)
79  {
80  typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model;
81  Model model;
82 
83  appendModel(modelA, modelB, frameInModelA, aMb, model);
84 
85  return model;
86  }
87 
100  template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
101  void appendModel(
102  const ModelTpl<Scalar, Options, JointCollectionTpl> & modelA,
103  const ModelTpl<Scalar, Options, JointCollectionTpl> & modelB,
104  const GeometryModel & geomModelA,
105  const GeometryModel & geomModelB,
106  const FrameIndex frameInModelA,
107  const SE3Tpl<Scalar, Options> & aMb,
108  ModelTpl<Scalar, Options, JointCollectionTpl> & model,
109  GeometryModel & geomModel);
110 
127  template<
128  typename Scalar,
129  int Options,
130  template<typename, int> class JointCollectionTpl,
131  typename ConfigVectorType>
132  void buildReducedModel(
133  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
134  std::vector<JointIndex> list_of_joints_to_lock,
135  const Eigen::MatrixBase<ConfigVectorType> & reference_configuration,
136  ModelTpl<Scalar, Options, JointCollectionTpl> & reduced_model);
137 
152  template<
153  typename Scalar,
154  int Options,
155  template<typename, int> class JointCollectionTpl,
156  typename ConfigVectorType>
157  ModelTpl<Scalar, Options, JointCollectionTpl> buildReducedModel(
158  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
159  const std::vector<JointIndex> & list_of_joints_to_lock,
160  const Eigen::MatrixBase<ConfigVectorType> & reference_configuration)
161  {
162  typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model;
163  Model reduced_model;
164 
165  buildReducedModel(model, list_of_joints_to_lock, reference_configuration, reduced_model);
166 
167  return reduced_model;
168  }
169 
186  template<
187  typename Scalar,
188  int Options,
189  template<typename, int> class JointCollectionTpl,
190  typename ConfigVectorType>
191  void buildReducedModel(
192  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
193  const GeometryModel & geom_model,
194  const std::vector<JointIndex> & list_of_joints_to_lock,
195  const Eigen::MatrixBase<ConfigVectorType> & reference_configuration,
196  ModelTpl<Scalar, Options, JointCollectionTpl> & reduced_model,
197  GeometryModel & reduced_geom_model);
198 
215  template<
216  typename Scalar,
217  int Options,
218  template<typename, int> class JointCollectionTpl,
219  typename ConfigVectorType>
220  void buildReducedModel(
221  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
222  const std::vector<GeometryModel> & list_of_geom_models,
223  const std::vector<JointIndex> & list_of_joints_to_lock,
224  const Eigen::MatrixBase<ConfigVectorType> & reference_configuration,
225  ModelTpl<Scalar, Options, JointCollectionTpl> & reduced_model,
226  std::vector<GeometryModel> & list_of_reduced_geom_models);
227 
241  template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
242  void transformJointIntoMimic(
243  const ModelTpl<Scalar, Options, JointCollectionTpl> & input_model,
244  const JointIndex & index_mimicked,
245  const JointIndex & index_mimicking,
246  const Scalar & scaling,
247  const Scalar & offset,
248  ModelTpl<Scalar, Options, JointCollectionTpl> & output_model);
249 
262  template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
263  void buildMimicModel(
264  const ModelTpl<Scalar, Options, JointCollectionTpl> & input_model,
265  const std::vector<JointIndex> & index_mimicked,
266  const std::vector<JointIndex> & index_mimicking,
267  const std::vector<Scalar> & scaling,
268  const std::vector<Scalar> & offset,
269  ModelTpl<Scalar, Options, JointCollectionTpl> & output_model);
270 
282  template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
283  JointIndex findCommonAncestor(
284  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
285  JointIndex joint1_id,
286  JointIndex joint2_id,
287  size_t & index_ancestor_in_support1,
288  size_t & index_ancestor_in_support2);
289 
299  template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
300  JointIndex findCommonAncestor(
301  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
302  JointIndex joint1_id,
303  JointIndex joint2_id)
304  {
305  size_t index_ancestor_in_support1, index_ancestor_in_support2;
306  return findCommonAncestor(
307  model, joint1_id, joint2_id, index_ancestor_in_support1, index_ancestor_in_support2);
308  }
309 
310 } // namespace pinocchio
311 
312 // IWYU pragma: begin_exports
313 #include "pinocchio/src/algorithm/model.hxx"
314 // IWYU pragma: end_exports
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.