kinematics-derivatives.hpp
Go to the documentation of this file.
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>
40  class JointCollectionTpl,
41  typename ConfigVectorType,
42  typename TangentVectorType1,
43  typename TangentVectorType2>
44  void computeForwardKinematicsDerivatives(
45  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
46  DataTpl<Scalar, Options, JointCollectionTpl> & data,
47  const Eigen::MatrixBase<ConfigVectorType> & q,
48  const Eigen::MatrixBase<TangentVectorType1> & v,
49  const Eigen::MatrixBase<TangentVectorType2> & a);
50 
67  template<
68  typename Scalar,
69  int Options,
70  template<typename, int>
71  class JointCollectionTpl,
72  typename Matrix6xOut1,
73  typename Matrix6xOut2>
74  void getJointVelocityDerivatives(
75  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
76  const DataTpl<Scalar, Options, JointCollectionTpl> & data,
77  const Model::JointIndex jointId,
78  const ReferenceFrame rf,
79  const Eigen::MatrixBase<Matrix6xOut1> & v_partial_dq,
80  const Eigen::MatrixBase<Matrix6xOut2> & v_partial_dv);
81 
108  template<
109  typename Scalar,
110  int Options,
111  template<typename, int>
112  class JointCollectionTpl,
113  typename Matrix6xOut1,
114  typename Matrix6xOut2,
115  typename Matrix6xOut3,
116  typename Matrix6xOut4>
117  void getJointAccelerationDerivatives(
118  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
119  const DataTpl<Scalar, Options, JointCollectionTpl> & data,
120  const Model::JointIndex jointId,
121  const ReferenceFrame rf,
122  const Eigen::MatrixBase<Matrix6xOut1> & v_partial_dq,
123  const Eigen::MatrixBase<Matrix6xOut2> & a_partial_dq,
124  const Eigen::MatrixBase<Matrix6xOut3> & a_partial_dv,
125  const Eigen::MatrixBase<Matrix6xOut4> & a_partial_da);
126 
155  template<
156  typename Scalar,
157  int Options,
158  template<typename, int>
159  class JointCollectionTpl,
160  typename Matrix6xOut1,
161  typename Matrix6xOut2,
162  typename Matrix6xOut3,
163  typename Matrix6xOut4,
164  typename Matrix6xOut5>
165  void getJointAccelerationDerivatives(
166  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
167  const DataTpl<Scalar, Options, JointCollectionTpl> & data,
168  const Model::JointIndex jointId,
169  const ReferenceFrame rf,
170  const Eigen::MatrixBase<Matrix6xOut1> & v_partial_dq,
171  const Eigen::MatrixBase<Matrix6xOut2> & v_partial_dv,
172  const Eigen::MatrixBase<Matrix6xOut3> & a_partial_dq,
173  const Eigen::MatrixBase<Matrix6xOut4> & a_partial_dv,
174  const Eigen::MatrixBase<Matrix6xOut5> & a_partial_da);
175 
195  template<
196  typename Scalar,
197  int Options,
198  template<typename, int>
199  class JointCollectionTpl,
200  typename Matrix3xOut1,
201  typename Matrix3xOut2>
202  void getPointVelocityDerivatives(
203  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
204  const DataTpl<Scalar, Options, JointCollectionTpl> & data,
205  const Model::JointIndex joint_id,
206  const SE3Tpl<Scalar, Options> & placement,
207  const ReferenceFrame rf,
208  const Eigen::MatrixBase<Matrix3xOut1> & v_point_partial_dq,
209  const Eigen::MatrixBase<Matrix3xOut2> & v_point_partial_dv);
210 
239  template<
240  typename Scalar,
241  int Options,
242  template<typename, int>
243  class JointCollectionTpl,
244  typename Matrix3xOut1,
245  typename Matrix3xOut2,
246  typename Matrix3xOut3,
247  typename Matrix3xOut4>
248  void getPointClassicAccelerationDerivatives(
249  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
250  const DataTpl<Scalar, Options, JointCollectionTpl> & data,
251  const Model::JointIndex joint_id,
252  const SE3Tpl<Scalar, Options> & placement,
253  const ReferenceFrame rf,
254  const Eigen::MatrixBase<Matrix3xOut1> & v_point_partial_dq,
255  const Eigen::MatrixBase<Matrix3xOut2> & a_point_partial_dq,
256  const Eigen::MatrixBase<Matrix3xOut3> & a_point_partial_dv,
257  const Eigen::MatrixBase<Matrix3xOut4> & a_point_partial_da);
258 
288  template<
289  typename Scalar,
290  int Options,
291  template<typename, int>
292  class JointCollectionTpl,
293  typename Matrix3xOut1,
294  typename Matrix3xOut2,
295  typename Matrix3xOut3,
296  typename Matrix3xOut4,
297  typename Matrix3xOut5>
298  void getPointClassicAccelerationDerivatives(
299  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
300  const DataTpl<Scalar, Options, JointCollectionTpl> & data,
301  const Model::JointIndex joint_id,
302  const SE3Tpl<Scalar, Options> & placement,
303  const ReferenceFrame rf,
304  const Eigen::MatrixBase<Matrix3xOut1> & v_point_partial_dq,
305  const Eigen::MatrixBase<Matrix3xOut2> & v_point_partial_dv,
306  const Eigen::MatrixBase<Matrix3xOut3> & a_point_partial_dq,
307  const Eigen::MatrixBase<Matrix3xOut4> & a_point_partial_dv,
308  const Eigen::MatrixBase<Matrix3xOut5> & a_point_partial_da);
309 
325  template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
326  void computeJointKinematicHessians(
327  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
328  DataTpl<Scalar, Options, JointCollectionTpl> & data);
329 
346  template<
347  typename Scalar,
348  int Options,
349  template<typename, int>
350  class JointCollectionTpl,
351  typename ConfigVectorType>
352  void computeJointKinematicHessians(
353  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
354  DataTpl<Scalar, Options, JointCollectionTpl> & data,
355  const Eigen::MatrixBase<ConfigVectorType> & q)
356  {
357  computeJointJacobians(model, data, q);
358  computeJointKinematicHessians(model, data);
359  }
360 
387  template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
388  void getJointKinematicHessian(
389  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
390  const DataTpl<Scalar, Options, JointCollectionTpl> & data,
391  const Model::JointIndex joint_id,
392  const ReferenceFrame rf,
393  Tensor<Scalar, 3, Options> & kinematic_hessian);
394 
423  template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
424  Tensor<Scalar, 3, Options> getJointKinematicHessian(
425  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
426  const DataTpl<Scalar, Options, JointCollectionTpl> & data,
427  const Model::JointIndex joint_id,
428  const ReferenceFrame rf)
429  {
430  typedef Tensor<Scalar, 3, Options> ReturnType;
431  ReturnType res(6, model.nv, model.nv);
432  res.setZero();
433  getJointKinematicHessian(model, data, joint_id, rf, res);
434  return res;
435  }
436 
437 } // namespace pinocchio
438 
439 #include "pinocchio/algorithm/kinematics-derivatives.hxx"
440 
441 #if PINOCCHIO_ENABLE_TEMPLATE_INSTANTIATION
442  #include "pinocchio/algorithm/kinematics-derivatives.txx"
443 #endif // PINOCCHIO_ENABLE_TEMPLATE_INSTANTIATION
444 
445 #endif // ifndef __pinocchio_algorithm_kinematics_derivatives_hpp__
pinocchio::DataTpl
Definition: context/generic.hpp:25
pinocchio::Options
Options
Definition: joint-configuration.hpp:1116
pinocchio::ReferenceFrame
ReferenceFrame
Various conventions to express the velocity of a moving frame.
Definition: multibody/fwd.hpp:46
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...
model.hpp
setup.data
data
Definition: cmake/cython/setup.in.py:48
pinocchio::python::Scalar
context::Scalar Scalar
Definition: admm-solver.cpp:29
pinocchio::res
ReturnType res
Definition: spatial/classic-acceleration.hpp:57
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::ModelTpl::JointIndex
pinocchio::JointIndex JointIndex
Definition: multibody/model.hpp:68
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::placement
const MotionDense< Motion2 > const SE3Tpl< SE3Scalar, SE3Options > & placement
Definition: spatial/classic-acceleration.hpp:122
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...
data.hpp
pinocchio::q
JointCollectionTpl const Eigen::MatrixBase< ConfigVectorType > & q
Definition: joint-configuration.hpp:1117
a
Vec3f a
pinocchio::v
JointCollectionTpl const Eigen::MatrixBase< ConfigVectorType > const Eigen::MatrixBase< TangentVectorType > & v
Definition: joint-configuration.hpp:1118
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::Tensor< Scalar, 3, Options >
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....
append-urdf-model-with-another-model.joint_id
joint_id
Definition: append-urdf-model-with-another-model.py:34
jacobian.hpp
pinocchio::ModelTpl
Definition: context/generic.hpp:20
pinocchio::model
JointCollectionTpl & model
Definition: joint-configuration.hpp:1116
pinocchio
Main pinocchio namespace.
Definition: timings.cpp:27

pinocchio
Author(s):
autogenerated on Sat Jun 22 2024 02:41:48