pinocchio  3.9.0
A fast and flexible implementation of Rigid Body Dynamics algorithms and their analytical derivatives
Loading...
Searching...
No Matches
frames.hpp
1//
2// Copyright (c) 2015-2020 CNRS INRIA
3//
4
5#ifndef __pinocchio_algorithm_frames_hpp__
6#define __pinocchio_algorithm_frames_hpp__
7
8#include "pinocchio/multibody/model.hpp"
9#include "pinocchio/multibody/data.hpp"
10
11namespace pinocchio
12{
13
24 template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
25 inline void updateFramePlacements(
26 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
27 DataTpl<Scalar, Options, JointCollectionTpl> & data);
28
40 template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
41 inline const typename DataTpl<Scalar, Options, JointCollectionTpl>::SE3 & updateFramePlacement(
42 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
43 DataTpl<Scalar, Options, JointCollectionTpl> & data,
44 const FrameIndex frame_id);
45
57 template<
58 typename Scalar,
59 int Options,
60 template<typename, int> class JointCollectionTpl,
61 typename ConfigVectorType>
62 inline void framesForwardKinematics(
63 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
64 DataTpl<Scalar, Options, JointCollectionTpl> & data,
65 const Eigen::MatrixBase<ConfigVectorType> & q);
66
82
83 template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
84 inline MotionTpl<Scalar, Options> getFrameVelocity(
85 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
86 const DataTpl<Scalar, Options, JointCollectionTpl> & data,
87 const JointIndex joint_id,
88 const SE3Tpl<Scalar, Options> & placement,
89 const ReferenceFrame rf = LOCAL);
90
105
106 template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
107 inline MotionTpl<Scalar, Options> getFrameVelocity(
108 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
109 const DataTpl<Scalar, Options, JointCollectionTpl> & data,
110 const FrameIndex frame_id,
111 const ReferenceFrame rf = LOCAL)
112 {
113 typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model;
114 const typename Model::Frame & frame = model.frames[frame_id];
115
116 return getFrameVelocity(model, data, frame.parentJoint, frame.placement, rf);
117 }
118
134
135 template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
136 inline MotionTpl<Scalar, Options> getFrameAcceleration(
137 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
138 const DataTpl<Scalar, Options, JointCollectionTpl> & data,
139 const JointIndex joint_id,
140 const SE3Tpl<Scalar, Options> & placement,
141 const ReferenceFrame rf = LOCAL);
142
163
164 template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
165 inline MotionTpl<Scalar, Options> getFrameAcceleration(
166 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
167 const DataTpl<Scalar, Options, JointCollectionTpl> & data,
168 const FrameIndex frame_id,
169 const ReferenceFrame rf = LOCAL)
170 {
171 typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model;
172 const typename Model::Frame & frame = model.frames[frame_id];
173 return getFrameAcceleration(model, data, frame.parentJoint, frame.placement, rf);
174 }
175
192
193 template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
194 inline MotionTpl<Scalar, Options> getFrameClassicalAcceleration(
195 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
196 const DataTpl<Scalar, Options, JointCollectionTpl> & data,
197 const JointIndex joint_id,
198 const SE3Tpl<Scalar, Options> & placement,
199 const ReferenceFrame rf = LOCAL);
200
223
224 template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
225 inline MotionTpl<Scalar, Options> getFrameClassicalAcceleration(
226 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
227 const DataTpl<Scalar, Options, JointCollectionTpl> & data,
228 const FrameIndex frame_id,
229 const ReferenceFrame rf = LOCAL)
230 {
231 typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model;
232 const typename Model::Frame & frame = model.frames[frame_id];
233 return getFrameClassicalAcceleration(model, data, frame.parentJoint, frame.placement, rf);
234 }
235
264 template<
265 typename Scalar,
266 int Options,
267 template<typename, int> class JointCollectionTpl,
268 typename Matrix6xLike>
269 void getFrameJacobian(
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 reference_frame,
275 const Eigen::MatrixBase<Matrix6xLike> & J);
276
301 template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
302 Eigen::Matrix<Scalar, 6, Eigen::Dynamic, Options> getFrameJacobian(
303 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
304 DataTpl<Scalar, Options, JointCollectionTpl> & data,
305 const JointIndex joint_id,
306 const SE3Tpl<Scalar, Options> & placement,
307 const ReferenceFrame reference_frame)
308 {
309 typedef Eigen::Matrix<Scalar, 6, Eigen::Dynamic, Options> Matrix6x;
310 Matrix6x res(Matrix6x::Zero(6, model.nv));
311
312 getFrameJacobian(model, data, joint_id, placement, reference_frame, res);
313 return res;
314 }
315
343 template<
344 typename Scalar,
345 int Options,
346 template<typename, int> class JointCollectionTpl,
347 typename Matrix6xLike>
348 inline void getFrameJacobian(
349 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
350 DataTpl<Scalar, Options, JointCollectionTpl> & data,
351 const FrameIndex frame_id,
352 const ReferenceFrame reference_frame,
353 const Eigen::MatrixBase<Matrix6xLike> & J)
354 {
355 PINOCCHIO_CHECK_INPUT_ARGUMENT(
356 frame_id < (FrameIndex)model.nframes, "The index of the Frame is outside the bounds.");
357
358 typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model;
359 typedef typename Model::Frame Frame;
360
361 const Frame & frame = model.frames[frame_id];
362 data.oMf[frame_id] = data.oMi[frame.parentJoint] * frame.placement;
363
364 getFrameJacobian(
365 model, data, frame.parentJoint, frame.placement, reference_frame,
366 PINOCCHIO_EIGEN_CONST_CAST(Matrix6xLike, J));
367 }
368
393 template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
394 Eigen::Matrix<Scalar, 6, Eigen::Dynamic, Options> getFrameJacobian(
395 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
396 DataTpl<Scalar, Options, JointCollectionTpl> & data,
397 const FrameIndex frame_id,
398 const ReferenceFrame reference_frame)
399 {
400 typedef Eigen::Matrix<Scalar, 6, Eigen::Dynamic, Options> Matrix6x;
401 Matrix6x res(Matrix6x::Zero(6, model.nv));
402
403 getFrameJacobian(model, data, frame_id, reference_frame, res);
404 return res;
405 }
406
431 template<
432 typename Scalar,
433 int Options,
434 template<typename, int> class JointCollectionTpl,
435 typename ConfigVectorType,
436 typename Matrix6xLike>
437 inline void computeFrameJacobian(
438 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
439 DataTpl<Scalar, Options, JointCollectionTpl> & data,
440 const Eigen::MatrixBase<ConfigVectorType> & q,
441 const FrameIndex frame_id,
442 const ReferenceFrame reference_frame,
443 const Eigen::MatrixBase<Matrix6xLike> & J);
444
469 template<
470 typename Scalar,
471 int Options,
472 template<typename, int> class JointCollectionTpl,
473 typename ConfigVectorType,
474 typename Matrix6xLike>
475 inline void computeFrameJacobian(
476 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
477 DataTpl<Scalar, Options, JointCollectionTpl> & data,
478 const Eigen::MatrixBase<ConfigVectorType> & q,
479 const FrameIndex frame_id,
480 const Eigen::MatrixBase<Matrix6xLike> & J)
481 {
482 computeFrameJacobian(
483 model, data, q.derived(), frame_id, LOCAL, PINOCCHIO_EIGEN_CONST_CAST(Matrix6xLike, J));
484 }
485
504 template<
505 typename Scalar,
506 int Options,
507 template<typename, int> class JointCollectionTpl,
508 typename Matrix6xLike>
509 void getFrameJacobianTimeVariation(
510 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
511 DataTpl<Scalar, Options, JointCollectionTpl> & data,
512 const FrameIndex frame_id,
513 const ReferenceFrame rf,
514 const Eigen::MatrixBase<Matrix6xLike> & dJ);
515
549 template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
550 InertiaTpl<Scalar, Options> computeSupportedInertiaByFrame(
551 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
552 const DataTpl<Scalar, Options, JointCollectionTpl> & data,
553 const FrameIndex frame_id,
554 bool with_subtree);
555
589 template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
590 ForceTpl<Scalar, Options> computeSupportedForceByFrame(
591 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
592 const DataTpl<Scalar, Options, JointCollectionTpl> & data,
593 const FrameIndex frame_id);
594
595} // namespace pinocchio
596
597/* --- Details -------------------------------------------------------------------- */
598#include "pinocchio/algorithm/frames.hxx"
599
600#if PINOCCHIO_ENABLE_TEMPLATE_INSTANTIATION
601 #include "pinocchio/algorithm/frames.txx"
602#endif // PINOCCHIO_ENABLE_TEMPLATE_INSTANTIATION
603
604#endif // ifndef __pinocchio_algorithm_frames_hpp__
pinocchio::ReferenceFrame
ReferenceFrame
Various conventions to express the velocity of a moving frame.
Definition fwd.hpp:47
pinocchio::LOCAL
@ LOCAL
Definition fwd.hpp:50
pinocchio
Main pinocchio namespace.
Definition treeview.dox:11
pinocchio::updateFramePlacement
const DataTpl< Scalar, Options, JointCollectionTpl >::SE3 & updateFramePlacement(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, const FrameIndex frame_id)
Updates the placement of the given frame.
pinocchio::getFrameAcceleration
MotionTpl< Scalar, Options > getFrameAcceleration(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const DataTpl< Scalar, Options, JointCollectionTpl > &data, const JointIndex joint_id, const SE3Tpl< Scalar, Options > &placement, const ReferenceFrame rf=LOCAL)
Returns the spatial acceleration of the Frame expressed in the desired reference frame....
pinocchio::getFrameJacobian
void getFrameJacobian(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, const JointIndex joint_id, const SE3Tpl< Scalar, Options > &placement, const ReferenceFrame reference_frame, const Eigen::MatrixBase< Matrix6xLike > &J)
Returns the jacobian of the frame given by its relative placement w.r.t. a joint frame,...
pinocchio::getFrameJacobianTimeVariation
void getFrameJacobianTimeVariation(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, const FrameIndex frame_id, const ReferenceFrame rf, const Eigen::MatrixBase< Matrix6xLike > &dJ)
Computes the Jacobian time variation of a specific frame (given by frame_id) expressed either in the ...
pinocchio::getFrameClassicalAcceleration
MotionTpl< Scalar, Options > getFrameClassicalAcceleration(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const DataTpl< Scalar, Options, JointCollectionTpl > &data, const JointIndex joint_id, const SE3Tpl< Scalar, Options > &placement, const ReferenceFrame rf=LOCAL)
Returns the "classical" acceleration of the Frame expressed in the desired reference frame....
pinocchio::framesForwardKinematics
void framesForwardKinematics(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, const Eigen::MatrixBase< ConfigVectorType > &q)
First calls the forwardKinematics on the model, then computes the placement of each frame....
pinocchio::updateFramePlacements
void updateFramePlacements(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data)
Updates the position of each frame contained in the model.
pinocchio::computeFrameJacobian
void computeFrameJacobian(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, const Eigen::MatrixBase< ConfigVectorType > &q, const FrameIndex frame_id, const ReferenceFrame reference_frame, const Eigen::MatrixBase< Matrix6xLike > &J)
Computes the Jacobian of a specific Frame expressed in the desired reference_frame given as argument.
pinocchio::computeSupportedInertiaByFrame
InertiaTpl< Scalar, Options > computeSupportedInertiaByFrame(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const DataTpl< Scalar, Options, JointCollectionTpl > &data, const FrameIndex frame_id, bool with_subtree)
Compute the inertia supported by a specific frame (given by frame_id) expressed in the LOCAL frame....
pinocchio::computeSupportedForceByFrame
ForceTpl< Scalar, Options > computeSupportedForceByFrame(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const DataTpl< Scalar, Options, JointCollectionTpl > &data, const FrameIndex frame_id)
Computes the force supported by a specific frame (given by frame_id) expressed in the LOCAL frame....
pinocchio::getFrameVelocity
MotionTpl< Scalar, Options > getFrameVelocity(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const DataTpl< Scalar, Options, JointCollectionTpl > &data, const JointIndex joint_id, const SE3Tpl< Scalar, Options > &placement, const ReferenceFrame rf=LOCAL)
Returns the spatial velocity of the Frame expressed in the desired reference frame....
pinocchio::FrameTpl::parentJoint
JointIndex parentJoint
Index of the parent joint.
Definition model-item.hpp:28
pinocchio::FrameTpl::placement
SE3 placement
Position of kinematic element in parent joint frame.
Definition model-item.hpp:39
pinocchio::ModelTpl::frames
FrameVector frames
Vector of operational frames registered on the model.
Definition model.hpp:193