$darkmode
pinocchio 4.0.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 #pragma once
6 
7 // IWYU pragma: begin_keep
8 #include <Eigen/Core>
9 
10 #include <cassert>
11 #include <cstddef>
12 #include <vector>
13 
14 #include <boost/fusion/container/vector.hpp>
15 
16 #include "pinocchio/macros.hpp"
17 #include "pinocchio/eigen-common.hpp"
18 
19 #include "pinocchio/math.hpp"
20 
21 #include "pinocchio/spatial.hpp"
22 
23 #include "pinocchio/multibody.hpp"
24 #include "pinocchio/multibody/joint.hpp"
25 #include "pinocchio/multibody/visitor.hpp"
26 
27 #include "pinocchio/algorithm/check.hpp"
28 #include "pinocchio/algorithm/kinematics-derivatives.hpp"
29 // IWYU pragma: end_keep
30 
31 namespace pinocchio
32 {
33 
54  template<
55  typename Scalar,
56  int Options,
57  template<typename, int> class JointCollectionTpl,
58  typename Matrix6xOut1,
59  typename Matrix6xOut2>
60  void getFrameVelocityDerivatives(
61  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
62  const DataTpl<Scalar, Options, JointCollectionTpl> & data,
63  const JointIndex joint_id,
64  const SE3Tpl<Scalar, Options> & placement,
65  const ReferenceFrame rf,
66  const Eigen::MatrixBase<Matrix6xOut1> & v_partial_dq,
67  const Eigen::MatrixBase<Matrix6xOut2> & v_partial_dv);
68 
88  template<
89  typename Scalar,
90  int Options,
91  template<typename, int> class JointCollectionTpl,
92  typename Matrix6xOut1,
93  typename Matrix6xOut2>
94  void getFrameVelocityDerivatives(
95  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
96  DataTpl<Scalar, Options, JointCollectionTpl> & data,
97  const FrameIndex frame_id,
98  const ReferenceFrame rf,
99  const Eigen::MatrixBase<Matrix6xOut1> & v_partial_dq,
100  const Eigen::MatrixBase<Matrix6xOut2> & v_partial_dv)
101  {
102  PINOCCHIO_CHECK_INPUT_ARGUMENT((int)frame_id < model.nframes, "The frame_id is not valid.");
103  typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model;
104  typedef DataTpl<Scalar, Options, JointCollectionTpl> Data;
105  typedef typename Model::Frame Frame;
106 
107  const Frame & frame = model.frames[frame_id];
108  typename Data::SE3 & oMframe = data.oMf[frame_id];
109  oMframe = data.oMi[frame.parentJoint] * frame.placement; // for backward compatibility
110  getFrameVelocityDerivatives(
111  model, data, frame.parentJoint, frame.placement, rf,
112  PINOCCHIO_EIGEN_CONST_CAST(Matrix6xOut1, v_partial_dq),
113  PINOCCHIO_EIGEN_CONST_CAST(Matrix6xOut1, v_partial_dv));
114  }
115 
147  template<
148  typename Scalar,
149  int Options,
150  template<typename, int> class JointCollectionTpl,
151  typename Matrix6xOut1,
152  typename Matrix6xOut2,
153  typename Matrix6xOut3,
154  typename Matrix6xOut4>
155  void getFrameAccelerationDerivatives(
156  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
157  DataTpl<Scalar, Options, JointCollectionTpl> & data,
158  const JointIndex joint_id,
159  const SE3Tpl<Scalar, Options> & placement,
160  const ReferenceFrame rf,
161  const Eigen::MatrixBase<Matrix6xOut1> & v_partial_dq,
162  const Eigen::MatrixBase<Matrix6xOut2> & a_partial_dq,
163  const Eigen::MatrixBase<Matrix6xOut3> & a_partial_dv,
164  const Eigen::MatrixBase<Matrix6xOut4> & a_partial_da);
165 
193  template<
194  typename Scalar,
195  int Options,
196  template<typename, int> class JointCollectionTpl,
197  typename Matrix6xOut1,
198  typename Matrix6xOut2,
199  typename Matrix6xOut3,
200  typename Matrix6xOut4>
201  void getFrameAccelerationDerivatives(
202  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
203  DataTpl<Scalar, Options, JointCollectionTpl> & data,
204  const FrameIndex frame_id,
205  const ReferenceFrame rf,
206  const Eigen::MatrixBase<Matrix6xOut1> & v_partial_dq,
207  const Eigen::MatrixBase<Matrix6xOut2> & a_partial_dq,
208  const Eigen::MatrixBase<Matrix6xOut3> & a_partial_dv,
209  const Eigen::MatrixBase<Matrix6xOut4> & a_partial_da)
210  {
211  PINOCCHIO_CHECK_INPUT_ARGUMENT((int)frame_id < model.nframes, "The frame_id is not valid.");
212  typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model;
213  typedef DataTpl<Scalar, Options, JointCollectionTpl> Data;
214  typedef typename Model::Frame Frame;
215 
216  const Frame & frame = model.frames[frame_id];
217  typename Data::SE3 & oMframe = data.oMf[frame_id];
218  oMframe = data.oMi[frame.parentJoint] * frame.placement; // for backward compatibility
219  getFrameAccelerationDerivatives(
220  model, data, frame.parentJoint, frame.placement, rf,
221  PINOCCHIO_EIGEN_CONST_CAST(Matrix6xOut1, v_partial_dq),
222  PINOCCHIO_EIGEN_CONST_CAST(Matrix6xOut2, a_partial_dq),
223  PINOCCHIO_EIGEN_CONST_CAST(Matrix6xOut3, a_partial_dv),
224  PINOCCHIO_EIGEN_CONST_CAST(Matrix6xOut4, a_partial_da));
225  }
226 
260  template<
261  typename Scalar,
262  int Options,
263  template<typename, int> class JointCollectionTpl,
264  typename Matrix6xOut1,
265  typename Matrix6xOut2,
266  typename Matrix6xOut3,
267  typename Matrix6xOut4,
268  typename Matrix6xOut5>
269  void getFrameAccelerationDerivatives(
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 rf,
275  const Eigen::MatrixBase<Matrix6xOut1> & v_partial_dq,
276  const Eigen::MatrixBase<Matrix6xOut2> & v_partial_dv,
277  const Eigen::MatrixBase<Matrix6xOut3> & a_partial_dq,
278  const Eigen::MatrixBase<Matrix6xOut4> & a_partial_dv,
279  const Eigen::MatrixBase<Matrix6xOut5> & a_partial_da)
280  {
281  getFrameAccelerationDerivatives(
282  model, data, joint_id, placement, rf, PINOCCHIO_EIGEN_CONST_CAST(Matrix6xOut1, v_partial_dq),
283  PINOCCHIO_EIGEN_CONST_CAST(Matrix6xOut3, a_partial_dq),
284  PINOCCHIO_EIGEN_CONST_CAST(Matrix6xOut4, a_partial_dv),
285  PINOCCHIO_EIGEN_CONST_CAST(Matrix6xOut5, a_partial_da));
286 
287  PINOCCHIO_EIGEN_CONST_CAST(Matrix6xOut2, v_partial_dv) = a_partial_da;
288  }
289 
320  template<
321  typename Scalar,
322  int Options,
323  template<typename, int> class JointCollectionTpl,
324  typename Matrix6xOut1,
325  typename Matrix6xOut2,
326  typename Matrix6xOut3,
327  typename Matrix6xOut4,
328  typename Matrix6xOut5>
329  void getFrameAccelerationDerivatives(
330  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
331  DataTpl<Scalar, Options, JointCollectionTpl> & data,
332  const FrameIndex frame_id,
333  const ReferenceFrame rf,
334  const Eigen::MatrixBase<Matrix6xOut1> & v_partial_dq,
335  const Eigen::MatrixBase<Matrix6xOut2> & v_partial_dv,
336  const Eigen::MatrixBase<Matrix6xOut3> & a_partial_dq,
337  const Eigen::MatrixBase<Matrix6xOut4> & a_partial_dv,
338  const Eigen::MatrixBase<Matrix6xOut5> & a_partial_da)
339  {
340  PINOCCHIO_CHECK_INPUT_ARGUMENT((int)frame_id < model.nframes, "The frame_id is not valid.");
341  typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model;
342  typedef DataTpl<Scalar, Options, JointCollectionTpl> Data;
343  typedef typename Model::Frame Frame;
344 
345  const Frame & frame = model.frames[frame_id];
346  typename Data::SE3 & oMframe = data.oMf[frame_id];
347  oMframe = data.oMi[frame.parentJoint] * frame.placement; // for backward compatibility
348  getFrameAccelerationDerivatives(
349  model, data, frame.parentJoint, frame.placement, rf,
350  PINOCCHIO_EIGEN_CONST_CAST(Matrix6xOut1, v_partial_dq),
351  PINOCCHIO_EIGEN_CONST_CAST(Matrix6xOut2, v_partial_dv),
352  PINOCCHIO_EIGEN_CONST_CAST(Matrix6xOut3, a_partial_dq),
353  PINOCCHIO_EIGEN_CONST_CAST(Matrix6xOut4, a_partial_dv),
354  PINOCCHIO_EIGEN_CONST_CAST(Matrix6xOut5, a_partial_da));
355  }
356 } // namespace pinocchio
357 
358 // IWYU pragma: begin_exports
359 #include "pinocchio/src/algorithm/frames-derivatives.hxx"
360 // IWYU pragma: end_exports
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,...