pinocchio 4.1.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
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
34namespace 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::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::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::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::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::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...