$darkmode
pinocchio 4.0.0
A fast and flexible implementation of Rigid Body Dynamics algorithms and their analytical derivatives
frames.hpp
1 //
2 // Copyright (c) 2015-2020 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 <iterator>
13 #include <vector>
14 
15 #include "pinocchio/macros.hpp"
16 #include "pinocchio/eigen-common.hpp"
17 
18 #include "pinocchio/math.hpp"
19 
20 #include "pinocchio/spatial.hpp"
21 
22 #include "pinocchio/multibody.hpp"
23 #include "pinocchio/multibody/visitor.hpp"
24 
25 #include "pinocchio/algorithm/check.hpp"
26 #include "pinocchio/algorithm/jacobian.hpp"
27 #include "pinocchio/algorithm/kinematics.hpp"
28 
29 // IWYU pragma: end_keep
30 
31 namespace pinocchio
32 {
33 
44  template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
45  inline void updateFramePlacements(
46  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
47  DataTpl<Scalar, Options, JointCollectionTpl> & data);
48 
60  template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
61  inline const typename DataTpl<Scalar, Options, JointCollectionTpl>::SE3 & updateFramePlacement(
62  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
63  DataTpl<Scalar, Options, JointCollectionTpl> & data,
64  const FrameIndex frame_id);
65 
77  template<
78  typename Scalar,
79  int Options,
80  template<typename, int> class JointCollectionTpl,
81  typename ConfigVectorType>
82  inline void framesForwardKinematics(
83  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
84  DataTpl<Scalar, Options, JointCollectionTpl> & data,
85  const Eigen::MatrixBase<ConfigVectorType> & q);
86 
103  template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
104  inline MotionTpl<Scalar, Options> getFrameVelocity(
105  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
106  const DataTpl<Scalar, Options, JointCollectionTpl> & data,
107  const JointIndex joint_id,
108  const SE3Tpl<Scalar, Options> & placement,
109  const ReferenceFrame rf = LOCAL);
110 
126  template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
127  inline MotionTpl<Scalar, Options> getFrameVelocity(
128  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
129  const DataTpl<Scalar, Options, JointCollectionTpl> & data,
130  const FrameIndex frame_id,
131  const ReferenceFrame rf = LOCAL)
132  {
133  typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model;
134  const typename Model::Frame & frame = model.frames[frame_id];
135 
136  return getFrameVelocity(model, data, frame.parentJoint, frame.placement, rf);
137  }
138 
155  template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
156  inline MotionTpl<Scalar, Options> getFrameAcceleration(
157  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
158  const DataTpl<Scalar, Options, JointCollectionTpl> & data,
159  const JointIndex joint_id,
160  const SE3Tpl<Scalar, Options> & placement,
161  const ReferenceFrame rf = LOCAL);
162 
184  template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
185  inline MotionTpl<Scalar, Options> getFrameAcceleration(
186  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
187  const DataTpl<Scalar, Options, JointCollectionTpl> & data,
188  const FrameIndex frame_id,
189  const ReferenceFrame rf = LOCAL)
190  {
191  typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model;
192  const typename Model::Frame & frame = model.frames[frame_id];
193  return getFrameAcceleration(model, data, frame.parentJoint, frame.placement, rf);
194  }
195 
213  template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
214  inline MotionTpl<Scalar, Options> getFrameClassicalAcceleration(
215  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
216  const DataTpl<Scalar, Options, JointCollectionTpl> & data,
217  const JointIndex joint_id,
218  const SE3Tpl<Scalar, Options> & placement,
219  const ReferenceFrame rf = LOCAL);
220 
244  template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
245  inline MotionTpl<Scalar, Options> getFrameClassicalAcceleration(
246  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
247  const DataTpl<Scalar, Options, JointCollectionTpl> & data,
248  const FrameIndex frame_id,
249  const ReferenceFrame rf = LOCAL)
250  {
251  typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model;
252  const typename Model::Frame & frame = model.frames[frame_id];
253  return getFrameClassicalAcceleration(model, data, frame.parentJoint, frame.placement, rf);
254  }
255 
285  template<
286  typename Scalar,
287  int Options,
288  template<typename, int> class JointCollectionTpl,
289  typename Matrix6xLike>
290  void getFrameJacobian(
291  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
292  DataTpl<Scalar, Options, JointCollectionTpl> & data,
293  const JointIndex joint_id,
294  const SE3Tpl<Scalar, Options> & placement,
295  const ReferenceFrame reference_frame,
296  const Eigen::MatrixBase<Matrix6xLike> & J);
297 
323  template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
324  Eigen::Matrix<Scalar, 6, Eigen::Dynamic, Options> getFrameJacobian(
325  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
326  DataTpl<Scalar, Options, JointCollectionTpl> & data,
327  const JointIndex joint_id,
328  const SE3Tpl<Scalar, Options> & placement,
329  const ReferenceFrame reference_frame)
330  {
331  typedef Eigen::Matrix<Scalar, 6, Eigen::Dynamic, Options> Matrix6x;
332  Matrix6x res(Matrix6x::Zero(6, model.nv));
333 
334  getFrameJacobian(model, data, joint_id, placement, reference_frame, res);
335  return res;
336  }
337 
365  template<
366  typename Scalar,
367  int Options,
368  template<typename, int> class JointCollectionTpl,
369  typename Matrix6xLike>
370  inline void getFrameJacobian(
371  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
372  DataTpl<Scalar, Options, JointCollectionTpl> & data,
373  const FrameIndex frame_id,
374  const ReferenceFrame reference_frame,
375  const Eigen::MatrixBase<Matrix6xLike> & J)
376  {
377  PINOCCHIO_CHECK_INPUT_ARGUMENT(
378  frame_id < (FrameIndex)model.nframes, "The index of the Frame is outside the bounds.");
379 
380  typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model;
381  typedef typename Model::Frame Frame;
382 
383  const Frame & frame = model.frames[frame_id];
384  data.oMf[frame_id] = data.oMi[frame.parentJoint] * frame.placement;
385 
386  getFrameJacobian(
387  model, data, frame.parentJoint, frame.placement, reference_frame,
388  PINOCCHIO_EIGEN_CONST_CAST(Matrix6xLike, J));
389  }
390 
415  template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
416  Eigen::Matrix<Scalar, 6, Eigen::Dynamic, Options> getFrameJacobian(
417  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
418  DataTpl<Scalar, Options, JointCollectionTpl> & data,
419  const FrameIndex frame_id,
420  const ReferenceFrame reference_frame)
421  {
422  typedef Eigen::Matrix<Scalar, 6, Eigen::Dynamic, Options> Matrix6x;
423  Matrix6x res(Matrix6x::Zero(6, model.nv));
424 
425  getFrameJacobian(model, data, frame_id, reference_frame, res);
426  return res;
427  }
428 
450  template<
451  typename Scalar,
452  int Options,
453  template<typename, int> class JointCollectionTpl,
454  typename ConfigVectorType,
455  typename Matrix6xLike>
456  inline void computeFrameJacobian(
457  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
458  DataTpl<Scalar, Options, JointCollectionTpl> & data,
459  const Eigen::MatrixBase<ConfigVectorType> & q,
460  const FrameIndex frame_id,
461  const ReferenceFrame reference_frame,
462  const Eigen::MatrixBase<Matrix6xLike> & J);
463 
485  template<
486  typename Scalar,
487  int Options,
488  template<typename, int> class JointCollectionTpl,
489  typename ConfigVectorType,
490  typename Matrix6xLike>
491  inline void computeFrameJacobian(
492  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
493  DataTpl<Scalar, Options, JointCollectionTpl> & data,
494  const Eigen::MatrixBase<ConfigVectorType> & q,
495  const FrameIndex frame_id,
496  const Eigen::MatrixBase<Matrix6xLike> & J)
497  {
498  computeFrameJacobian(
499  model, data, q.derived(), frame_id, LOCAL, PINOCCHIO_EIGEN_CONST_CAST(Matrix6xLike, J));
500  }
501 
521  template<
522  typename Scalar,
523  int Options,
524  template<typename, int> class JointCollectionTpl,
525  typename Matrix6xLike>
526  void getFrameJacobianTimeVariation(
527  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
528  DataTpl<Scalar, Options, JointCollectionTpl> & data,
529  const FrameIndex frame_id,
530  const ReferenceFrame rf,
531  const Eigen::MatrixBase<Matrix6xLike> & dJ);
532 
566  template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
567  InertiaTpl<Scalar, Options> computeSupportedInertiaByFrame(
568  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
569  const DataTpl<Scalar, Options, JointCollectionTpl> & data,
570  const FrameIndex frame_id,
571  bool with_subtree);
572 
606  template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
607  ForceTpl<Scalar, Options> computeSupportedForceByFrame(
608  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
609  const DataTpl<Scalar, Options, JointCollectionTpl> & data,
610  const FrameIndex frame_id);
611 
612 } // namespace pinocchio
613 // IWYU pragma: begin_exports
614 #include "pinocchio/src/algorithm/frames.hxx"
615 // IWYU pragma: end_exports
pinocchio
Main pinocchio namespace.
Definition: treeview.dox:11
pinocchio::getFrameClassicalAcceleration
MotionTpl< Scalar, Options > getFrameClassicalAcceleration(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const DataTpl< Scalar, Options, JointCollectionTpl > &data, const JointIndex joint_id, const SE3Tpl< Scalar, Options > &placement, const ReferenceFrame rf=LOCAL)
Returns the "classical" acceleration of the Frame expressed in the desired reference frame....
pinocchio::getFrameJacobian
void getFrameJacobian(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, const JointIndex joint_id, const SE3Tpl< Scalar, Options > &placement, const ReferenceFrame reference_frame, const Eigen::MatrixBase< Matrix6xLike > &J)
Returns the jacobian of the frame given by its relative placement w.r.t. a joint frame,...
pinocchio::getFrameVelocity
MotionTpl< Scalar, Options > getFrameVelocity(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const DataTpl< Scalar, Options, JointCollectionTpl > &data, const JointIndex joint_id, const SE3Tpl< Scalar, Options > &placement, const ReferenceFrame rf=LOCAL)
Returns the spatial velocity of the Frame expressed in the desired reference frame....
pinocchio::updateFramePlacements
void updateFramePlacements(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data)
Updates the position of each frame contained in the model.
pinocchio::updateFramePlacement
const DataTpl< Scalar, Options, JointCollectionTpl >::SE3 & updateFramePlacement(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, const FrameIndex frame_id)
Updates the placement of the given frame.
pinocchio::getFrameAcceleration
MotionTpl< Scalar, Options > getFrameAcceleration(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const DataTpl< Scalar, Options, JointCollectionTpl > &data, const JointIndex joint_id, const SE3Tpl< Scalar, Options > &placement, const ReferenceFrame rf=LOCAL)
Returns the spatial acceleration of the Frame expressed in the desired reference frame....
pinocchio::computeFrameJacobian
void computeFrameJacobian(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, const Eigen::MatrixBase< ConfigVectorType > &q, const FrameIndex frame_id, const ReferenceFrame reference_frame, const Eigen::MatrixBase< Matrix6xLike > &J)
Computes the Jacobian of a specific Frame expressed in the desired reference_frame given as argument.
pinocchio::framesForwardKinematics
void framesForwardKinematics(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, const Eigen::MatrixBase< ConfigVectorType > &q)
First calls the forwardKinematics on the model, then computes the placement of each frame....
pinocchio::getFrameJacobianTimeVariation
void getFrameJacobianTimeVariation(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, const FrameIndex frame_id, const ReferenceFrame rf, const Eigen::MatrixBase< Matrix6xLike > &dJ)
Computes the Jacobian time variation of a specific frame (given by frame_id) expressed either in the ...
pinocchio::computeSupportedInertiaByFrame
InertiaTpl< Scalar, Options > computeSupportedInertiaByFrame(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const DataTpl< Scalar, Options, JointCollectionTpl > &data, const FrameIndex frame_id, bool with_subtree)
Compute the inertia supported by a specific frame (given by frame_id) expressed in the LOCAL frame....
pinocchio::computeSupportedForceByFrame
ForceTpl< Scalar, Options > computeSupportedForceByFrame(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const DataTpl< Scalar, Options, JointCollectionTpl > &data, const FrameIndex frame_id)
Computes the force supported by a specific frame (given by frame_id) expressed in the LOCAL frame....