$darkmode
pinocchio 4.0.0
A fast and flexible implementation of Rigid Body Dynamics algorithms and their analytical derivatives
kinematics-derivatives.hpp
1 //
2 // Copyright (c) 2017-2020 CNRS
3 // Copyright (c) 2018-2026 INRIA
4 //
5 
6 #pragma once
7 
8 // IWYU pragma: begin_keep
9 #include <Eigen/Core>
10 
11 #include <cassert>
12 #include <cstddef>
13 #include <type_traits>
14 #include <vector>
15 
16 #include <boost/fusion/container/vector.hpp>
17 
18 #include "pinocchio/macros.hpp"
19 #include "pinocchio/eigen-common.hpp"
20 #include "pinocchio/fwd.hpp"
21 
22 #include "pinocchio/math.hpp"
23 
24 #include "pinocchio/spatial.hpp"
25 
26 #include "pinocchio/multibody.hpp"
27 #include "pinocchio/multibody/joint.hpp"
28 #include "pinocchio/multibody/visitor.hpp"
29 
30 #include "pinocchio/algorithm/jacobian.hpp"
31 #include "pinocchio/algorithm/check.hpp"
32 // IWYU pragma: end_keep
33 
34 namespace pinocchio
35 {
36 
56  template<
57  typename Scalar,
58  int Options,
59  template<typename, int> class JointCollectionTpl,
60  typename ConfigVectorType,
61  typename TangentVectorType>
62  void computeForwardKinematicsDerivatives(
63  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
64  DataTpl<Scalar, Options, JointCollectionTpl> & data,
65  const Eigen::MatrixBase<ConfigVectorType> & q,
66  const Eigen::MatrixBase<TangentVectorType> & v);
67 
89  template<
90  typename Scalar,
91  int Options,
92  template<typename, int> class JointCollectionTpl,
93  typename ConfigVectorType,
94  typename TangentVectorType1,
95  typename TangentVectorType2>
96  void computeForwardKinematicsDerivatives(
97  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
98  DataTpl<Scalar, Options, JointCollectionTpl> & data,
99  const Eigen::MatrixBase<ConfigVectorType> & q,
100  const Eigen::MatrixBase<TangentVectorType1> & v,
101  const Eigen::MatrixBase<TangentVectorType2> & a);
102 
121  template<
122  typename Scalar,
123  int Options,
124  template<typename, int> class JointCollectionTpl,
125  typename Matrix6xOut1,
126  typename Matrix6xOut2>
127  void getJointVelocityDerivatives(
128  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
129  const DataTpl<Scalar, Options, JointCollectionTpl> & data,
130  const Model::JointIndex jointId,
131  const ReferenceFrame rf,
132  const Eigen::MatrixBase<Matrix6xOut1> & v_partial_dq,
133  const Eigen::MatrixBase<Matrix6xOut2> & v_partial_dv);
134 
166  template<
167  typename Scalar,
168  int Options,
169  template<typename, int> class JointCollectionTpl,
170  typename Matrix6xOut1,
171  typename Matrix6xOut2,
172  typename Matrix6xOut3,
173  typename Matrix6xOut4>
174  void getJointAccelerationDerivatives(
175  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
176  const DataTpl<Scalar, Options, JointCollectionTpl> & data,
177  const Model::JointIndex jointId,
178  const ReferenceFrame rf,
179  const Eigen::MatrixBase<Matrix6xOut1> & v_partial_dq,
180  const Eigen::MatrixBase<Matrix6xOut2> & a_partial_dq,
181  const Eigen::MatrixBase<Matrix6xOut3> & a_partial_dv,
182  const Eigen::MatrixBase<Matrix6xOut4> & a_partial_da);
183 
216  template<
217  typename Scalar,
218  int Options,
219  template<typename, int> class JointCollectionTpl,
220  typename Matrix6xOut1,
221  typename Matrix6xOut2,
222  typename Matrix6xOut3,
223  typename Matrix6xOut4,
224  typename Matrix6xOut5>
225  void getJointAccelerationDerivatives(
226  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
227  const DataTpl<Scalar, Options, JointCollectionTpl> & data,
228  const Model::JointIndex jointId,
229  const ReferenceFrame rf,
230  const Eigen::MatrixBase<Matrix6xOut1> & v_partial_dq,
231  const Eigen::MatrixBase<Matrix6xOut2> & v_partial_dv,
232  const Eigen::MatrixBase<Matrix6xOut3> & a_partial_dq,
233  const Eigen::MatrixBase<Matrix6xOut4> & a_partial_dv,
234  const Eigen::MatrixBase<Matrix6xOut5> & a_partial_da);
235 
258  template<
259  typename Scalar,
260  int Options,
261  template<typename, int> class JointCollectionTpl,
262  typename Matrix3xOut1,
263  typename Matrix3xOut2>
264  void getPointVelocityDerivatives(
265  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
266  const DataTpl<Scalar, Options, JointCollectionTpl> & data,
267  const Model::JointIndex joint_id,
268  const SE3Tpl<Scalar, Options> & placement,
269  const ReferenceFrame rf,
270  const Eigen::MatrixBase<Matrix3xOut1> & v_point_partial_dq,
271  const Eigen::MatrixBase<Matrix3xOut2> & v_point_partial_dv);
272 
306  template<
307  typename Scalar,
308  int Options,
309  template<typename, int> class JointCollectionTpl,
310  typename Matrix3xOut1,
311  typename Matrix3xOut2,
312  typename Matrix3xOut3,
313  typename Matrix3xOut4>
314  void getPointClassicAccelerationDerivatives(
315  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
316  const DataTpl<Scalar, Options, JointCollectionTpl> & data,
317  const Model::JointIndex joint_id,
318  const SE3Tpl<Scalar, Options> & placement,
319  const ReferenceFrame rf,
320  const Eigen::MatrixBase<Matrix3xOut1> & v_point_partial_dq,
321  const Eigen::MatrixBase<Matrix3xOut2> & a_point_partial_dq,
322  const Eigen::MatrixBase<Matrix3xOut3> & a_point_partial_dv,
323  const Eigen::MatrixBase<Matrix3xOut4> & a_point_partial_da);
324 
359  template<
360  typename Scalar,
361  int Options,
362  template<typename, int> class JointCollectionTpl,
363  typename Matrix3xOut1,
364  typename Matrix3xOut2,
365  typename Matrix3xOut3,
366  typename Matrix3xOut4,
367  typename Matrix3xOut5>
368  void getPointClassicAccelerationDerivatives(
369  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
370  const DataTpl<Scalar, Options, JointCollectionTpl> & data,
371  const Model::JointIndex joint_id,
372  const SE3Tpl<Scalar, Options> & placement,
373  const ReferenceFrame rf,
374  const Eigen::MatrixBase<Matrix3xOut1> & v_point_partial_dq,
375  const Eigen::MatrixBase<Matrix3xOut2> & v_point_partial_dv,
376  const Eigen::MatrixBase<Matrix3xOut3> & a_point_partial_dq,
377  const Eigen::MatrixBase<Matrix3xOut4> & a_point_partial_dv,
378  const Eigen::MatrixBase<Matrix3xOut5> & a_point_partial_da);
379 
395  template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
396  void computeJointKinematicHessians(
397  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
398  DataTpl<Scalar, Options, JointCollectionTpl> & data);
399 
416  template<
417  typename Scalar,
418  int Options,
419  template<typename, int> class JointCollectionTpl,
420  typename ConfigVectorType>
421  void computeJointKinematicHessians(
422  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
423  DataTpl<Scalar, Options, JointCollectionTpl> & data,
424  const Eigen::MatrixBase<ConfigVectorType> & q)
425  {
426  computeJointJacobians(model, data, q);
427  computeJointKinematicHessians(model, data);
428  }
429 
456  template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
457  void getJointKinematicHessian(
458  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
459  const DataTpl<Scalar, Options, JointCollectionTpl> & data,
460  const Model::JointIndex joint_id,
461  const ReferenceFrame rf,
462  Tensor<Scalar, 3, Options> & kinematic_hessian);
463 
491  template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
492  void getFrameKinematicHessian(
493  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
494  const DataTpl<Scalar, Options, JointCollectionTpl> & data,
495  const Model::JointIndex joint_id,
496  const SE3Tpl<Scalar, Options> & frame_placement,
497  const ReferenceFrame rf,
498  Tensor<Scalar, 3, Options> & kinematic_hessian);
499 
526  template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
527  void getFrameKinematicHessian(
528  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
529  const DataTpl<Scalar, Options, JointCollectionTpl> & data,
530  const Model::FrameIndex frame_id,
531  const ReferenceFrame rf,
532  Tensor<Scalar, 3, Options> & kinematic_hessian)
533  {
534  PINOCCHIO_CHECK_INPUT_ARGUMENT(
535  frame_id < (FrameIndex)model.nframes, "The index of the Frame is outside the bounds.");
536  const auto frame = model.frames[frame_id];
537 
538  getFrameKinematicHessian(
539  model, data, frame.parentJoint, frame.placement, rf, kinematic_hessian);
540  }
541 
570  template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
571  Tensor<Scalar, 3, Options> getJointKinematicHessian(
572  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
573  const DataTpl<Scalar, Options, JointCollectionTpl> & data,
574  const Model::JointIndex joint_id,
575  const ReferenceFrame rf)
576  {
577  typedef Tensor<Scalar, 3, Options> ReturnType;
578  ReturnType res(6, model.nv, model.nv);
579  res.setZero();
580  getJointKinematicHessian(model, data, joint_id, rf, res);
581  return res;
582  }
583 
612  template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
613  Tensor<Scalar, 3, Options> getFrameKinematicHessian(
614  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
615  const DataTpl<Scalar, Options, JointCollectionTpl> & data,
616  const Model::JointIndex joint_id,
617  const SE3Tpl<Scalar, Options> & frame_placement,
618  const ReferenceFrame rf)
619  {
620  typedef Tensor<Scalar, 3, Options> ReturnType;
621  ReturnType res(6, model.nv, model.nv);
622  res.setZero();
623  getFrameKinematicHessian(model, data, joint_id, frame_placement, rf, res);
624  return res;
625  }
626 
654  template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
655  Tensor<Scalar, 3, Options> getFrameKinematicHessian(
656  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
657  const DataTpl<Scalar, Options, JointCollectionTpl> & data,
658  const Model::FrameIndex frame_id,
659  const ReferenceFrame rf)
660  {
661  typedef Tensor<Scalar, 3, Options> ReturnType;
662  ReturnType res(6, model.nv, model.nv);
663  res.setZero();
664  getFrameKinematicHessian(model, data, frame_id, rf, res);
665  return res;
666  }
667 
668 } // namespace pinocchio
669 
670 // IWYU pragma: begin_exports
671 #include "pinocchio/src/algorithm/kinematics-derivatives.hxx"
672 // IWYU pragma: end_exports
pinocchio
Main pinocchio namespace.
Definition: treeview.dox:11
pinocchio::computeForwardKinematicsDerivatives
void computeForwardKinematicsDerivatives(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, const Eigen::MatrixBase< ConfigVectorType > &q, const Eigen::MatrixBase< TangentVectorType > &v)
Computes all the terms required to compute the derivatives of the placement, spatial velocity for any...
pinocchio::getPointVelocityDerivatives
void getPointVelocityDerivatives(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const DataTpl< Scalar, Options, JointCollectionTpl > &data, const Model::JointIndex joint_id, const SE3Tpl< Scalar, Options > &placement, const ReferenceFrame rf, const Eigen::MatrixBase< Matrix3xOut1 > &v_point_partial_dq, const Eigen::MatrixBase< Matrix3xOut2 > &v_point_partial_dv)
Computes the partial derivatives of the velocity of a point given by its placement information w....
pinocchio::computeJointJacobians
const DataTpl< Scalar, Options, JointCollectionTpl >::Matrix6x & computeJointJacobians(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, const Eigen::MatrixBase< ConfigVectorType > &q)
Computes the full model Jacobian, i.e. the stack of all motion subspace expressed in the world frame....
pinocchio::computeJointKinematicHessians
void computeJointKinematicHessians(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data)
Computes all the terms required to compute the second order derivatives of the placement information,...
pinocchio::getJointAccelerationDerivatives
void getJointAccelerationDerivatives(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const DataTpl< Scalar, Options, JointCollectionTpl > &data, const Model::JointIndex jointId, 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 derivaties of the spatial acceleration of a given with respect to the joint conf...
pinocchio::getFrameKinematicHessian
void getFrameKinematicHessian(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const DataTpl< Scalar, Options, JointCollectionTpl > &data, const Model::JointIndex joint_id, const SE3Tpl< Scalar, Options > &frame_placement, const ReferenceFrame rf, Tensor< Scalar, 3, Options > &kinematic_hessian)
Retrieves the kinematic Hessian of a given joint according to the values aleardy computed by computeJ...
pinocchio::getJointKinematicHessian
void getJointKinematicHessian(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const DataTpl< Scalar, Options, JointCollectionTpl > &data, const Model::JointIndex joint_id, const ReferenceFrame rf, Tensor< Scalar, 3, Options > &kinematic_hessian)
Retrieves the kinematic Hessian of a given joint according to the values aleardy computed by computeJ...
pinocchio::getJointVelocityDerivatives
void getJointVelocityDerivatives(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const DataTpl< Scalar, Options, JointCollectionTpl > &data, const Model::JointIndex jointId, const ReferenceFrame rf, const Eigen::MatrixBase< Matrix6xOut1 > &v_partial_dq, const Eigen::MatrixBase< Matrix6xOut2 > &v_partial_dv)
Computes the partial derivaties of the spatial velocity of a given with respect to the joint configur...
pinocchio::getPointClassicAccelerationDerivatives
void getPointClassicAccelerationDerivatives(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const DataTpl< Scalar, Options, JointCollectionTpl > &data, const Model::JointIndex joint_id, const SE3Tpl< Scalar, Options > &placement, const ReferenceFrame rf, const Eigen::MatrixBase< Matrix3xOut1 > &v_point_partial_dq, const Eigen::MatrixBase< Matrix3xOut2 > &a_point_partial_dq, const Eigen::MatrixBase< Matrix3xOut3 > &a_point_partial_dv, const Eigen::MatrixBase< Matrix3xOut4 > &a_point_partial_da)
Computes the partial derivatives of the classic acceleration of a point given by its placement inform...