6#include <pinocchio/multibody/model.hpp>
7#include <pinocchio/multibody/frame.hpp>
11template <
typename Scalar>
struct FramePlacementDataTpl;
13template <
typename _Scalar>
16 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
21 using Model = pinocchio::ModelTpl<Scalar>;
23 using SE3 = pinocchio::SE3Tpl<Scalar>;
24 using Data = FramePlacementDataTpl<Scalar>;
29 const shared_ptr<Model> &model,
const SE3 &frame,
30 const pinocchio::FrameIndex frame_id)
47 return allocate_shared_eigen_aligned<Data>(*
this);
55template <
typename Scalar>
57 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
58 using Base = StageFunctionDataTpl<Scalar>;
59 using PinData = pinocchio::DataTpl<Scalar>;
60 using SE3 = pinocchio::SE3Tpl<Scalar>;
67 typename math_types<Scalar>::Matrix6s
rJf_;
69 typename math_types<Scalar>::Matrix6Xs
fJf_;
76#include "aligator/modelling/multibody/frame-placement.hxx"
78#ifdef ALIGATOR_ENABLE_TEMPLATE_INSTANTIATION
79#include "aligator/modelling/multibody/frame-placement.txx"
pinocchio::DataTpl< Scalar > PinData
FramePlacementDataTpl(const FramePlacementResidualTpl< Scalar > &model)
math_types< Scalar >::Matrix6s rJf_
Jacobian of the error.
math_types< Scalar >::Matrix6Xs fJf_
Jacobian of the error, local frame.
SE3 rMf_
Placement error of the frame.
PinData pin_data_
Pinocchio data object.
pinocchio::SE3Tpl< Scalar > SE3
void setReference(const SE3 &p_new)
ALIGATOR_DYNAMIC_TYPEDEFS(Scalar)
void evaluate(const ConstVectorRef &x, BaseData &data) const
void computeJacobians(const ConstVectorRef &x, BaseData &data) const
FramePlacementDataTpl< Scalar > Data
ALIGATOR_UNARY_FUNCTION_INTERFACE(Scalar)
shared_ptr< ManifoldAbstractTpl< Scalar > > ManifoldPtr
pinocchio::ModelTpl< Scalar > Model
shared_ptr< Model > pin_model_
shared_ptr< BaseData > createData() const
Instantiate a Data object.
FramePlacementResidualTpl(const int ndx, const int nu, const shared_ptr< Model > &model, const SE3 &frame, const pinocchio::FrameIndex frame_id)
const SE3 & getReference() const
pinocchio::SE3Tpl< Scalar > SE3
const int nu
Control dimension.
Represents unary functions of the form , with no control (or next-state) arguments.
pinocchio::FrameIndex pin_frame_id_