10#ifndef CROCODDYL_MULTIBODY_RESIDUALS_MOMENTUM_HPP_
11#define CROCODDYL_MULTIBODY_RESIDUALS_MOMENTUM_HPP_
13#include "crocoddyl/core/residual-base.hpp"
14#include "crocoddyl/multibody/data/multibody.hpp"
15#include "crocoddyl/multibody/fwd.hpp"
16#include "crocoddyl/multibody/states/multibody.hpp"
35template <
typename _Scalar>
39 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
42 typedef _Scalar Scalar;
50 typedef typename MathBase::Vector6s Vector6s;
51 typedef typename MathBase::VectorXs VectorXs;
52 typedef typename MathBase::Matrix6xs Matrix6xs;
63 const std::size_t nu);
74 const Vector6s& href);
84 virtual void calc(
const std::shared_ptr<ResidualDataAbstract>& data,
85 const Eigen::Ref<const VectorXs>& x,
86 const Eigen::Ref<const VectorXs>& u)
override;
95 virtual void calcDiff(
const std::shared_ptr<ResidualDataAbstract>& data,
96 const Eigen::Ref<const VectorXs>& x,
97 const Eigen::Ref<const VectorXs>& u)
override;
115 template <
typename NewScalar>
133 virtual void print(std::ostream& os)
const override;
142 std::shared_ptr<typename StateMultibody::PinocchioModel>
146template <
typename _Scalar>
149 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
151 typedef _Scalar Scalar;
155 typedef typename MathBase::Matrix6xs Matrix6xs;
157 template <
template <
typename Scalar>
class Model>
161 dhd_dq(6, model->get_state()->get_nv()),
162 dhd_dv(6, model->get_state()->get_nv()) {
171 "Invalid argument: the shared data should be derived from "
172 "DataCollectorMultibody");
194#include "crocoddyl/multibody/residuals/centroidal-momentum.hxx"
196CROCODDYL_DECLARE_EXTERN_TEMPLATE_CLASS(
198CROCODDYL_DECLARE_EXTERN_TEMPLATE_STRUCT(
Abstract class for residual models.
std::shared_ptr< StateAbstract > state_
State description.
std::size_t nu_
Control dimension.
Centroidal momentum residual.
ResidualModelCentroidalMomentumTpl(std::shared_ptr< StateMultibody > state, const Vector6s &href, const std::size_t nu)
Initialize the centroidal momentum residual model.
const Vector6s & get_reference() const
Return the reference centroidal momentum.
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 centroidal momentum residual.
void set_reference(const Vector6s &href)
Modify the reference centroidal momentum.
virtual std::shared_ptr< ResidualDataAbstract > createData(DataCollectorAbstract *const data) override
Create the centroidal momentum residual data.
ResidualModelCentroidalMomentumTpl(std::shared_ptr< StateMultibody > state, const Vector6s &href)
Initialize the centroidal momentum residual model.
ResidualModelCentroidalMomentumTpl< NewScalar > cast() const
Cast the centroidal-momentum 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 centroidal momentum residual.
virtual void print(std::ostream &os) const override
Print relevant information of the centroidal-momentum 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 dhd_dq
Jacobian of the centroidal momentum.
DataCollectorAbstract * shared
Shared data allocated by the action model.
Matrix6xs dhd_dv
Jacobian of the centroidal momentum.
pinocchio::DataTpl< Scalar > * pinocchio
Pinocchio data.