pinocchio  3.3.1
A fast and flexible implementation of Rigid Body Dynamics algorithms and their analytical derivatives
frames-derivatives.hpp
1 //
2 // Copyright (c) 2020 INRIA
3 //
4 
5 #ifndef __pinocchio_algorithm_frames_derivatives_hpp__
6 #define __pinocchio_algorithm_frames_derivatives_hpp__
7 
8 #include "pinocchio/multibody/model.hpp"
9 #include "pinocchio/multibody/data.hpp"
10 
11 namespace pinocchio
12 {
13 
34  template<
35  typename Scalar,
36  int Options,
37  template<typename, int> class JointCollectionTpl,
38  typename Matrix6xOut1,
39  typename Matrix6xOut2>
40  void getFrameVelocityDerivatives(
41  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
42  const DataTpl<Scalar, Options, JointCollectionTpl> & data,
43  const JointIndex joint_id,
44  const SE3Tpl<Scalar, Options> & placement,
45  const ReferenceFrame rf,
46  const Eigen::MatrixBase<Matrix6xOut1> & v_partial_dq,
47  const Eigen::MatrixBase<Matrix6xOut2> & v_partial_dv);
48 
68  template<
69  typename Scalar,
70  int Options,
71  template<typename, int> class JointCollectionTpl,
72  typename Matrix6xOut1,
73  typename Matrix6xOut2>
74  void getFrameVelocityDerivatives(
75  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
76  DataTpl<Scalar, Options, JointCollectionTpl> & data,
77  const FrameIndex frame_id,
78  const ReferenceFrame rf,
79  const Eigen::MatrixBase<Matrix6xOut1> & v_partial_dq,
80  const Eigen::MatrixBase<Matrix6xOut2> & v_partial_dv)
81  {
82  PINOCCHIO_CHECK_INPUT_ARGUMENT((int)frame_id < model.nframes, "The frame_id is not valid.");
83  typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model;
84  typedef DataTpl<Scalar, Options, JointCollectionTpl> Data;
85  typedef typename Model::Frame Frame;
86 
87  const Frame & frame = model.frames[frame_id];
88  typename Data::SE3 & oMframe = data.oMf[frame_id];
89  oMframe = data.oMi[frame.parentJoint] * frame.placement; // for backward compatibility
90  getFrameVelocityDerivatives(
91  model, data, frame.parentJoint, frame.placement, rf,
92  PINOCCHIO_EIGEN_CONST_CAST(Matrix6xOut1, v_partial_dq),
93  PINOCCHIO_EIGEN_CONST_CAST(Matrix6xOut1, v_partial_dv));
94  }
95 
127  template<
128  typename Scalar,
129  int Options,
130  template<typename, int> class JointCollectionTpl,
131  typename Matrix6xOut1,
132  typename Matrix6xOut2,
133  typename Matrix6xOut3,
134  typename Matrix6xOut4>
135  void getFrameAccelerationDerivatives(
136  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
137  DataTpl<Scalar, Options, JointCollectionTpl> & data,
138  const JointIndex joint_id,
139  const SE3Tpl<Scalar, Options> & placement,
140  const ReferenceFrame rf,
141  const Eigen::MatrixBase<Matrix6xOut1> & v_partial_dq,
142  const Eigen::MatrixBase<Matrix6xOut2> & a_partial_dq,
143  const Eigen::MatrixBase<Matrix6xOut3> & a_partial_dv,
144  const Eigen::MatrixBase<Matrix6xOut4> & a_partial_da);
145 
173  template<
174  typename Scalar,
175  int Options,
176  template<typename, int> class JointCollectionTpl,
177  typename Matrix6xOut1,
178  typename Matrix6xOut2,
179  typename Matrix6xOut3,
180  typename Matrix6xOut4>
181  void getFrameAccelerationDerivatives(
182  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
183  DataTpl<Scalar, Options, JointCollectionTpl> & data,
184  const FrameIndex frame_id,
185  const ReferenceFrame rf,
186  const Eigen::MatrixBase<Matrix6xOut1> & v_partial_dq,
187  const Eigen::MatrixBase<Matrix6xOut2> & a_partial_dq,
188  const Eigen::MatrixBase<Matrix6xOut3> & a_partial_dv,
189  const Eigen::MatrixBase<Matrix6xOut4> & a_partial_da)
190  {
191  PINOCCHIO_CHECK_INPUT_ARGUMENT((int)frame_id < model.nframes, "The frame_id is not valid.");
192  typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model;
193  typedef DataTpl<Scalar, Options, JointCollectionTpl> Data;
194  typedef typename Model::Frame Frame;
195 
196  const Frame & frame = model.frames[frame_id];
197  typename Data::SE3 & oMframe = data.oMf[frame_id];
198  oMframe = data.oMi[frame.parentJoint] * frame.placement; // for backward compatibility
199  getFrameAccelerationDerivatives(
200  model, data, frame.parentJoint, frame.placement, rf,
201  PINOCCHIO_EIGEN_CONST_CAST(Matrix6xOut1, v_partial_dq),
202  PINOCCHIO_EIGEN_CONST_CAST(Matrix6xOut2, a_partial_dq),
203  PINOCCHIO_EIGEN_CONST_CAST(Matrix6xOut3, a_partial_dv),
204  PINOCCHIO_EIGEN_CONST_CAST(Matrix6xOut4, a_partial_da));
205  }
206 
240  template<
241  typename Scalar,
242  int Options,
243  template<typename, int> class JointCollectionTpl,
244  typename Matrix6xOut1,
245  typename Matrix6xOut2,
246  typename Matrix6xOut3,
247  typename Matrix6xOut4,
248  typename Matrix6xOut5>
249  void getFrameAccelerationDerivatives(
250  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
251  DataTpl<Scalar, Options, JointCollectionTpl> & data,
252  const JointIndex joint_id,
253  const SE3Tpl<Scalar, Options> & placement,
254  const ReferenceFrame rf,
255  const Eigen::MatrixBase<Matrix6xOut1> & v_partial_dq,
256  const Eigen::MatrixBase<Matrix6xOut2> & v_partial_dv,
257  const Eigen::MatrixBase<Matrix6xOut3> & a_partial_dq,
258  const Eigen::MatrixBase<Matrix6xOut4> & a_partial_dv,
259  const Eigen::MatrixBase<Matrix6xOut5> & a_partial_da)
260  {
261  getFrameAccelerationDerivatives(
262  model, data, joint_id, placement, rf, PINOCCHIO_EIGEN_CONST_CAST(Matrix6xOut1, v_partial_dq),
263  PINOCCHIO_EIGEN_CONST_CAST(Matrix6xOut3, a_partial_dq),
264  PINOCCHIO_EIGEN_CONST_CAST(Matrix6xOut4, a_partial_dv),
265  PINOCCHIO_EIGEN_CONST_CAST(Matrix6xOut5, a_partial_da));
266 
267  PINOCCHIO_EIGEN_CONST_CAST(Matrix6xOut2, v_partial_dv) = a_partial_da;
268  }
269 
300  template<
301  typename Scalar,
302  int Options,
303  template<typename, int> class JointCollectionTpl,
304  typename Matrix6xOut1,
305  typename Matrix6xOut2,
306  typename Matrix6xOut3,
307  typename Matrix6xOut4,
308  typename Matrix6xOut5>
309  void getFrameAccelerationDerivatives(
310  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
311  DataTpl<Scalar, Options, JointCollectionTpl> & data,
312  const FrameIndex frame_id,
313  const ReferenceFrame rf,
314  const Eigen::MatrixBase<Matrix6xOut1> & v_partial_dq,
315  const Eigen::MatrixBase<Matrix6xOut2> & v_partial_dv,
316  const Eigen::MatrixBase<Matrix6xOut3> & a_partial_dq,
317  const Eigen::MatrixBase<Matrix6xOut4> & a_partial_dv,
318  const Eigen::MatrixBase<Matrix6xOut5> & a_partial_da)
319  {
320  PINOCCHIO_CHECK_INPUT_ARGUMENT((int)frame_id < model.nframes, "The frame_id is not valid.");
321  typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model;
322  typedef DataTpl<Scalar, Options, JointCollectionTpl> Data;
323  typedef typename Model::Frame Frame;
324 
325  const Frame & frame = model.frames[frame_id];
326  typename Data::SE3 & oMframe = data.oMf[frame_id];
327  oMframe = data.oMi[frame.parentJoint] * frame.placement; // for backward compatibility
328  getFrameAccelerationDerivatives(
329  model, data, frame.parentJoint, frame.placement, rf,
330  PINOCCHIO_EIGEN_CONST_CAST(Matrix6xOut1, v_partial_dq),
331  PINOCCHIO_EIGEN_CONST_CAST(Matrix6xOut2, v_partial_dv),
332  PINOCCHIO_EIGEN_CONST_CAST(Matrix6xOut3, a_partial_dq),
333  PINOCCHIO_EIGEN_CONST_CAST(Matrix6xOut4, a_partial_dv),
334  PINOCCHIO_EIGEN_CONST_CAST(Matrix6xOut5, a_partial_da));
335  }
336 } // namespace pinocchio
337 
338 #include "pinocchio/algorithm/frames-derivatives.hxx"
339 
340 #if PINOCCHIO_ENABLE_TEMPLATE_INSTANTIATION
341  #include "pinocchio/algorithm/frames-derivatives.txx"
342 #endif // PINOCCHIO_ENABLE_TEMPLATE_INSTANTIATION
343 
344 #endif // ifndef __pinocchio_algorithm_frames_derivatives_hpp__
pinocchio::ReferenceFrame
ReferenceFrame
Various conventions to express the velocity of a moving frame.
Definition: fwd.hpp:47
pinocchio
Main pinocchio namespace.
Definition: treeview.dox:11
pinocchio::getFrameAccelerationDerivatives
void getFrameAccelerationDerivatives(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, const JointIndex joint_id, const SE3Tpl< Scalar, Options > &placement, const ReferenceFrame rf, const Eigen::MatrixBase< Matrix6xOut1 > &v_partial_dq, const Eigen::MatrixBase< Matrix6xOut2 > &a_partial_dq, const Eigen::MatrixBase< Matrix6xOut3 > &a_partial_dv, const Eigen::MatrixBase< Matrix6xOut4 > &a_partial_da)
Computes the partial derivatives of the spatial acceleration of a frame given by its relative placeme...
pinocchio::getFrameVelocityDerivatives
void getFrameVelocityDerivatives(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const DataTpl< Scalar, Options, JointCollectionTpl > &data, const JointIndex joint_id, const SE3Tpl< Scalar, Options > &placement, const ReferenceFrame rf, const Eigen::MatrixBase< Matrix6xOut1 > &v_partial_dq, const Eigen::MatrixBase< Matrix6xOut2 > &v_partial_dv)
Computes the partial derivatives of the spatial velocity of a frame given by its relative placement,...
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