pinocchio  3.3.1
A fast and flexible implementation of Rigid Body Dynamics algorithms and their analytical derivatives
kinematics-derivatives.hpp
1 //
2 // Copyright (c) 2017-2020 CNRS INRIA
3 //
4 
5 #ifndef __pinocchio_algorithm_kinematics_derivatives_hpp__
6 #define __pinocchio_algorithm_kinematics_derivatives_hpp__
7 
8 #include "pinocchio/multibody/model.hpp"
9 #include "pinocchio/multibody/data.hpp"
10 
11 #include "pinocchio/algorithm/jacobian.hpp"
12 
13 namespace pinocchio
14 {
15 
36  template<
37  typename Scalar,
38  int Options,
39  template<typename, int> class JointCollectionTpl,
40  typename ConfigVectorType,
41  typename TangentVectorType1,
42  typename TangentVectorType2>
43  void computeForwardKinematicsDerivatives(
44  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
45  DataTpl<Scalar, Options, JointCollectionTpl> & data,
46  const Eigen::MatrixBase<ConfigVectorType> & q,
47  const Eigen::MatrixBase<TangentVectorType1> & v,
48  const Eigen::MatrixBase<TangentVectorType2> & a);
49 
66  template<
67  typename Scalar,
68  int Options,
69  template<typename, int> class JointCollectionTpl,
70  typename Matrix6xOut1,
71  typename Matrix6xOut2>
72  void getJointVelocityDerivatives(
73  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
74  const DataTpl<Scalar, Options, JointCollectionTpl> & data,
75  const Model::JointIndex jointId,
76  const ReferenceFrame rf,
77  const Eigen::MatrixBase<Matrix6xOut1> & v_partial_dq,
78  const Eigen::MatrixBase<Matrix6xOut2> & v_partial_dv);
79 
106  template<
107  typename Scalar,
108  int Options,
109  template<typename, int> class JointCollectionTpl,
110  typename Matrix6xOut1,
111  typename Matrix6xOut2,
112  typename Matrix6xOut3,
113  typename Matrix6xOut4>
114  void getJointAccelerationDerivatives(
115  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
116  const DataTpl<Scalar, Options, JointCollectionTpl> & data,
117  const Model::JointIndex jointId,
118  const ReferenceFrame rf,
119  const Eigen::MatrixBase<Matrix6xOut1> & v_partial_dq,
120  const Eigen::MatrixBase<Matrix6xOut2> & a_partial_dq,
121  const Eigen::MatrixBase<Matrix6xOut3> & a_partial_dv,
122  const Eigen::MatrixBase<Matrix6xOut4> & a_partial_da);
123 
152  template<
153  typename Scalar,
154  int Options,
155  template<typename, int> class JointCollectionTpl,
156  typename Matrix6xOut1,
157  typename Matrix6xOut2,
158  typename Matrix6xOut3,
159  typename Matrix6xOut4,
160  typename Matrix6xOut5>
161  void getJointAccelerationDerivatives(
162  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
163  const DataTpl<Scalar, Options, JointCollectionTpl> & data,
164  const Model::JointIndex jointId,
165  const ReferenceFrame rf,
166  const Eigen::MatrixBase<Matrix6xOut1> & v_partial_dq,
167  const Eigen::MatrixBase<Matrix6xOut2> & v_partial_dv,
168  const Eigen::MatrixBase<Matrix6xOut3> & a_partial_dq,
169  const Eigen::MatrixBase<Matrix6xOut4> & a_partial_dv,
170  const Eigen::MatrixBase<Matrix6xOut5> & a_partial_da);
171 
191  template<
192  typename Scalar,
193  int Options,
194  template<typename, int> class JointCollectionTpl,
195  typename Matrix3xOut1,
196  typename Matrix3xOut2>
197  void getPointVelocityDerivatives(
198  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
199  const DataTpl<Scalar, Options, JointCollectionTpl> & data,
200  const Model::JointIndex joint_id,
201  const SE3Tpl<Scalar, Options> & placement,
202  const ReferenceFrame rf,
203  const Eigen::MatrixBase<Matrix3xOut1> & v_point_partial_dq,
204  const Eigen::MatrixBase<Matrix3xOut2> & v_point_partial_dv);
205 
234  template<
235  typename Scalar,
236  int Options,
237  template<typename, int> class JointCollectionTpl,
238  typename Matrix3xOut1,
239  typename Matrix3xOut2,
240  typename Matrix3xOut3,
241  typename Matrix3xOut4>
242  void getPointClassicAccelerationDerivatives(
243  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
244  const DataTpl<Scalar, Options, JointCollectionTpl> & data,
245  const Model::JointIndex joint_id,
246  const SE3Tpl<Scalar, Options> & placement,
247  const ReferenceFrame rf,
248  const Eigen::MatrixBase<Matrix3xOut1> & v_point_partial_dq,
249  const Eigen::MatrixBase<Matrix3xOut2> & a_point_partial_dq,
250  const Eigen::MatrixBase<Matrix3xOut3> & a_point_partial_dv,
251  const Eigen::MatrixBase<Matrix3xOut4> & a_point_partial_da);
252 
282  template<
283  typename Scalar,
284  int Options,
285  template<typename, int> class JointCollectionTpl,
286  typename Matrix3xOut1,
287  typename Matrix3xOut2,
288  typename Matrix3xOut3,
289  typename Matrix3xOut4,
290  typename Matrix3xOut5>
291  void getPointClassicAccelerationDerivatives(
292  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
293  const DataTpl<Scalar, Options, JointCollectionTpl> & data,
294  const Model::JointIndex joint_id,
295  const SE3Tpl<Scalar, Options> & placement,
296  const ReferenceFrame rf,
297  const Eigen::MatrixBase<Matrix3xOut1> & v_point_partial_dq,
298  const Eigen::MatrixBase<Matrix3xOut2> & v_point_partial_dv,
299  const Eigen::MatrixBase<Matrix3xOut3> & a_point_partial_dq,
300  const Eigen::MatrixBase<Matrix3xOut4> & a_point_partial_dv,
301  const Eigen::MatrixBase<Matrix3xOut5> & a_point_partial_da);
302 
318  template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
319  void computeJointKinematicHessians(
320  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
321  DataTpl<Scalar, Options, JointCollectionTpl> & data);
322 
339  template<
340  typename Scalar,
341  int Options,
342  template<typename, int> class JointCollectionTpl,
343  typename ConfigVectorType>
344  void computeJointKinematicHessians(
345  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
346  DataTpl<Scalar, Options, JointCollectionTpl> & data,
347  const Eigen::MatrixBase<ConfigVectorType> & q)
348  {
349  computeJointJacobians(model, data, q);
350  computeJointKinematicHessians(model, data);
351  }
352 
379  template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
380  void getJointKinematicHessian(
381  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
382  const DataTpl<Scalar, Options, JointCollectionTpl> & data,
383  const Model::JointIndex joint_id,
384  const ReferenceFrame rf,
385  Tensor<Scalar, 3, Options> & kinematic_hessian);
386 
415  template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
416  Tensor<Scalar, 3, Options> getJointKinematicHessian(
417  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
418  const DataTpl<Scalar, Options, JointCollectionTpl> & data,
419  const Model::JointIndex joint_id,
420  const ReferenceFrame rf)
421  {
422  typedef Tensor<Scalar, 3, Options> ReturnType;
423  ReturnType res(6, model.nv, model.nv);
424  res.setZero();
425  getJointKinematicHessian(model, data, joint_id, rf, res);
426  return res;
427  }
428 
429 } // namespace pinocchio
430 
431 #include "pinocchio/algorithm/kinematics-derivatives.hxx"
432 
433 #if PINOCCHIO_ENABLE_TEMPLATE_INSTANTIATION
434  #include "pinocchio/algorithm/kinematics-derivatives.txx"
435 #endif // PINOCCHIO_ENABLE_TEMPLATE_INSTANTIATION
436 
437 #endif // ifndef __pinocchio_algorithm_kinematics_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::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::computeForwardKinematicsDerivatives
void computeForwardKinematicsDerivatives(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, const Eigen::MatrixBase< ConfigVectorType > &q, const Eigen::MatrixBase< TangentVectorType1 > &v, const Eigen::MatrixBase< TangentVectorType2 > &a)
Computes all the terms required to compute the derivatives of the placement, spatial velocity and acc...
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...