pinocchio  3.7.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#ifndef __pinocchio_algorithm_frames_derivatives_hpp__
6#define __pinocchio_algorithm_frames_derivatives_hpp__
7
8#include "pinocchio/multibody/model.hpp"
9#include "pinocchio/multibody/data.hpp"
10
11namespace pinocchio
12{
13
34 template<
35 typename Scalar,
36 int Options,
37 template<typename, int> class JointCollectionTpl,
38 typename Matrix6xOut1,
39 typename Matrix6xOut2>
40 void getFrameVelocityDerivatives(
41 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
42 const DataTpl<Scalar, Options, JointCollectionTpl> & data,
43 const JointIndex joint_id,
44 const SE3Tpl<Scalar, Options> & placement,
45 const ReferenceFrame rf,
46 const Eigen::MatrixBase<Matrix6xOut1> & v_partial_dq,
47 const Eigen::MatrixBase<Matrix6xOut2> & v_partial_dv);
48
68 template<
69 typename Scalar,
70 int Options,
71 template<typename, int> class JointCollectionTpl,
72 typename Matrix6xOut1,
73 typename Matrix6xOut2>
74 void getFrameVelocityDerivatives(
75 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
76 DataTpl<Scalar, Options, JointCollectionTpl> & data,
77 const FrameIndex frame_id,
78 const ReferenceFrame rf,
79 const Eigen::MatrixBase<Matrix6xOut1> & v_partial_dq,
80 const Eigen::MatrixBase<Matrix6xOut2> & v_partial_dv)
81 {
82 PINOCCHIO_CHECK_INPUT_ARGUMENT((int)frame_id < model.nframes, "The frame_id is not valid.");
83 typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model;
84 typedef DataTpl<Scalar, Options, JointCollectionTpl> Data;
85 typedef typename Model::Frame Frame;
86
87 const Frame & frame = model.frames[frame_id];
88 typename Data::SE3 & oMframe = data.oMf[frame_id];
89 oMframe = data.oMi[frame.parentJoint] * frame.placement; // for backward compatibility
90 getFrameVelocityDerivatives(
91 model, data, frame.parentJoint, frame.placement, rf,
92 PINOCCHIO_EIGEN_CONST_CAST(Matrix6xOut1, v_partial_dq),
93 PINOCCHIO_EIGEN_CONST_CAST(Matrix6xOut1, v_partial_dv));
94 }
95
127 template<
128 typename Scalar,
129 int Options,
130 template<typename, int> class JointCollectionTpl,
131 typename Matrix6xOut1,
132 typename Matrix6xOut2,
133 typename Matrix6xOut3,
134 typename Matrix6xOut4>
135 void getFrameAccelerationDerivatives(
136 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
137 DataTpl<Scalar, Options, JointCollectionTpl> & data,
138 const JointIndex joint_id,
139 const SE3Tpl<Scalar, Options> & placement,
140 const ReferenceFrame rf,
141 const Eigen::MatrixBase<Matrix6xOut1> & v_partial_dq,
142 const Eigen::MatrixBase<Matrix6xOut2> & a_partial_dq,
143 const Eigen::MatrixBase<Matrix6xOut3> & a_partial_dv,
144 const Eigen::MatrixBase<Matrix6xOut4> & a_partial_da);
145
173 template<
174 typename Scalar,
175 int Options,
176 template<typename, int> class JointCollectionTpl,
177 typename Matrix6xOut1,
178 typename Matrix6xOut2,
179 typename Matrix6xOut3,
180 typename Matrix6xOut4>
181 void getFrameAccelerationDerivatives(
182 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
183 DataTpl<Scalar, Options, JointCollectionTpl> & data,
184 const FrameIndex frame_id,
185 const ReferenceFrame rf,
186 const Eigen::MatrixBase<Matrix6xOut1> & v_partial_dq,
187 const Eigen::MatrixBase<Matrix6xOut2> & a_partial_dq,
188 const Eigen::MatrixBase<Matrix6xOut3> & a_partial_dv,
189 const Eigen::MatrixBase<Matrix6xOut4> & a_partial_da)
190 {
191 PINOCCHIO_CHECK_INPUT_ARGUMENT((int)frame_id < model.nframes, "The frame_id is not valid.");
192 typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model;
193 typedef DataTpl<Scalar, Options, JointCollectionTpl> Data;
194 typedef typename Model::Frame Frame;
195
196 const Frame & frame = model.frames[frame_id];
197 typename Data::SE3 & oMframe = data.oMf[frame_id];
198 oMframe = data.oMi[frame.parentJoint] * frame.placement; // for backward compatibility
199 getFrameAccelerationDerivatives(
200 model, data, frame.parentJoint, frame.placement, rf,
201 PINOCCHIO_EIGEN_CONST_CAST(Matrix6xOut1, v_partial_dq),
202 PINOCCHIO_EIGEN_CONST_CAST(Matrix6xOut2, a_partial_dq),
203 PINOCCHIO_EIGEN_CONST_CAST(Matrix6xOut3, a_partial_dv),
204 PINOCCHIO_EIGEN_CONST_CAST(Matrix6xOut4, a_partial_da));
205 }
206
240 template<
241 typename Scalar,
242 int Options,
243 template<typename, int> class JointCollectionTpl,
244 typename Matrix6xOut1,
245 typename Matrix6xOut2,
246 typename Matrix6xOut3,
247 typename Matrix6xOut4,
248 typename Matrix6xOut5>
249 void getFrameAccelerationDerivatives(
250 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
251 DataTpl<Scalar, Options, JointCollectionTpl> & data,
252 const JointIndex joint_id,
253 const SE3Tpl<Scalar, Options> & placement,
254 const ReferenceFrame rf,
255 const Eigen::MatrixBase<Matrix6xOut1> & v_partial_dq,
256 const Eigen::MatrixBase<Matrix6xOut2> & v_partial_dv,
257 const Eigen::MatrixBase<Matrix6xOut3> & a_partial_dq,
258 const Eigen::MatrixBase<Matrix6xOut4> & a_partial_dv,
259 const Eigen::MatrixBase<Matrix6xOut5> & a_partial_da)
260 {
261 getFrameAccelerationDerivatives(
262 model, data, joint_id, placement, rf, PINOCCHIO_EIGEN_CONST_CAST(Matrix6xOut1, v_partial_dq),
263 PINOCCHIO_EIGEN_CONST_CAST(Matrix6xOut3, a_partial_dq),
264 PINOCCHIO_EIGEN_CONST_CAST(Matrix6xOut4, a_partial_dv),
265 PINOCCHIO_EIGEN_CONST_CAST(Matrix6xOut5, a_partial_da));
266
267 PINOCCHIO_EIGEN_CONST_CAST(Matrix6xOut2, v_partial_dv) = a_partial_da;
268 }
269
300 template<
301 typename Scalar,
302 int Options,
303 template<typename, int> class JointCollectionTpl,
304 typename Matrix6xOut1,
305 typename Matrix6xOut2,
306 typename Matrix6xOut3,
307 typename Matrix6xOut4,
308 typename Matrix6xOut5>
309 void getFrameAccelerationDerivatives(
310 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
311 DataTpl<Scalar, Options, JointCollectionTpl> & data,
312 const FrameIndex frame_id,
313 const ReferenceFrame rf,
314 const Eigen::MatrixBase<Matrix6xOut1> & v_partial_dq,
315 const Eigen::MatrixBase<Matrix6xOut2> & v_partial_dv,
316 const Eigen::MatrixBase<Matrix6xOut3> & a_partial_dq,
317 const Eigen::MatrixBase<Matrix6xOut4> & a_partial_dv,
318 const Eigen::MatrixBase<Matrix6xOut5> & a_partial_da)
319 {
320 PINOCCHIO_CHECK_INPUT_ARGUMENT((int)frame_id < model.nframes, "The frame_id is not valid.");
321 typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model;
322 typedef DataTpl<Scalar, Options, JointCollectionTpl> Data;
323 typedef typename Model::Frame Frame;
324
325 const Frame & frame = model.frames[frame_id];
326 typename Data::SE3 & oMframe = data.oMf[frame_id];
327 oMframe = data.oMi[frame.parentJoint] * frame.placement; // for backward compatibility
328 getFrameAccelerationDerivatives(
329 model, data, frame.parentJoint, frame.placement, rf,
330 PINOCCHIO_EIGEN_CONST_CAST(Matrix6xOut1, v_partial_dq),
331 PINOCCHIO_EIGEN_CONST_CAST(Matrix6xOut2, v_partial_dv),
332 PINOCCHIO_EIGEN_CONST_CAST(Matrix6xOut3, a_partial_dq),
333 PINOCCHIO_EIGEN_CONST_CAST(Matrix6xOut4, a_partial_dv),
334 PINOCCHIO_EIGEN_CONST_CAST(Matrix6xOut5, a_partial_da));
335 }
336} // namespace pinocchio
337
338#include "pinocchio/algorithm/frames-derivatives.hxx"
339
340#if PINOCCHIO_ENABLE_TEMPLATE_INSTANTIATION
341 #include "pinocchio/algorithm/frames-derivatives.txx"
342#endif // PINOCCHIO_ENABLE_TEMPLATE_INSTANTIATION
343
344#endif // ifndef __pinocchio_algorithm_frames_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::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...