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 
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::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:192
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....
pinocchio::python::Scalar
context::Scalar Scalar
Definition: admm-solver.cpp:29
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::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::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.
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::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
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:27
pinocchio::LOCAL
@ LOCAL
Definition: multibody/fwd.hpp:50

pinocchio
Author(s):
autogenerated on Thu Dec 19 2024 03:41:29