pinocchio 4.1.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#pragma once
5
6// IWYU pragma: begin_keep
7#include <Eigen/Core>
8
9#include <cassert>
10#include <cstddef>
11#include <stdexcept>
12#include <iterator>
13#include <vector>
14
15#include "pinocchio/macros.hpp"
16#include "pinocchio/eigen-common.hpp"
17
18#include "pinocchio/math.hpp"
19
20#include "pinocchio/spatial.hpp"
21
22#include "pinocchio/multibody.hpp"
23#include "pinocchio/multibody/visitor.hpp"
24
25#include "pinocchio/algorithm/check.hpp"
26#include "pinocchio/algorithm/jacobian.hpp"
27#include "pinocchio/algorithm/kinematics.hpp"
28
29// IWYU pragma: end_keep
30
31namespace pinocchio
32{
33
44 template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
45 inline void updateFramePlacements(
46 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
47 DataTpl<Scalar, Options, JointCollectionTpl> & data);
48
60 template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
61 inline const typename DataTpl<Scalar, Options, JointCollectionTpl>::SE3 & updateFramePlacement(
62 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
63 DataTpl<Scalar, Options, JointCollectionTpl> & data,
64 const FrameIndex frame_id);
65
77 template<
78 typename Scalar,
79 int Options,
80 template<typename, int> class JointCollectionTpl,
81 typename ConfigVectorType>
82 inline void framesForwardKinematics(
83 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
84 DataTpl<Scalar, Options, JointCollectionTpl> & data,
85 const Eigen::MatrixBase<ConfigVectorType> & q);
86
102
103 template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
104 inline MotionTpl<Scalar, Options> getFrameVelocity(
105 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
106 const DataTpl<Scalar, Options, JointCollectionTpl> & data,
107 const JointIndex joint_id,
108 const SE3Tpl<Scalar, Options> & placement,
109 const ReferenceFrame rf = LOCAL);
110
125
126 template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
127 inline MotionTpl<Scalar, Options> getFrameVelocity(
128 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
129 const DataTpl<Scalar, Options, JointCollectionTpl> & data,
130 const FrameIndex frame_id,
131 const ReferenceFrame rf = LOCAL)
132 {
133 typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model;
134 const typename Model::Frame & frame = model.frames[frame_id];
135
136 return getFrameVelocity(model, data, frame.parentJoint, frame.placement, rf);
137 }
138
154
155 template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
156 inline MotionTpl<Scalar, Options> getFrameAcceleration(
157 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
158 const DataTpl<Scalar, Options, JointCollectionTpl> & data,
159 const JointIndex joint_id,
160 const SE3Tpl<Scalar, Options> & placement,
161 const ReferenceFrame rf = LOCAL);
162
183
184 template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
185 inline MotionTpl<Scalar, Options> getFrameAcceleration(
186 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
187 const DataTpl<Scalar, Options, JointCollectionTpl> & data,
188 const FrameIndex frame_id,
189 const ReferenceFrame rf = LOCAL)
190 {
191 typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model;
192 const typename Model::Frame & frame = model.frames[frame_id];
193 return getFrameAcceleration(model, data, frame.parentJoint, frame.placement, rf);
194 }
195
212
213 template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
214 inline MotionTpl<Scalar, Options> getFrameClassicalAcceleration(
215 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
216 const DataTpl<Scalar, Options, JointCollectionTpl> & data,
217 const JointIndex joint_id,
218 const SE3Tpl<Scalar, Options> & placement,
219 const ReferenceFrame rf = LOCAL);
220
243
244 template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
245 inline MotionTpl<Scalar, Options> getFrameClassicalAcceleration(
246 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
247 const DataTpl<Scalar, Options, JointCollectionTpl> & data,
248 const FrameIndex frame_id,
249 const ReferenceFrame rf = LOCAL)
250 {
251 typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model;
252 const typename Model::Frame & frame = model.frames[frame_id];
253 return getFrameClassicalAcceleration(model, data, frame.parentJoint, frame.placement, rf);
254 }
255
285 template<
286 typename Scalar,
287 int Options,
288 template<typename, int> class JointCollectionTpl,
289 typename Matrix6xLike>
290 void getFrameJacobian(
291 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
292 DataTpl<Scalar, Options, JointCollectionTpl> & data,
293 const JointIndex joint_id,
294 const SE3Tpl<Scalar, Options> & placement,
295 const ReferenceFrame reference_frame,
296 const Eigen::MatrixBase<Matrix6xLike> & J);
297
323 template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
324 Eigen::Matrix<Scalar, 6, Eigen::Dynamic, Options> getFrameJacobian(
325 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
326 DataTpl<Scalar, Options, JointCollectionTpl> & data,
327 const JointIndex joint_id,
328 const SE3Tpl<Scalar, Options> & placement,
329 const ReferenceFrame reference_frame)
330 {
331 typedef Eigen::Matrix<Scalar, 6, Eigen::Dynamic, Options> Matrix6x;
332 Matrix6x res(Matrix6x::Zero(6, model.nv));
333
334 getFrameJacobian(model, data, joint_id, placement, reference_frame, res);
335 return res;
336 }
337
365 template<
366 typename Scalar,
367 int Options,
368 template<typename, int> class JointCollectionTpl,
369 typename Matrix6xLike>
370 inline void getFrameJacobian(
371 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
372 DataTpl<Scalar, Options, JointCollectionTpl> & data,
373 const FrameIndex frame_id,
374 const ReferenceFrame reference_frame,
375 const Eigen::MatrixBase<Matrix6xLike> & J)
376 {
377 PINOCCHIO_CHECK_INPUT_ARGUMENT(
378 frame_id < (FrameIndex)model.nframes, "The index of the Frame is outside the bounds.");
379
380 typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model;
381 typedef typename Model::Frame Frame;
382
383 const Frame & frame = model.frames[frame_id];
384 data.oMf[frame_id] = data.oMi[frame.parentJoint] * frame.placement;
385
386 getFrameJacobian(
387 model, data, frame.parentJoint, frame.placement, reference_frame,
388 PINOCCHIO_EIGEN_CONST_CAST(Matrix6xLike, J));
389 }
390
415 template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
416 Eigen::Matrix<Scalar, 6, Eigen::Dynamic, Options> getFrameJacobian(
417 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
418 DataTpl<Scalar, Options, JointCollectionTpl> & data,
419 const FrameIndex frame_id,
420 const ReferenceFrame reference_frame)
421 {
422 typedef Eigen::Matrix<Scalar, 6, Eigen::Dynamic, Options> Matrix6x;
423 Matrix6x res(Matrix6x::Zero(6, model.nv));
424
425 getFrameJacobian(model, data, frame_id, reference_frame, res);
426 return res;
427 }
428
450 template<
451 typename Scalar,
452 int Options,
453 template<typename, int> class JointCollectionTpl,
454 typename ConfigVectorType,
455 typename Matrix6xLike>
456 inline void computeFrameJacobian(
457 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
458 DataTpl<Scalar, Options, JointCollectionTpl> & data,
459 const Eigen::MatrixBase<ConfigVectorType> & q,
460 const FrameIndex frame_id,
461 const ReferenceFrame reference_frame,
462 const Eigen::MatrixBase<Matrix6xLike> & J);
463
485 template<
486 typename Scalar,
487 int Options,
488 template<typename, int> class JointCollectionTpl,
489 typename ConfigVectorType,
490 typename Matrix6xLike>
491 inline void computeFrameJacobian(
492 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
493 DataTpl<Scalar, Options, JointCollectionTpl> & data,
494 const Eigen::MatrixBase<ConfigVectorType> & q,
495 const FrameIndex frame_id,
496 const Eigen::MatrixBase<Matrix6xLike> & J)
497 {
498 computeFrameJacobian(
499 model, data, q.derived(), frame_id, LOCAL, PINOCCHIO_EIGEN_CONST_CAST(Matrix6xLike, J));
500 }
501
521 template<
522 typename Scalar,
523 int Options,
524 template<typename, int> class JointCollectionTpl,
525 typename Matrix6xLike>
526 void getFrameJacobianTimeVariation(
527 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
528 DataTpl<Scalar, Options, JointCollectionTpl> & data,
529 const FrameIndex frame_id,
530 const ReferenceFrame rf,
531 const Eigen::MatrixBase<Matrix6xLike> & dJ);
532
566 template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
567 InertiaTpl<Scalar, Options> computeSupportedInertiaByFrame(
568 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
569 const DataTpl<Scalar, Options, JointCollectionTpl> & data,
570 const FrameIndex frame_id,
571 bool with_subtree);
572
606 template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
607 ForceTpl<Scalar, Options> computeSupportedForceByFrame(
608 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
609 const DataTpl<Scalar, Options, JointCollectionTpl> & data,
610 const FrameIndex frame_id);
611
612} // namespace pinocchio
613// IWYU pragma: begin_exports
614#include "pinocchio/src/algorithm/frames.hxx"
615// IWYU pragma: end_exports
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....