pinocchio  2.4.4
A fast and flexible implementation of Rigid Body Dynamics algorithms and their analytical derivatives
kinematics-derivatives.hpp
1 //
2 // Copyright (c) 2017-2019 CNRS INRIA
3 //
4 
5 #ifndef __pinocchio_kinematics_derivatives_hpp__
6 #define __pinocchio_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 
33  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType, typename TangentVectorType1, typename TangentVectorType2>
34  inline void computeForwardKinematicsDerivatives(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
35  DataTpl<Scalar,Options,JointCollectionTpl> & data,
36  const Eigen::MatrixBase<ConfigVectorType> & q,
37  const Eigen::MatrixBase<TangentVectorType1> & v,
38  const Eigen::MatrixBase<TangentVectorType2> & a);
39 
55  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename Matrix6xOut1, typename Matrix6xOut2>
56  inline void getJointVelocityDerivatives(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
57  DataTpl<Scalar,Options,JointCollectionTpl> & data,
58  const Model::JointIndex jointId,
59  const ReferenceFrame rf,
60  const Eigen::MatrixBase<Matrix6xOut1> & v_partial_dq,
61  const Eigen::MatrixBase<Matrix6xOut2> & v_partial_dv);
62 
84  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename Matrix6xOut1, typename Matrix6xOut2, typename Matrix6xOut3, typename Matrix6xOut4>
85  inline void getJointAccelerationDerivatives(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
86  DataTpl<Scalar,Options,JointCollectionTpl> & data,
87  const Model::JointIndex jointId,
88  const ReferenceFrame rf,
89  const Eigen::MatrixBase<Matrix6xOut1> & v_partial_dq,
90  const Eigen::MatrixBase<Matrix6xOut2> & a_partial_dq,
91  const Eigen::MatrixBase<Matrix6xOut3> & a_partial_dv,
92  const Eigen::MatrixBase<Matrix6xOut4> & a_partial_da);
93 
117  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename Matrix6xOut1, typename Matrix6xOut2, typename Matrix6xOut3, typename Matrix6xOut4, typename Matrix6xOut5>
118  inline void getJointAccelerationDerivatives(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
119  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> & v_partial_dv,
124  const Eigen::MatrixBase<Matrix6xOut3> & a_partial_dq,
125  const Eigen::MatrixBase<Matrix6xOut4> & a_partial_dv,
126  const Eigen::MatrixBase<Matrix6xOut5> & a_partial_da);
127 
142  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
143  inline void
144  computeJointKinematicHessians(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
145  DataTpl<Scalar,Options,JointCollectionTpl> & data);
146 
162  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType>
163  inline void
166  const Eigen::MatrixBase<ConfigVectorType> & q)
167  {
168  computeJointJacobians(model,data,q);
169  computeJointKinematicHessians(model,data);
170  }
171 
192  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
193  inline void
196  const Model::JointIndex joint_id,
197  const ReferenceFrame rf,
198  Tensor<Scalar,3,Options> & kinematic_hessian);
199 
221  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
225  const Model::JointIndex joint_id,
226  const ReferenceFrame rf)
227  {
228  typedef Tensor<Scalar,3,Options> ReturnType;
229  ReturnType res(6,model.nv,model.nv); res.setZero();
230  getJointKinematicHessian(model,data,joint_id,rf,res);
231  return res;
232  }
233 
234 } // namespace pinocchio
235 
236 #include "pinocchio/algorithm/kinematics-derivatives.hxx"
237 
238 #endif // ifndef __pinocchio_kinematics_derivatives_hpp__
void getJointVelocityDerivatives(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, 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...
void getJointAccelerationDerivatives(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, 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...
int nv
Dimension of the velocity vector space.
Definition: model.hpp:78
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...
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...
Main pinocchio namespace.
Definition: treeview.dox:24
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...
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...