5 #ifndef __pinocchio_algorithm_frames_derivatives_hpp__
6 #define __pinocchio_algorithm_frames_derivatives_hpp__
8 #include "pinocchio/multibody/model.hpp"
9 #include "pinocchio/multibody/data.hpp"
37 template<
typename,
int>
38 class JointCollectionTpl,
39 typename Matrix6xOut1,
40 typename Matrix6xOut2>
44 const JointIndex joint_id,
47 const Eigen::MatrixBase<Matrix6xOut1> & v_partial_dq,
48 const Eigen::MatrixBase<Matrix6xOut2> & v_partial_dv);
72 template<
typename,
int>
73 class JointCollectionTpl,
74 typename Matrix6xOut1,
75 typename Matrix6xOut2>
79 const FrameIndex frame_id,
81 const Eigen::MatrixBase<Matrix6xOut1> & v_partial_dq,
82 const Eigen::MatrixBase<Matrix6xOut2> & v_partial_dv)
84 PINOCCHIO_CHECK_INPUT_ARGUMENT((
int)frame_id < model.nframes,
"The frame_id is not valid.");
89 const Frame & frame = model.frames[frame_id];
90 typename Data::SE3 & oMframe = data.oMf[frame_id];
94 PINOCCHIO_EIGEN_CONST_CAST(Matrix6xOut1, v_partial_dq),
95 PINOCCHIO_EIGEN_CONST_CAST(Matrix6xOut1, v_partial_dv));
132 template<
typename,
int>
133 class JointCollectionTpl,
134 typename Matrix6xOut1,
135 typename Matrix6xOut2,
136 typename Matrix6xOut3,
137 typename Matrix6xOut4>
141 const JointIndex joint_id,
144 const Eigen::MatrixBase<Matrix6xOut1> & v_partial_dq,
145 const Eigen::MatrixBase<Matrix6xOut2> & a_partial_dq,
146 const Eigen::MatrixBase<Matrix6xOut3> & a_partial_dv,
147 const Eigen::MatrixBase<Matrix6xOut4> & a_partial_da);
179 template<
typename,
int>
180 class JointCollectionTpl,
181 typename Matrix6xOut1,
182 typename Matrix6xOut2,
183 typename Matrix6xOut3,
184 typename Matrix6xOut4>
188 const FrameIndex frame_id,
190 const Eigen::MatrixBase<Matrix6xOut1> & v_partial_dq,
191 const Eigen::MatrixBase<Matrix6xOut2> & a_partial_dq,
192 const Eigen::MatrixBase<Matrix6xOut3> & a_partial_dv,
193 const Eigen::MatrixBase<Matrix6xOut4> & a_partial_da)
195 PINOCCHIO_CHECK_INPUT_ARGUMENT((
int)frame_id < model.nframes,
"The frame_id is not valid.");
200 const Frame & frame = model.frames[frame_id];
201 typename Data::SE3 & oMframe = data.oMf[frame_id];
205 PINOCCHIO_EIGEN_CONST_CAST(Matrix6xOut1, v_partial_dq),
206 PINOCCHIO_EIGEN_CONST_CAST(Matrix6xOut2, a_partial_dq),
207 PINOCCHIO_EIGEN_CONST_CAST(Matrix6xOut3, a_partial_dv),
208 PINOCCHIO_EIGEN_CONST_CAST(Matrix6xOut4, a_partial_da));
247 template<
typename,
int>
248 class JointCollectionTpl,
249 typename Matrix6xOut1,
250 typename Matrix6xOut2,
251 typename Matrix6xOut3,
252 typename Matrix6xOut4,
253 typename Matrix6xOut5>
257 const JointIndex joint_id,
260 const Eigen::MatrixBase<Matrix6xOut1> & v_partial_dq,
261 const Eigen::MatrixBase<Matrix6xOut2> & v_partial_dv,
262 const Eigen::MatrixBase<Matrix6xOut3> & a_partial_dq,
263 const Eigen::MatrixBase<Matrix6xOut4> & a_partial_dv,
264 const Eigen::MatrixBase<Matrix6xOut5> & a_partial_da)
267 model, data, joint_id, placement, rf, PINOCCHIO_EIGEN_CONST_CAST(Matrix6xOut1, v_partial_dq),
268 PINOCCHIO_EIGEN_CONST_CAST(Matrix6xOut3, a_partial_dq),
269 PINOCCHIO_EIGEN_CONST_CAST(Matrix6xOut4, a_partial_dv),
270 PINOCCHIO_EIGEN_CONST_CAST(Matrix6xOut5, a_partial_da));
272 PINOCCHIO_EIGEN_CONST_CAST(Matrix6xOut2, v_partial_dv) = a_partial_da;
308 template<
typename,
int>
309 class JointCollectionTpl,
310 typename Matrix6xOut1,
311 typename Matrix6xOut2,
312 typename Matrix6xOut3,
313 typename Matrix6xOut4,
314 typename Matrix6xOut5>
318 const FrameIndex frame_id,
320 const Eigen::MatrixBase<Matrix6xOut1> & v_partial_dq,
321 const Eigen::MatrixBase<Matrix6xOut2> & v_partial_dv,
322 const Eigen::MatrixBase<Matrix6xOut3> & a_partial_dq,
323 const Eigen::MatrixBase<Matrix6xOut4> & a_partial_dv,
324 const Eigen::MatrixBase<Matrix6xOut5> & a_partial_da)
326 PINOCCHIO_CHECK_INPUT_ARGUMENT((
int)frame_id < model.nframes,
"The frame_id is not valid.");
331 const Frame & frame = model.frames[frame_id];
332 typename Data::SE3 & oMframe = data.oMf[frame_id];
336 PINOCCHIO_EIGEN_CONST_CAST(Matrix6xOut1, v_partial_dq),
337 PINOCCHIO_EIGEN_CONST_CAST(Matrix6xOut2, v_partial_dv),
338 PINOCCHIO_EIGEN_CONST_CAST(Matrix6xOut3, a_partial_dq),
339 PINOCCHIO_EIGEN_CONST_CAST(Matrix6xOut4, a_partial_dv),
340 PINOCCHIO_EIGEN_CONST_CAST(Matrix6xOut5, a_partial_da));
344 #include "pinocchio/algorithm/frames-derivatives.hxx"
346 #if PINOCCHIO_ENABLE_TEMPLATE_INSTANTIATION
347 #include "pinocchio/algorithm/frames-derivatives.txx"
ReferenceFrame
Various conventions to express the velocity of a moving frame.
Main pinocchio namespace.
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...
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,...
A Plucker coordinate frame attached to a parent joint inside a kinematic tree.
JointIndex parentJoint
Index of the parent joint.
SE3 placement
Position of kinematic element in parent joint frame.