pinocchio  3.7.0
A fast and flexible implementation of Rigid Body Dynamics algorithms and their analytical derivatives
 
Loading...
Searching...
No Matches
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
13namespace 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::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::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::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::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::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...
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::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...