Crocoddyl
 
Loading...
Searching...
No Matches
impulse-com.hpp
1
2// BSD 3-Clause License
3//
4// Copyright (C) 2021-2025, LAAS-CNRS, University of Edinburgh,
5// Heriot-Watt University
6// Copyright note valid unless otherwise stated in individual files.
7// All rights reserved.
9
10#ifndef CROCODDYL_MULTIBODY_RESIDUALS_IMPULSE_COM_HPP_
11#define CROCODDYL_MULTIBODY_RESIDUALS_IMPULSE_COM_HPP_
12
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"
18
19namespace crocoddyl {
20
37template <typename _Scalar>
39 public:
40 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
41 CROCODDYL_DERIVED_CAST(ResidualModelBase, ResidualModelImpulseCoMTpl)
42
43 typedef _Scalar Scalar;
51 typedef typename MathBase::VectorXs VectorXs;
52
58 ResidualModelImpulseCoMTpl(std::shared_ptr<StateMultibody> state);
59 virtual ~ResidualModelImpulseCoMTpl() = default;
60
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;
71
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;
82
86 virtual std::shared_ptr<ResidualDataAbstract> createData(
87 DataCollectorAbstract* const data) override;
88
98 template <typename NewScalar>
100
106 virtual void print(std::ostream& os) const override;
107
108 protected:
109 using Base::nu_;
110 using Base::state_;
111 using Base::u_dependent_;
112
113 private:
114 std::shared_ptr<typename StateMultibody::PinocchioModel> pin_model_;
115};
116
117template <typename _Scalar>
119 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
120
121 typedef _Scalar Scalar;
126 typedef typename MathBase::MatrixXs MatrixXs;
127 typedef typename MathBase::Matrix3xs Matrix3xs;
128
129 template <template <typename Scalar> class Model>
130 ResidualDataImpulseCoMTpl(Model<Scalar>* const model,
131 DataCollectorAbstract* const data)
132 : Base(model, data),
133 dvc_dq(3, model->get_state()->get_nv()),
134 ddv_dv(model->get_state()->get_nv(), model->get_state()->get_nv()) {
135 dvc_dq.setZero();
136 ddv_dv.setZero();
137 const std::shared_ptr<StateMultibody>& state =
138 std::static_pointer_cast<StateMultibody>(model->get_state());
140 pinocchio::DataTpl<Scalar>(*state->get_pinocchio().get());
141 // Check that proper shared data has been passed
144 if (d == NULL) {
145 throw_pretty(
146 "Invalid argument: the shared data should be derived from "
147 "DataCollectorMultibodyInImpulse");
148 }
149 pinocchio = d->pinocchio;
150 impulses = d->impulses;
151 }
152 virtual ~ResidualDataImpulseCoMTpl() = default;
153
154 pinocchio::DataTpl<Scalar>* pinocchio;
155 std::shared_ptr<crocoddyl::ImpulseDataMultipleTpl<Scalar> >
157 Matrix3xs dvc_dq;
158 MatrixXs ddv_dv;
159 pinocchio::DataTpl<Scalar>
161 using Base::r;
162 using Base::Ru;
163 using Base::Rx;
164 using Base::shared;
165};
166
167} // namespace crocoddyl
168
169/* --- Details -------------------------------------------------------------- */
170/* --- Details -------------------------------------------------------------- */
171/* --- Details -------------------------------------------------------------- */
172#include "crocoddyl/multibody/residuals/impulse-com.hxx"
173
174CROCODDYL_DECLARE_EXTERN_TEMPLATE_CLASS(crocoddyl::ResidualModelImpulseCoMTpl)
175CROCODDYL_DECLARE_EXTERN_TEMPLATE_STRUCT(crocoddyl::ResidualDataImpulseCoMTpl)
176
177#endif // CROCODDYL_MULTIBODY_RESIDUALS_IMPULSE_COM_HPP_
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.
Definition multibody.hpp:34
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.
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.