pinocchio 4.1.0
A fast and flexible implementation of Rigid Body Dynamics algorithms and their analytical derivatives
Loading...
Searching...
No Matches
frames-derivatives.hpp
1//
2// Copyright (c) 2020 INRIA
3//
4
5#pragma once
6
7// IWYU pragma: begin_keep
8#include <Eigen/Core>
9
10#include <cassert>
11#include <cstddef>
12#include <vector>
13
14#include <boost/fusion/container/vector.hpp>
15
16#include "pinocchio/macros.hpp"
17#include "pinocchio/eigen-common.hpp"
18
19#include "pinocchio/math.hpp"
20
21#include "pinocchio/spatial.hpp"
22
23#include "pinocchio/multibody.hpp"
24#include "pinocchio/multibody/joint.hpp"
25#include "pinocchio/multibody/visitor.hpp"
26
27#include "pinocchio/algorithm/check.hpp"
28#include "pinocchio/algorithm/kinematics-derivatives.hpp"
29// IWYU pragma: end_keep
30
31namespace pinocchio
32{
33
54 template<
55 typename Scalar,
56 int Options,
57 template<typename, int> class JointCollectionTpl,
58 typename Matrix6xOut1,
59 typename Matrix6xOut2>
60 void getFrameVelocityDerivatives(
61 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
62 const DataTpl<Scalar, Options, JointCollectionTpl> & data,
63 const JointIndex joint_id,
64 const SE3Tpl<Scalar, Options> & placement,
65 const ReferenceFrame rf,
66 const Eigen::MatrixBase<Matrix6xOut1> & v_partial_dq,
67 const Eigen::MatrixBase<Matrix6xOut2> & v_partial_dv);
68
88 template<
89 typename Scalar,
90 int Options,
91 template<typename, int> class JointCollectionTpl,
92 typename Matrix6xOut1,
93 typename Matrix6xOut2>
94 void getFrameVelocityDerivatives(
95 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
96 DataTpl<Scalar, Options, JointCollectionTpl> & data,
97 const FrameIndex frame_id,
98 const ReferenceFrame rf,
99 const Eigen::MatrixBase<Matrix6xOut1> & v_partial_dq,
100 const Eigen::MatrixBase<Matrix6xOut2> & v_partial_dv)
101 {
102 PINOCCHIO_CHECK_INPUT_ARGUMENT((int)frame_id < model.nframes, "The frame_id is not valid.");
103 typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model;
104 typedef DataTpl<Scalar, Options, JointCollectionTpl> Data;
105 typedef typename Model::Frame Frame;
106
107 const Frame & frame = model.frames[frame_id];
108 typename Data::SE3 & oMframe = data.oMf[frame_id];
109 oMframe = data.oMi[frame.parentJoint] * frame.placement; // for backward compatibility
110 getFrameVelocityDerivatives(
111 model, data, frame.parentJoint, frame.placement, rf,
112 PINOCCHIO_EIGEN_CONST_CAST(Matrix6xOut1, v_partial_dq),
113 PINOCCHIO_EIGEN_CONST_CAST(Matrix6xOut1, v_partial_dv));
114 }
115
147 template<
148 typename Scalar,
149 int Options,
150 template<typename, int> class JointCollectionTpl,
151 typename Matrix6xOut1,
152 typename Matrix6xOut2,
153 typename Matrix6xOut3,
154 typename Matrix6xOut4>
155 void getFrameAccelerationDerivatives(
156 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
157 DataTpl<Scalar, Options, JointCollectionTpl> & data,
158 const JointIndex joint_id,
159 const SE3Tpl<Scalar, Options> & placement,
160 const ReferenceFrame rf,
161 const Eigen::MatrixBase<Matrix6xOut1> & v_partial_dq,
162 const Eigen::MatrixBase<Matrix6xOut2> & a_partial_dq,
163 const Eigen::MatrixBase<Matrix6xOut3> & a_partial_dv,
164 const Eigen::MatrixBase<Matrix6xOut4> & a_partial_da);
165
193 template<
194 typename Scalar,
195 int Options,
196 template<typename, int> class JointCollectionTpl,
197 typename Matrix6xOut1,
198 typename Matrix6xOut2,
199 typename Matrix6xOut3,
200 typename Matrix6xOut4>
201 void getFrameAccelerationDerivatives(
202 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
203 DataTpl<Scalar, Options, JointCollectionTpl> & data,
204 const FrameIndex frame_id,
205 const ReferenceFrame rf,
206 const Eigen::MatrixBase<Matrix6xOut1> & v_partial_dq,
207 const Eigen::MatrixBase<Matrix6xOut2> & a_partial_dq,
208 const Eigen::MatrixBase<Matrix6xOut3> & a_partial_dv,
209 const Eigen::MatrixBase<Matrix6xOut4> & a_partial_da)
210 {
211 PINOCCHIO_CHECK_INPUT_ARGUMENT((int)frame_id < model.nframes, "The frame_id is not valid.");
212 typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model;
213 typedef DataTpl<Scalar, Options, JointCollectionTpl> Data;
214 typedef typename Model::Frame Frame;
215
216 const Frame & frame = model.frames[frame_id];
217 typename Data::SE3 & oMframe = data.oMf[frame_id];
218 oMframe = data.oMi[frame.parentJoint] * frame.placement; // for backward compatibility
219 getFrameAccelerationDerivatives(
220 model, data, frame.parentJoint, frame.placement, rf,
221 PINOCCHIO_EIGEN_CONST_CAST(Matrix6xOut1, v_partial_dq),
222 PINOCCHIO_EIGEN_CONST_CAST(Matrix6xOut2, a_partial_dq),
223 PINOCCHIO_EIGEN_CONST_CAST(Matrix6xOut3, a_partial_dv),
224 PINOCCHIO_EIGEN_CONST_CAST(Matrix6xOut4, a_partial_da));
225 }
226
260 template<
261 typename Scalar,
262 int Options,
263 template<typename, int> class JointCollectionTpl,
264 typename Matrix6xOut1,
265 typename Matrix6xOut2,
266 typename Matrix6xOut3,
267 typename Matrix6xOut4,
268 typename Matrix6xOut5>
269 void getFrameAccelerationDerivatives(
270 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
271 DataTpl<Scalar, Options, JointCollectionTpl> & data,
272 const JointIndex joint_id,
273 const SE3Tpl<Scalar, Options> & placement,
274 const ReferenceFrame rf,
275 const Eigen::MatrixBase<Matrix6xOut1> & v_partial_dq,
276 const Eigen::MatrixBase<Matrix6xOut2> & v_partial_dv,
277 const Eigen::MatrixBase<Matrix6xOut3> & a_partial_dq,
278 const Eigen::MatrixBase<Matrix6xOut4> & a_partial_dv,
279 const Eigen::MatrixBase<Matrix6xOut5> & a_partial_da)
280 {
281 getFrameAccelerationDerivatives(
282 model, data, joint_id, placement, rf, PINOCCHIO_EIGEN_CONST_CAST(Matrix6xOut1, v_partial_dq),
283 PINOCCHIO_EIGEN_CONST_CAST(Matrix6xOut3, a_partial_dq),
284 PINOCCHIO_EIGEN_CONST_CAST(Matrix6xOut4, a_partial_dv),
285 PINOCCHIO_EIGEN_CONST_CAST(Matrix6xOut5, a_partial_da));
286
287 PINOCCHIO_EIGEN_CONST_CAST(Matrix6xOut2, v_partial_dv) = a_partial_da;
288 }
289
320 template<
321 typename Scalar,
322 int Options,
323 template<typename, int> class JointCollectionTpl,
324 typename Matrix6xOut1,
325 typename Matrix6xOut2,
326 typename Matrix6xOut3,
327 typename Matrix6xOut4,
328 typename Matrix6xOut5>
329 void getFrameAccelerationDerivatives(
330 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
331 DataTpl<Scalar, Options, JointCollectionTpl> & data,
332 const FrameIndex frame_id,
333 const ReferenceFrame rf,
334 const Eigen::MatrixBase<Matrix6xOut1> & v_partial_dq,
335 const Eigen::MatrixBase<Matrix6xOut2> & v_partial_dv,
336 const Eigen::MatrixBase<Matrix6xOut3> & a_partial_dq,
337 const Eigen::MatrixBase<Matrix6xOut4> & a_partial_dv,
338 const Eigen::MatrixBase<Matrix6xOut5> & a_partial_da)
339 {
340 PINOCCHIO_CHECK_INPUT_ARGUMENT((int)frame_id < model.nframes, "The frame_id is not valid.");
341 typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model;
342 typedef DataTpl<Scalar, Options, JointCollectionTpl> Data;
343 typedef typename Model::Frame Frame;
344
345 const Frame & frame = model.frames[frame_id];
346 typename Data::SE3 & oMframe = data.oMf[frame_id];
347 oMframe = data.oMi[frame.parentJoint] * frame.placement; // for backward compatibility
348 getFrameAccelerationDerivatives(
349 model, data, frame.parentJoint, frame.placement, rf,
350 PINOCCHIO_EIGEN_CONST_CAST(Matrix6xOut1, v_partial_dq),
351 PINOCCHIO_EIGEN_CONST_CAST(Matrix6xOut2, v_partial_dv),
352 PINOCCHIO_EIGEN_CONST_CAST(Matrix6xOut3, a_partial_dq),
353 PINOCCHIO_EIGEN_CONST_CAST(Matrix6xOut4, a_partial_dv),
354 PINOCCHIO_EIGEN_CONST_CAST(Matrix6xOut5, a_partial_da));
355 }
356} // namespace pinocchio
357
358// IWYU pragma: begin_exports
359#include "pinocchio/src/algorithm/frames-derivatives.hxx"
360// IWYU pragma: end_exports
pinocchio
Main pinocchio namespace.
Definition treeview.dox:11
pinocchio::getFrameVelocityDerivatives
void getFrameVelocityDerivatives(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const DataTpl< Scalar, Options, JointCollectionTpl > &data, const JointIndex joint_id, const SE3Tpl< Scalar, Options > &placement, const ReferenceFrame rf, const Eigen::MatrixBase< Matrix6xOut1 > &v_partial_dq, const Eigen::MatrixBase< Matrix6xOut2 > &v_partial_dv)
Computes the partial derivatives of the spatial velocity of a frame given by its relative placement,...
pinocchio::getFrameAccelerationDerivatives
void getFrameAccelerationDerivatives(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, const JointIndex joint_id, const SE3Tpl< Scalar, Options > &placement, 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 derivatives of the spatial acceleration of a frame given by its relative placeme...