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>
61  class JointCollectionTpl,
62  typename ConfigVectorType>
63  inline void framesForwardKinematics(
64  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
65  DataTpl<Scalar, Options, JointCollectionTpl> & data,
66  const Eigen::MatrixBase<ConfigVectorType> & q);
67 
84  template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
85  inline MotionTpl<Scalar, Options> getFrameVelocity(
86  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
87  const DataTpl<Scalar, Options, JointCollectionTpl> & data,
88  const JointIndex joint_id,
89  const SE3Tpl<Scalar, Options> & placement,
90  const ReferenceFrame rf = LOCAL);
91 
107  template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
108  inline MotionTpl<Scalar, Options> getFrameVelocity(
109  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
110  const DataTpl<Scalar, Options, JointCollectionTpl> & data,
111  const FrameIndex frame_id,
112  const ReferenceFrame rf = LOCAL)
113  {
114  typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model;
115  const typename Model::Frame & frame = model.frames[frame_id];
116 
117  return getFrameVelocity(model, data, frame.parentJoint, frame.placement, rf);
118  }
119 
136  template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
137  inline MotionTpl<Scalar, Options> getFrameAcceleration(
138  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
139  const DataTpl<Scalar, Options, JointCollectionTpl> & data,
140  const JointIndex joint_id,
141  const SE3Tpl<Scalar, Options> & placement,
142  const ReferenceFrame rf = LOCAL);
143 
165  template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
166  inline MotionTpl<Scalar, Options> getFrameAcceleration(
167  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
168  const DataTpl<Scalar, Options, JointCollectionTpl> & data,
169  const FrameIndex frame_id,
170  const ReferenceFrame rf = LOCAL)
171  {
172  typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model;
173  const typename Model::Frame & frame = model.frames[frame_id];
174  return getFrameAcceleration(model, data, frame.parentJoint, frame.placement, rf);
175  }
176 
194  template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
195  inline MotionTpl<Scalar, Options> getFrameClassicalAcceleration(
196  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
197  const DataTpl<Scalar, Options, JointCollectionTpl> & data,
198  const JointIndex joint_id,
199  const SE3Tpl<Scalar, Options> & placement,
200  const ReferenceFrame rf = LOCAL);
201 
225  template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
226  inline MotionTpl<Scalar, Options> getFrameClassicalAcceleration(
227  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
228  const DataTpl<Scalar, Options, JointCollectionTpl> & data,
229  const FrameIndex frame_id,
230  const ReferenceFrame rf = LOCAL)
231  {
232  typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model;
233  const typename Model::Frame & frame = model.frames[frame_id];
234  return getFrameClassicalAcceleration(model, data, frame.parentJoint, frame.placement, rf);
235  }
236 
265  template<
266  typename Scalar,
267  int Options,
268  template<typename, int>
269  class JointCollectionTpl,
270  typename Matrix6xLike>
271  void getFrameJacobian(
272  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
273  DataTpl<Scalar, Options, JointCollectionTpl> & data,
274  const JointIndex joint_id,
275  const SE3Tpl<Scalar, Options> & placement,
276  const ReferenceFrame reference_frame,
277  const Eigen::MatrixBase<Matrix6xLike> & J);
278 
303  template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
304  Eigen::Matrix<Scalar, 6, Eigen::Dynamic, Options> getFrameJacobian(
305  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
306  DataTpl<Scalar, Options, JointCollectionTpl> & data,
307  const JointIndex joint_id,
308  const SE3Tpl<Scalar, Options> & placement,
309  const ReferenceFrame reference_frame)
310  {
311  typedef Eigen::Matrix<Scalar, 6, Eigen::Dynamic, Options> Matrix6x;
312  Matrix6x res(Matrix6x::Zero(6, model.nv));
313 
314  getFrameJacobian(model, data, joint_id, placement, reference_frame, res);
315  return res;
316  }
317 
345  template<
346  typename Scalar,
347  int Options,
348  template<typename, int>
349  class JointCollectionTpl,
350  typename Matrix6xLike>
351  inline void getFrameJacobian(
352  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
353  DataTpl<Scalar, Options, JointCollectionTpl> & data,
354  const FrameIndex frame_id,
355  const ReferenceFrame reference_frame,
356  const Eigen::MatrixBase<Matrix6xLike> & J)
357  {
358  PINOCCHIO_CHECK_INPUT_ARGUMENT(
359  frame_id < (FrameIndex)model.nframes, "The index of the Frame is outside the bounds.");
360 
361  typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model;
362  typedef typename Model::Frame Frame;
363 
364  const Frame & frame = model.frames[frame_id];
365  data.oMf[frame_id] = data.oMi[frame.parentJoint] * frame.placement;
366 
367  getFrameJacobian(
368  model, data, frame.parentJoint, frame.placement, reference_frame,
369  PINOCCHIO_EIGEN_CONST_CAST(Matrix6xLike, J));
370  }
371 
396  template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
397  Eigen::Matrix<Scalar, 6, Eigen::Dynamic, Options> getFrameJacobian(
398  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
399  DataTpl<Scalar, Options, JointCollectionTpl> & data,
400  const FrameIndex frame_id,
401  const ReferenceFrame reference_frame)
402  {
403  typedef Eigen::Matrix<Scalar, 6, Eigen::Dynamic, Options> Matrix6x;
404  Matrix6x res(Matrix6x::Zero(6, model.nv));
405 
406  getFrameJacobian(model, data, frame_id, reference_frame, res);
407  return res;
408  }
409 
435  template<
436  typename Scalar,
437  int Options,
438  template<typename, int>
439  class JointCollectionTpl,
440  typename ConfigVectorType,
441  typename Matrix6xLike>
442  inline void computeFrameJacobian(
443  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
444  DataTpl<Scalar, Options, JointCollectionTpl> & data,
445  const Eigen::MatrixBase<ConfigVectorType> & q,
446  const FrameIndex frameId,
447  const ReferenceFrame reference_frame,
448  const Eigen::MatrixBase<Matrix6xLike> & J);
449 
474  template<
475  typename Scalar,
476  int Options,
477  template<typename, int>
478  class JointCollectionTpl,
479  typename ConfigVectorType,
480  typename Matrix6xLike>
481  inline void computeFrameJacobian(
482  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
483  DataTpl<Scalar, Options, JointCollectionTpl> & data,
484  const Eigen::MatrixBase<ConfigVectorType> & q,
485  const FrameIndex frameId,
486  const Eigen::MatrixBase<Matrix6xLike> & J)
487  {
488  computeFrameJacobian(
489  model, data, q.derived(), frameId, LOCAL, PINOCCHIO_EIGEN_CONST_CAST(Matrix6xLike, J));
490  }
491 
510  template<
511  typename Scalar,
512  int Options,
513  template<typename, int>
514  class JointCollectionTpl,
515  typename Matrix6xLike>
516  void getFrameJacobianTimeVariation(
517  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
518  DataTpl<Scalar, Options, JointCollectionTpl> & data,
519  const FrameIndex frame_id,
520  const ReferenceFrame rf,
521  const Eigen::MatrixBase<Matrix6xLike> & dJ);
522 
556  template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
557  InertiaTpl<Scalar, Options> computeSupportedInertiaByFrame(
558  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
559  const DataTpl<Scalar, Options, JointCollectionTpl> & data,
560  const FrameIndex frame_id,
561  bool with_subtree);
562 
596  template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
597  ForceTpl<Scalar, Options> computeSupportedForceByFrame(
598  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
599  const DataTpl<Scalar, Options, JointCollectionTpl> & data,
600  const FrameIndex frame_id);
601 
602 } // namespace pinocchio
603 
604 /* --- Details -------------------------------------------------------------------- */
605 #include "pinocchio/algorithm/frames.hxx"
606 
607 #if PINOCCHIO_ENABLE_TEMPLATE_INSTANTIATION
608  #include "pinocchio/algorithm/frames.txx"
609 #endif // PINOCCHIO_ENABLE_TEMPLATE_INSTANTIATION
610 
611 #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:193
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:1116
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 >
inverse-kinematics.J
J
Definition: inverse-kinematics.py:31
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
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:27
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:1117
contact-cholesky.frame_id
frame_id
Definition: contact-cholesky.py:22
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:1116
pinocchio
Main pinocchio namespace.
Definition: timings.cpp:27
pinocchio::LOCAL
@ LOCAL
Definition: multibody/fwd.hpp:50

pinocchio
Author(s):
autogenerated on Sat Jun 22 2024 02:41:46