frames.hpp
Go to the documentation of this file.
1 //
2 // Copyright (c) 2015-2020 CNRS INRIA
3 //
4 
5 #ifndef __pinocchio_algorithm_frames_hpp__
6 #define __pinocchio_algorithm_frames_hpp__
7 
8 #include "pinocchio/multibody/model.hpp"
9 #include "pinocchio/multibody/data.hpp"
10 
11 namespace pinocchio
12 {
13 
24  template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
25  inline void updateFramePlacements(
26  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
27  DataTpl<Scalar, Options, JointCollectionTpl> & data);
28 
40  template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
41  inline const typename DataTpl<Scalar, Options, JointCollectionTpl>::SE3 & updateFramePlacement(
42  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
43  DataTpl<Scalar, Options, JointCollectionTpl> & data,
44  const FrameIndex frame_id);
45 
57  template<
58  typename Scalar,
59  int Options,
60  template<typename, int> class JointCollectionTpl,
61  typename ConfigVectorType>
62  inline void framesForwardKinematics(
63  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
64  DataTpl<Scalar, Options, JointCollectionTpl> & data,
65  const Eigen::MatrixBase<ConfigVectorType> & q);
66 
83  template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
84  inline MotionTpl<Scalar, Options> getFrameVelocity(
85  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
86  const DataTpl<Scalar, Options, JointCollectionTpl> & data,
87  const JointIndex joint_id,
88  const SE3Tpl<Scalar, Options> & placement,
89  const ReferenceFrame rf = LOCAL);
90 
106  template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
107  inline MotionTpl<Scalar, Options> getFrameVelocity(
108  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
109  const DataTpl<Scalar, Options, JointCollectionTpl> & data,
110  const FrameIndex frame_id,
111  const ReferenceFrame rf = LOCAL)
112  {
113  typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model;
114  const typename Model::Frame & frame = model.frames[frame_id];
115 
116  return getFrameVelocity(model, data, frame.parentJoint, frame.placement, rf);
117  }
118 
135  template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
136  inline MotionTpl<Scalar, Options> getFrameAcceleration(
137  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
138  const DataTpl<Scalar, Options, JointCollectionTpl> & data,
139  const JointIndex joint_id,
140  const SE3Tpl<Scalar, Options> & placement,
141  const ReferenceFrame rf = LOCAL);
142 
164  template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
165  inline MotionTpl<Scalar, Options> getFrameAcceleration(
166  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
167  const DataTpl<Scalar, Options, JointCollectionTpl> & data,
168  const FrameIndex frame_id,
169  const ReferenceFrame rf = LOCAL)
170  {
171  typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model;
172  const typename Model::Frame & frame = model.frames[frame_id];
173  return getFrameAcceleration(model, data, frame.parentJoint, frame.placement, rf);
174  }
175 
193  template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
194  inline MotionTpl<Scalar, Options> getFrameClassicalAcceleration(
195  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
196  const DataTpl<Scalar, Options, JointCollectionTpl> & data,
197  const JointIndex joint_id,
198  const SE3Tpl<Scalar, Options> & placement,
199  const ReferenceFrame rf = LOCAL);
200 
224  template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
225  inline MotionTpl<Scalar, Options> getFrameClassicalAcceleration(
226  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
227  const DataTpl<Scalar, Options, JointCollectionTpl> & data,
228  const FrameIndex frame_id,
229  const ReferenceFrame rf = LOCAL)
230  {
231  typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model;
232  const typename Model::Frame & frame = model.frames[frame_id];
233  return getFrameClassicalAcceleration(model, data, frame.parentJoint, frame.placement, rf);
234  }
235 
264  template<
265  typename Scalar,
266  int Options,
267  template<typename, int> class JointCollectionTpl,
268  typename Matrix6xLike>
269  void getFrameJacobian(
270  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
271  DataTpl<Scalar, Options, JointCollectionTpl> & data,
272  const JointIndex joint_id,
273  const SE3Tpl<Scalar, Options> & placement,
274  const ReferenceFrame reference_frame,
275  const Eigen::MatrixBase<Matrix6xLike> & J);
276 
301  template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
302  Eigen::Matrix<Scalar, 6, Eigen::Dynamic, Options> getFrameJacobian(
303  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
304  DataTpl<Scalar, Options, JointCollectionTpl> & data,
305  const JointIndex joint_id,
306  const SE3Tpl<Scalar, Options> & placement,
307  const ReferenceFrame reference_frame)
308  {
309  typedef Eigen::Matrix<Scalar, 6, Eigen::Dynamic, Options> Matrix6x;
310  Matrix6x res(Matrix6x::Zero(6, model.nv));
311 
312  getFrameJacobian(model, data, joint_id, placement, reference_frame, res);
313  return res;
314  }
315 
343  template<
344  typename Scalar,
345  int Options,
346  template<typename, int> class JointCollectionTpl,
347  typename Matrix6xLike>
348  inline void getFrameJacobian(
349  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
350  DataTpl<Scalar, Options, JointCollectionTpl> & data,
351  const FrameIndex frame_id,
352  const ReferenceFrame reference_frame,
353  const Eigen::MatrixBase<Matrix6xLike> & J)
354  {
355  PINOCCHIO_CHECK_INPUT_ARGUMENT(
356  frame_id < (FrameIndex)model.nframes, "The index of the Frame is outside the bounds.");
357 
358  typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model;
359  typedef typename Model::Frame Frame;
360 
361  const Frame & frame = model.frames[frame_id];
362  data.oMf[frame_id] = data.oMi[frame.parentJoint] * frame.placement;
363 
364  getFrameJacobian(
365  model, data, frame.parentJoint, frame.placement, reference_frame,
366  PINOCCHIO_EIGEN_CONST_CAST(Matrix6xLike, J));
367  }
368 
393  template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
394  Eigen::Matrix<Scalar, 6, Eigen::Dynamic, Options> getFrameJacobian(
395  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
396  DataTpl<Scalar, Options, JointCollectionTpl> & data,
397  const FrameIndex frame_id,
398  const ReferenceFrame reference_frame)
399  {
400  typedef Eigen::Matrix<Scalar, 6, Eigen::Dynamic, Options> Matrix6x;
401  Matrix6x res(Matrix6x::Zero(6, model.nv));
402 
403  getFrameJacobian(model, data, frame_id, reference_frame, res);
404  return res;
405  }
406 
431  template<
432  typename Scalar,
433  int Options,
434  template<typename, int> class JointCollectionTpl,
435  typename ConfigVectorType,
436  typename Matrix6xLike>
437  inline void computeFrameJacobian(
438  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
439  DataTpl<Scalar, Options, JointCollectionTpl> & data,
440  const Eigen::MatrixBase<ConfigVectorType> & q,
441  const FrameIndex frame_id,
442  const ReferenceFrame reference_frame,
443  const Eigen::MatrixBase<Matrix6xLike> & J);
444 
469  template<
470  typename Scalar,
471  int Options,
472  template<typename, int> class JointCollectionTpl,
473  typename ConfigVectorType,
474  typename Matrix6xLike>
475  inline void computeFrameJacobian(
476  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
477  DataTpl<Scalar, Options, JointCollectionTpl> & data,
478  const Eigen::MatrixBase<ConfigVectorType> & q,
479  const FrameIndex frame_id,
480  const Eigen::MatrixBase<Matrix6xLike> & J)
481  {
482  computeFrameJacobian(
483  model, data, q.derived(), frame_id, LOCAL, PINOCCHIO_EIGEN_CONST_CAST(Matrix6xLike, J));
484  }
485 
504  template<
505  typename Scalar,
506  int Options,
507  template<typename, int> class JointCollectionTpl,
508  typename Matrix6xLike>
509  void getFrameJacobianTimeVariation(
510  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
511  DataTpl<Scalar, Options, JointCollectionTpl> & data,
512  const FrameIndex frame_id,
513  const ReferenceFrame rf,
514  const Eigen::MatrixBase<Matrix6xLike> & dJ);
515 
549  template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
550  InertiaTpl<Scalar, Options> computeSupportedInertiaByFrame(
551  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
552  const DataTpl<Scalar, Options, JointCollectionTpl> & data,
553  const FrameIndex frame_id,
554  bool with_subtree);
555 
589  template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
590  ForceTpl<Scalar, Options> computeSupportedForceByFrame(
591  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
592  const DataTpl<Scalar, Options, JointCollectionTpl> & data,
593  const FrameIndex frame_id);
594 
595 } // namespace pinocchio
596 
597 /* --- Details -------------------------------------------------------------------- */
598 #include "pinocchio/algorithm/frames.hxx"
599 
600 #if PINOCCHIO_ENABLE_TEMPLATE_INSTANTIATION
601  #include "pinocchio/algorithm/frames.txx"
602 #endif // PINOCCHIO_ENABLE_TEMPLATE_INSTANTIATION
603 
604 #endif // ifndef __pinocchio_algorithm_frames_hpp__
pinocchio::FrameIndex
Index FrameIndex
Definition: multibody/fwd.hpp:28
pinocchio::DataTpl
Definition: context/generic.hpp:25
PINOCCHIO_CHECK_INPUT_ARGUMENT
#define PINOCCHIO_CHECK_INPUT_ARGUMENT(...)
Macro to check an assert-like condition and throw a std::invalid_argument exception (with a message) ...
Definition: include/pinocchio/macros.hpp:196
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::Options
Options
Definition: joint-configuration.hpp:1082
pinocchio::ReferenceFrame
ReferenceFrame
Various conventions to express the velocity of a moving frame.
Definition: multibody/fwd.hpp:46
PINOCCHIO_EIGEN_CONST_CAST
#define PINOCCHIO_EIGEN_CONST_CAST(TYPE, OBJ)
Macro for an automatic const_cast.
Definition: eigen-macros.hpp:51
pinocchio::SE3Tpl< Scalar, Options >
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....
model.hpp
setup.data
data
Definition: cmake/cython/setup.in.py:48
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....
inverse-kinematics-3d.J
J
Definition: inverse-kinematics-3d.py:28
pinocchio::res
ReturnType res
Definition: spatial/classic-acceleration.hpp:57
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....
pinocchio::FrameTpl
A Plucker coordinate frame attached to a parent joint inside a kinematic tree.
Definition: multibody/frame.hpp:55
pinocchio::placement
const MotionDense< Motion2 > const SE3Tpl< SE3Scalar, SE3Options > & placement
Definition: spatial/classic-acceleration.hpp:122
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.
ConfigVectorType
Model::ConfigVectorType ConfigVectorType
Definition: timings-cppad-jit.cpp:57
data.hpp
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....
contact-cholesky.frame
frame
Definition: contact-cholesky.py:24
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::Frame
FrameTpl< context::Scalar, context::Options > Frame
Definition: multibody/fwd.hpp:31
pinocchio::q
JointCollectionTpl const Eigen::MatrixBase< ConfigVectorType > & q
Definition: joint-configuration.hpp:1083
contact-cholesky.frame_id
frame_id
Definition: contact-cholesky.py:19
Matrix6x
Eigen::Matrix< double, 6, Eigen::Dynamic > Matrix6x
Definition: joint-mimic.cpp:15
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::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::JointIndex
Index JointIndex
Definition: multibody/fwd.hpp:26
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::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::MotionTpl< Scalar, Options >
append-urdf-model-with-another-model.joint_id
joint_id
Definition: append-urdf-model-with-another-model.py:34
Scalar
double Scalar
Definition: timings-cppad-jit.cpp:37
pinocchio::ModelTpl
Definition: context/generic.hpp:20
pinocchio::DataTpl::SE3
SE3Tpl< Scalar, Options > SE3
Definition: multibody/data.hpp:56
pinocchio::Model
ModelTpl< context::Scalar, context::Options > Model
Definition: multibody/fwd.hpp:33
pinocchio::model
JointCollectionTpl & model
Definition: joint-configuration.hpp:1082
pinocchio
Main pinocchio namespace.
Definition: timings.cpp:33
pinocchio::LOCAL
@ LOCAL
Definition: multibody/fwd.hpp:50

pinocchio
Author(s):
autogenerated on Wed May 28 2025 02:41:18