11#ifndef CROCODDYL_MULTIBODY_RESIDUALS_FRAME_TRANSLATION_HPP_
12#define CROCODDYL_MULTIBODY_RESIDUALS_FRAME_TRANSLATION_HPP_
14#include "crocoddyl/core/residual-base.hpp"
15#include "crocoddyl/multibody/data/multibody.hpp"
16#include "crocoddyl/multibody/fwd.hpp"
17#include "crocoddyl/multibody/states/multibody.hpp"
36template <
typename _Scalar>
40 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
43 typedef _Scalar Scalar;
50 typedef typename MathBase::VectorXs VectorXs;
51 typedef typename MathBase::Vector3s Vector3s;
62 const pinocchio::FrameIndex
id,
63 const Vector3s& xref,
const std::size_t nu);
75 const pinocchio::FrameIndex
id,
76 const Vector3s& xref);
86 virtual void calc(
const std::shared_ptr<ResidualDataAbstract>& data,
87 const Eigen::Ref<const VectorXs>& x,
88 const Eigen::Ref<const VectorXs>& u)
override;
97 virtual void calcDiff(
const std::shared_ptr<ResidualDataAbstract>& data,
98 const Eigen::Ref<const VectorXs>& x,
99 const Eigen::Ref<const VectorXs>& u)
override;
117 template <
typename NewScalar>
133 void set_id(
const pinocchio::FrameIndex
id);
145 virtual void print(std::ostream& os)
const override;
154 pinocchio::FrameIndex id_;
156 std::shared_ptr<typename StateMultibody::PinocchioModel>
160template <
typename _Scalar>
163 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
165 typedef _Scalar Scalar;
169 typedef typename MathBase::Matrix6xs Matrix6xs;
171 template <
template <
typename Scalar>
class Model>
174 :
Base(model, data),
fJf(6, model->get_state()->get_nv()) {
181 "Invalid argument: the shared data should be derived from "
182 "DataCollectorMultibody");
204#include "crocoddyl/multibody/residuals/frame-translation.hxx"
206CROCODDYL_DECLARE_EXTERN_TEMPLATE_CLASS(
208CROCODDYL_DECLARE_EXTERN_TEMPLATE_STRUCT(
Abstract class for residual models.
std::shared_ptr< StateAbstract > state_
State description.
std::size_t nu_
Control dimension.
Frame translation residual.
void set_reference(const Vector3s &reference)
Modify the reference frame translation reference.
const Vector3s & get_reference() const
Return the reference frame translation.
ResidualModelFrameTranslationTpl(std::shared_ptr< StateMultibody > state, const pinocchio::FrameIndex id, const Vector3s &xref)
Initialize the frame translation residual model.
void set_id(const pinocchio::FrameIndex id)
Modify the reference frame id.
virtual void calcDiff(const std::shared_ptr< ResidualDataAbstract > &data, const Eigen::Ref< const VectorXs > &x, const Eigen::Ref< const VectorXs > &u) override
Compute the derivatives of the frame translation residual.
ResidualModelFrameTranslationTpl(std::shared_ptr< StateMultibody > state, const pinocchio::FrameIndex id, const Vector3s &xref, const std::size_t nu)
Initialize the frame translation residual model.
virtual std::shared_ptr< ResidualDataAbstract > createData(DataCollectorAbstract *const data) override
Create the frame translation residual data.
pinocchio::FrameIndex get_id() const
Return the reference frame id.
ResidualModelFrameTranslationTpl< NewScalar > cast() const
Cast the frame-translation residual model to a different scalar type.
virtual void calc(const std::shared_ptr< ResidualDataAbstract > &data, const Eigen::Ref< const VectorXs > &x, const Eigen::Ref< const VectorXs > &u) override
Compute the frame translation residual.
virtual void print(std::ostream &os) const override
Print relevant information of the frame-translation residual.
State multibody representation.
MatrixXs Ru
Jacobian of the residual vector with respect the control.
MatrixXs Rx
Jacobian of the residual vector with respect the state.
DataCollectorAbstract * shared
Shared data allocated by the action model.
VectorXs r
Residual vector.
Matrix6xs fJf
Local Jacobian of the frame.
DataCollectorAbstract * shared
Shared data allocated by the action model.
pinocchio::DataTpl< Scalar > * pinocchio
Pinocchio data.