pinocchio  3.3.1
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 
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 
432  template<
433  typename Scalar,
434  int Options,
435  template<typename, int> class JointCollectionTpl,
436  typename ConfigVectorType,
437  typename Matrix6xLike>
438  inline void computeFrameJacobian(
439  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
440  DataTpl<Scalar, Options, JointCollectionTpl> & data,
441  const Eigen::MatrixBase<ConfigVectorType> & q,
442  const FrameIndex frameId,
443  const ReferenceFrame reference_frame,
444  const Eigen::MatrixBase<Matrix6xLike> & J);
445 
470  template<
471  typename Scalar,
472  int Options,
473  template<typename, int> class JointCollectionTpl,
474  typename ConfigVectorType,
475  typename Matrix6xLike>
476  inline void computeFrameJacobian(
477  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
478  DataTpl<Scalar, Options, JointCollectionTpl> & data,
479  const Eigen::MatrixBase<ConfigVectorType> & q,
480  const FrameIndex frameId,
481  const Eigen::MatrixBase<Matrix6xLike> & J)
482  {
483  computeFrameJacobian(
484  model, data, q.derived(), frameId, LOCAL, PINOCCHIO_EIGEN_CONST_CAST(Matrix6xLike, J));
485  }
486 
505  template<
506  typename Scalar,
507  int Options,
508  template<typename, int> class JointCollectionTpl,
509  typename Matrix6xLike>
510  void getFrameJacobianTimeVariation(
511  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
512  DataTpl<Scalar, Options, JointCollectionTpl> & data,
513  const FrameIndex frame_id,
514  const ReferenceFrame rf,
515  const Eigen::MatrixBase<Matrix6xLike> & dJ);
516 
550  template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
551  InertiaTpl<Scalar, Options> computeSupportedInertiaByFrame(
552  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
553  const DataTpl<Scalar, Options, JointCollectionTpl> & data,
554  const FrameIndex frame_id,
555  bool with_subtree);
556 
590  template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
591  ForceTpl<Scalar, Options> computeSupportedForceByFrame(
592  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
593  const DataTpl<Scalar, Options, JointCollectionTpl> & data,
594  const FrameIndex frame_id);
595 
596 } // namespace pinocchio
597 
598 /* --- Details -------------------------------------------------------------------- */
599 #include "pinocchio/algorithm/frames.hxx"
600 
601 #if PINOCCHIO_ENABLE_TEMPLATE_INSTANTIATION
602  #include "pinocchio/algorithm/frames.txx"
603 #endif // PINOCCHIO_ENABLE_TEMPLATE_INSTANTIATION
604 
605 #endif // ifndef __pinocchio_algorithm_frames_hpp__
pinocchio::ReferenceFrame
ReferenceFrame
Various conventions to express the velocity of a moving frame.
Definition: fwd.hpp:47
pinocchio::LOCAL
@ LOCAL
Definition: fwd.hpp:50
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 frameId, 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....
pinocchio::FrameTpl
A Plucker coordinate frame attached to a parent joint inside a kinematic tree.
Definition: frame.hpp:56
pinocchio::FrameTpl::parentJoint
JointIndex parentJoint
Index of the parent joint.
Definition: model-item.hpp:28
pinocchio::FrameTpl::placement
SE3 placement
Position of kinematic element in parent joint frame.
Definition: model-item.hpp:39