5#ifndef __pinocchio_algorithm_frames_hpp__
6#define __pinocchio_algorithm_frames_hpp__
8#include "pinocchio/multibody/model.hpp"
9#include "pinocchio/multibody/data.hpp"
24 template<
typename Scalar,
int Options,
template<
typename,
int>
class JointCollectionTpl>
40 template<
typename Scalar,
int Options,
template<
typename,
int>
class JointCollectionTpl>
60 template<
typename,
int>
class JointCollectionTpl,
61 typename ConfigVectorType>
65 const Eigen::MatrixBase<ConfigVectorType> & q);
83 template<
typename Scalar,
int Options,
template<
typename,
int>
class JointCollectionTpl>
87 const JointIndex joint_id,
106 template<
typename Scalar,
int Options,
template<
typename,
int>
class JointCollectionTpl>
135 template<
typename Scalar,
int Options,
template<
typename,
int>
class JointCollectionTpl>
139 const JointIndex joint_id,
164 template<
typename Scalar,
int Options,
template<
typename,
int>
class JointCollectionTpl>
193 template<
typename Scalar,
int Options,
template<
typename,
int>
class JointCollectionTpl>
197 const JointIndex joint_id,
224 template<
typename Scalar,
int Options,
template<
typename,
int>
class JointCollectionTpl>
267 template<
typename,
int>
class JointCollectionTpl,
268 typename Matrix6xLike>
272 const JointIndex joint_id,
275 const Eigen::MatrixBase<Matrix6xLike> & J);
301 template<
typename Scalar,
int Options,
template<
typename,
int>
class JointCollectionTpl>
305 const JointIndex joint_id,
309 typedef Eigen::Matrix<Scalar, 6, Eigen::Dynamic, Options> Matrix6x;
310 Matrix6x res(Matrix6x::Zero(6, model.nv));
346 template<
typename,
int>
class JointCollectionTpl,
347 typename Matrix6xLike>
353 const Eigen::MatrixBase<Matrix6xLike> & J)
355 PINOCCHIO_CHECK_INPUT_ARGUMENT(
356 frame_id < (FrameIndex)model.nframes,
"The index of the Frame is outside the bounds.");
393 template<
typename Scalar,
int Options,
template<
typename,
int>
class JointCollectionTpl>
400 typedef Eigen::Matrix<Scalar, 6, Eigen::Dynamic, Options> Matrix6x;
401 Matrix6x res(Matrix6x::Zero(6, model.nv));
434 template<
typename,
int>
class JointCollectionTpl,
435 typename ConfigVectorType,
436 typename Matrix6xLike>
440 const Eigen::MatrixBase<ConfigVectorType> & q,
443 const Eigen::MatrixBase<Matrix6xLike> & J);
472 template<
typename,
int>
class JointCollectionTpl,
473 typename ConfigVectorType,
478 const Eigen::MatrixBase<ConfigVectorType> & q,
480 const Eigen::MatrixBase<Matrix6xLike> & J)
507 template<
typename,
int>
class JointCollectionTpl,
508 typename Matrix6xLike>
514 const Eigen::MatrixBase<Matrix6xLike> & dJ);
549 template<
typename Scalar,
int Options,
template<
typename,
int>
class JointCollectionTpl>
589 template<
typename Scalar,
int Options,
template<
typename,
int>
class JointCollectionTpl>
598#include "pinocchio/algorithm/frames.hxx"
600#if PINOCCHIO_ENABLE_TEMPLATE_INSTANTIATION
601 #include "pinocchio/algorithm/frames.txx"
ReferenceFrame
Various conventions to express the velocity of a moving frame.
Main pinocchio namespace.
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.
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....
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,...
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 ...
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....
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....
void updateFramePlacements(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data)
Updates the position of each frame contained in the model.
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.
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....
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....
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....