Crocoddyl
 
Loading...
Searching...
No Matches
control-gravity.hpp
1
2// BSD 3-Clause License
3//
4// Copyright (C) 2020-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_CONTROL_GRAVITY_HPP_
11#define CROCODDYL_MULTIBODY_RESIDUALS_CONTROL_GRAVITY_HPP_
12
13#include "crocoddyl/core/residual-base.hpp"
14#include "crocoddyl/multibody/data/multibody.hpp"
15#include "crocoddyl/multibody/states/multibody.hpp"
16
17namespace crocoddyl {
18
35template <typename _Scalar>
37 public:
38 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
39 CROCODDYL_DERIVED_CAST(ResidualModelBase, ResidualModelControlGravTpl)
40
41 typedef _Scalar Scalar;
48 typedef typename MathBase::VectorXs VectorXs;
49 typedef typename MathBase::MatrixXs MatrixXs;
50
57 ResidualModelControlGravTpl(std::shared_ptr<StateMultibody> state,
58 const std::size_t nu);
59
67 ResidualModelControlGravTpl(std::shared_ptr<StateMultibody> state);
68 virtual ~ResidualModelControlGravTpl() = default;
69
77 virtual void calc(const std::shared_ptr<ResidualDataAbstract> &data,
78 const Eigen::Ref<const VectorXs> &x,
79 const Eigen::Ref<const VectorXs> &u) override;
80
85 virtual void calc(const std::shared_ptr<ResidualDataAbstract> &data,
86 const Eigen::Ref<const VectorXs> &x) override;
87
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;
98
104 virtual void calcDiff(const std::shared_ptr<ResidualDataAbstract> &data,
105 const Eigen::Ref<const VectorXs> &x) override;
106
107 virtual std::shared_ptr<ResidualDataAbstract> createData(
108 DataCollectorAbstract *const data) override;
109
119 template <typename NewScalar>
121
127 virtual void print(std::ostream &os) const override;
128
129 protected:
130 using Base::nu_;
131 using Base::state_;
132 using Base::v_dependent_;
133
134 private:
135 typename StateMultibody::PinocchioModel
136 pin_model_;
137};
138
139template <typename _Scalar>
141 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
142
143 typedef _Scalar Scalar;
148 typedef pinocchio::DataTpl<Scalar> PinocchioData;
149
150 template <template <typename Scalar> class Model>
151 ResidualDataControlGravTpl(Model<Scalar> *const model,
152 DataCollectorAbstract *const data)
153 : Base(model, data) {
154 // Check that proper shared data has been passed
157 if (d == NULL) {
158 throw_pretty(
159 "Invalid argument: the shared data should be derived from "
160 "DataCollectorActMultibodyTpl");
161 }
162 // Avoids data casting at runtime
163 StateMultibody *sm =
164 static_cast<StateMultibody *>(model->get_state().get());
165 pinocchio = PinocchioData(*(sm->get_pinocchio().get()));
166 actuation = d->actuation;
167 }
168 virtual ~ResidualDataControlGravTpl() = default;
169
170 PinocchioData pinocchio;
171 std::shared_ptr<ActuationDataAbstractTpl<Scalar> >
173 using Base::r;
174 using Base::Ru;
175 using Base::Rx;
176 using Base::shared;
177};
178
179} // namespace crocoddyl
180
181/* --- Details -------------------------------------------------------------- */
182/* --- Details -------------------------------------------------------------- */
183/* --- Details -------------------------------------------------------------- */
184#include "crocoddyl/multibody/residuals/control-gravity.hxx"
185
186CROCODDYL_DECLARE_EXTERN_TEMPLATE_CLASS(crocoddyl::ResidualModelControlGravTpl)
187CROCODDYL_DECLARE_EXTERN_TEMPLATE_STRUCT(crocoddyl::ResidualDataControlGravTpl)
188
189#endif // CROCODDYL_MULTIBODY_RESIDUALS_CONTROL_GRAVITY_HPP_
Abstract class for residual models.
std::shared_ptr< StateAbstract > state_
State description.
std::size_t nu_
Control dimension.
virtual void calc(const std::shared_ptr< ResidualDataAbstract > &data, const Eigen::Ref< const VectorXs > &x) override
Compute the residual vector for nodes that depends only on the state.
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 control gravity residual.
virtual std::shared_ptr< ResidualDataAbstract > createData(DataCollectorAbstract *const data) override
Create the residual data.
ResidualModelControlGravTpl(std::shared_ptr< StateMultibody > state)
Initialize the control gravity residual model.
virtual void calc(const std::shared_ptr< ResidualDataAbstract > &data, const Eigen::Ref< const VectorXs > &x, const Eigen::Ref< const VectorXs > &u) override
Compute the control gravity residual.
ResidualModelControlGravTpl(std::shared_ptr< StateMultibody > state, const std::size_t nu)
Initialize the control gravity residual model.
virtual void print(std::ostream &os) const override
Print relevant information of the control-grav residual.
ResidualModelControlGravTpl< NewScalar > cast() const
Cast the control-gravity residual model to a different scalar type.
virtual void calcDiff(const std::shared_ptr< ResidualDataAbstract > &data, const Eigen::Ref< const VectorXs > &x) override
State multibody representation.
Definition multibody.hpp:34
const std::shared_ptr< PinocchioModel > & get_pinocchio() const
Return the Pinocchio model (i.e., model of the rigid body system)
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.
PinocchioData pinocchio
Pinocchio data.
std::shared_ptr< ActuationDataAbstractTpl< Scalar > > actuation
Actuation data.
DataCollectorAbstract * shared
Shared data allocated by the action model.