10#ifndef CROCODDYL_MULTIBODY_RESIDUALS_IMPULSE_COM_HPP_
11#define CROCODDYL_MULTIBODY_RESIDUALS_IMPULSE_COM_HPP_
13#include "crocoddyl/core/residual-base.hpp"
14#include "crocoddyl/multibody/data/impulses.hpp"
15#include "crocoddyl/multibody/fwd.hpp"
16#include "crocoddyl/multibody/impulse-base.hpp"
17#include "crocoddyl/multibody/states/multibody.hpp"
37template <
typename _Scalar>
40 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
43 typedef _Scalar Scalar;
51 typedef typename MathBase::VectorXs VectorXs;
68 virtual void calc(
const std::shared_ptr<ResidualDataAbstract>& data,
69 const Eigen::Ref<const VectorXs>& x,
70 const Eigen::Ref<const VectorXs>& u)
override;
79 virtual void calcDiff(
const std::shared_ptr<ResidualDataAbstract>& data,
80 const Eigen::Ref<const VectorXs>& x,
81 const Eigen::Ref<const VectorXs>& u)
override;
86 virtual std::shared_ptr<ResidualDataAbstract>
createData(
98 template <
typename NewScalar>
106 virtual void print(std::ostream& os)
const override;
114 std::shared_ptr<typename StateMultibody::PinocchioModel> pin_model_;
117template <
typename _Scalar>
119 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
121 typedef _Scalar Scalar;
126 typedef typename MathBase::MatrixXs MatrixXs;
127 typedef typename MathBase::Matrix3xs Matrix3xs;
129 template <
template <
typename Scalar>
class Model>
133 dvc_dq(3, model->get_state()->get_nv()),
134 ddv_dv(model->get_state()->get_nv(), model->get_state()->get_nv()) {
137 const std::shared_ptr<StateMultibody>& state =
138 std::static_pointer_cast<StateMultibody>(model->get_state());
140 pinocchio::DataTpl<Scalar>(*state->get_pinocchio().get());
146 "Invalid argument: the shared data should be derived from "
147 "DataCollectorMultibodyInImpulse");
155 std::shared_ptr<crocoddyl::ImpulseDataMultipleTpl<Scalar> >
159 pinocchio::DataTpl<Scalar>
172#include "crocoddyl/multibody/residuals/impulse-com.hxx"
Abstract class for residual models.
std::shared_ptr< StateAbstract > state_
State description.
std::size_t nu_
Control dimension.
ResidualModelImpulseCoMTpl< NewScalar > cast() const
Cast the impulse-com residual model to a different scalar type.
ResidualModelImpulseCoMTpl(std::shared_ptr< StateMultibody > state)
Initialize the impulse CoM residual model.
virtual void calcDiff(const std::shared_ptr< ResidualDataAbstract > &data, const Eigen::Ref< const VectorXs > &x, const Eigen::Ref< const VectorXs > &u) override
Compute the Jacobians of the impulse CoM residual.
virtual std::shared_ptr< ResidualDataAbstract > createData(DataCollectorAbstract *const data) override
Create the impulse CoM residual data.
virtual void calc(const std::shared_ptr< ResidualDataAbstract > &data, const Eigen::Ref< const VectorXs > &x, const Eigen::Ref< const VectorXs > &u) override
Compute the impulse CoM residual.
virtual void print(std::ostream &os) const override
Print relevant information of the impulse-com 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.
pinocchio::DataTpl< Scalar > pinocchio_internal
Pinocchio data for internal computation.
MatrixXs ddv_dv
Jacobian of the CoM velocity.
Matrix3xs dvc_dq
Jacobian of the CoM velocity.
std::shared_ptr< crocoddyl::ImpulseDataMultipleTpl< Scalar > > impulses
Impulses data.
DataCollectorAbstract * shared
Shared data allocated by the action model.
pinocchio::DataTpl< Scalar > * pinocchio
Pinocchio data.