pinocchio  3.2.0
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>
38  class JointCollectionTpl,
39  typename Matrix6xOut1,
40  typename Matrix6xOut2>
41  void getFrameVelocityDerivatives(
42  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
43  const DataTpl<Scalar, Options, JointCollectionTpl> & data,
44  const JointIndex joint_id,
45  const SE3Tpl<Scalar, Options> & placement,
46  const ReferenceFrame rf,
47  const Eigen::MatrixBase<Matrix6xOut1> & v_partial_dq,
48  const Eigen::MatrixBase<Matrix6xOut2> & v_partial_dv);
49 
69  template<
70  typename Scalar,
71  int Options,
72  template<typename, int>
73  class JointCollectionTpl,
74  typename Matrix6xOut1,
75  typename Matrix6xOut2>
76  void getFrameVelocityDerivatives(
77  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
78  DataTpl<Scalar, Options, JointCollectionTpl> & data,
79  const FrameIndex frame_id,
80  const ReferenceFrame rf,
81  const Eigen::MatrixBase<Matrix6xOut1> & v_partial_dq,
82  const Eigen::MatrixBase<Matrix6xOut2> & v_partial_dv)
83  {
84  PINOCCHIO_CHECK_INPUT_ARGUMENT((int)frame_id < model.nframes, "The frame_id is not valid.");
85  typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model;
86  typedef DataTpl<Scalar, Options, JointCollectionTpl> Data;
87  typedef typename Model::Frame Frame;
88 
89  const Frame & frame = model.frames[frame_id];
90  typename Data::SE3 & oMframe = data.oMf[frame_id];
91  oMframe = data.oMi[frame.parentJoint] * frame.placement; // for backward compatibility
92  getFrameVelocityDerivatives(
93  model, data, frame.parentJoint, frame.placement, rf,
94  PINOCCHIO_EIGEN_CONST_CAST(Matrix6xOut1, v_partial_dq),
95  PINOCCHIO_EIGEN_CONST_CAST(Matrix6xOut1, v_partial_dv));
96  }
97 
129  template<
130  typename Scalar,
131  int Options,
132  template<typename, int>
133  class JointCollectionTpl,
134  typename Matrix6xOut1,
135  typename Matrix6xOut2,
136  typename Matrix6xOut3,
137  typename Matrix6xOut4>
138  void getFrameAccelerationDerivatives(
139  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
140  DataTpl<Scalar, Options, JointCollectionTpl> & data,
141  const JointIndex joint_id,
142  const SE3Tpl<Scalar, Options> & placement,
143  const ReferenceFrame rf,
144  const Eigen::MatrixBase<Matrix6xOut1> & v_partial_dq,
145  const Eigen::MatrixBase<Matrix6xOut2> & a_partial_dq,
146  const Eigen::MatrixBase<Matrix6xOut3> & a_partial_dv,
147  const Eigen::MatrixBase<Matrix6xOut4> & a_partial_da);
148 
176  template<
177  typename Scalar,
178  int Options,
179  template<typename, int>
180  class JointCollectionTpl,
181  typename Matrix6xOut1,
182  typename Matrix6xOut2,
183  typename Matrix6xOut3,
184  typename Matrix6xOut4>
185  void getFrameAccelerationDerivatives(
186  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
187  DataTpl<Scalar, Options, JointCollectionTpl> & data,
188  const FrameIndex frame_id,
189  const ReferenceFrame rf,
190  const Eigen::MatrixBase<Matrix6xOut1> & v_partial_dq,
191  const Eigen::MatrixBase<Matrix6xOut2> & a_partial_dq,
192  const Eigen::MatrixBase<Matrix6xOut3> & a_partial_dv,
193  const Eigen::MatrixBase<Matrix6xOut4> & a_partial_da)
194  {
195  PINOCCHIO_CHECK_INPUT_ARGUMENT((int)frame_id < model.nframes, "The frame_id is not valid.");
196  typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model;
197  typedef DataTpl<Scalar, Options, JointCollectionTpl> Data;
198  typedef typename Model::Frame Frame;
199 
200  const Frame & frame = model.frames[frame_id];
201  typename Data::SE3 & oMframe = data.oMf[frame_id];
202  oMframe = data.oMi[frame.parentJoint] * frame.placement; // for backward compatibility
203  getFrameAccelerationDerivatives(
204  model, data, frame.parentJoint, frame.placement, rf,
205  PINOCCHIO_EIGEN_CONST_CAST(Matrix6xOut1, v_partial_dq),
206  PINOCCHIO_EIGEN_CONST_CAST(Matrix6xOut2, a_partial_dq),
207  PINOCCHIO_EIGEN_CONST_CAST(Matrix6xOut3, a_partial_dv),
208  PINOCCHIO_EIGEN_CONST_CAST(Matrix6xOut4, a_partial_da));
209  }
210 
244  template<
245  typename Scalar,
246  int Options,
247  template<typename, int>
248  class JointCollectionTpl,
249  typename Matrix6xOut1,
250  typename Matrix6xOut2,
251  typename Matrix6xOut3,
252  typename Matrix6xOut4,
253  typename Matrix6xOut5>
254  void getFrameAccelerationDerivatives(
255  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
256  DataTpl<Scalar, Options, JointCollectionTpl> & data,
257  const JointIndex joint_id,
258  const SE3Tpl<Scalar, Options> & placement,
259  const ReferenceFrame rf,
260  const Eigen::MatrixBase<Matrix6xOut1> & v_partial_dq,
261  const Eigen::MatrixBase<Matrix6xOut2> & v_partial_dv,
262  const Eigen::MatrixBase<Matrix6xOut3> & a_partial_dq,
263  const Eigen::MatrixBase<Matrix6xOut4> & a_partial_dv,
264  const Eigen::MatrixBase<Matrix6xOut5> & a_partial_da)
265  {
266  getFrameAccelerationDerivatives(
267  model, data, joint_id, placement, rf, PINOCCHIO_EIGEN_CONST_CAST(Matrix6xOut1, v_partial_dq),
268  PINOCCHIO_EIGEN_CONST_CAST(Matrix6xOut3, a_partial_dq),
269  PINOCCHIO_EIGEN_CONST_CAST(Matrix6xOut4, a_partial_dv),
270  PINOCCHIO_EIGEN_CONST_CAST(Matrix6xOut5, a_partial_da));
271 
272  PINOCCHIO_EIGEN_CONST_CAST(Matrix6xOut2, v_partial_dv) = a_partial_da;
273  }
274 
305  template<
306  typename Scalar,
307  int Options,
308  template<typename, int>
309  class JointCollectionTpl,
310  typename Matrix6xOut1,
311  typename Matrix6xOut2,
312  typename Matrix6xOut3,
313  typename Matrix6xOut4,
314  typename Matrix6xOut5>
315  void getFrameAccelerationDerivatives(
316  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
317  DataTpl<Scalar, Options, JointCollectionTpl> & data,
318  const FrameIndex frame_id,
319  const ReferenceFrame rf,
320  const Eigen::MatrixBase<Matrix6xOut1> & v_partial_dq,
321  const Eigen::MatrixBase<Matrix6xOut2> & v_partial_dv,
322  const Eigen::MatrixBase<Matrix6xOut3> & a_partial_dq,
323  const Eigen::MatrixBase<Matrix6xOut4> & a_partial_dv,
324  const Eigen::MatrixBase<Matrix6xOut5> & a_partial_da)
325  {
326  PINOCCHIO_CHECK_INPUT_ARGUMENT((int)frame_id < model.nframes, "The frame_id is not valid.");
327  typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model;
328  typedef DataTpl<Scalar, Options, JointCollectionTpl> Data;
329  typedef typename Model::Frame Frame;
330 
331  const Frame & frame = model.frames[frame_id];
332  typename Data::SE3 & oMframe = data.oMf[frame_id];
333  oMframe = data.oMi[frame.parentJoint] * frame.placement; // for backward compatibility
334  getFrameAccelerationDerivatives(
335  model, data, frame.parentJoint, frame.placement, rf,
336  PINOCCHIO_EIGEN_CONST_CAST(Matrix6xOut1, v_partial_dq),
337  PINOCCHIO_EIGEN_CONST_CAST(Matrix6xOut2, v_partial_dv),
338  PINOCCHIO_EIGEN_CONST_CAST(Matrix6xOut3, a_partial_dq),
339  PINOCCHIO_EIGEN_CONST_CAST(Matrix6xOut4, a_partial_dv),
340  PINOCCHIO_EIGEN_CONST_CAST(Matrix6xOut5, a_partial_da));
341  }
342 } // namespace pinocchio
343 
344 #include "pinocchio/algorithm/frames-derivatives.hxx"
345 
346 #if PINOCCHIO_ENABLE_TEMPLATE_INSTANTIATION
347  #include "pinocchio/algorithm/frames-derivatives.txx"
348 #endif // PINOCCHIO_ENABLE_TEMPLATE_INSTANTIATION
349 
350 #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