Crocoddyl
 
Loading...
Searching...
No Matches
residual.hpp
1
2// BSD 3-Clause License
3//
4// Copyright (C) 2021-2025, University of Edinburgh, Heriot-Watt University
5// Copyright note valid unless otherwise stated in individual files.
6// All rights reserved.
8
9#ifndef CROCODDYL_CORE_NUMDIFF_RESIDUAL_HPP_
10#define CROCODDYL_CORE_NUMDIFF_RESIDUAL_HPP_
11
12#include <boost/function.hpp>
13
14#include "crocoddyl/core/residual-base.hpp"
15#include "crocoddyl/multibody/fwd.hpp"
16
17namespace crocoddyl {
18
28template <typename _Scalar>
30 public:
31 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
32 CROCODDYL_DERIVED_CAST(ResidualModelBase, ResidualModelNumDiffTpl)
33
34 typedef _Scalar Scalar;
40 typedef typename MathBaseTpl<Scalar>::VectorXs VectorXs;
41 typedef typename MathBaseTpl<Scalar>::MatrixXs MatrixXs;
42 typedef boost::function<void(const VectorXs&, const VectorXs&)>
43 ReevaluationFunction;
44
51 explicit ResidualModelNumDiffTpl(const std::shared_ptr<Base>& model);
52
56 virtual ~ResidualModelNumDiffTpl() = default;
57
61 virtual void calc(const std::shared_ptr<ResidualDataAbstract>& data,
62 const Eigen::Ref<const VectorXs>& x,
63 const Eigen::Ref<const VectorXs>& u) override;
64
69 virtual void calc(const std::shared_ptr<ResidualDataAbstract>& data,
70 const Eigen::Ref<const VectorXs>& x) override;
71
75 virtual void calcDiff(const std::shared_ptr<ResidualDataAbstract>& data,
76 const Eigen::Ref<const VectorXs>& x,
77 const Eigen::Ref<const VectorXs>& u) override;
78
84 virtual void calcDiff(const std::shared_ptr<ResidualDataAbstract>& data,
85 const Eigen::Ref<const VectorXs>& x) override;
86
90 virtual std::shared_ptr<ResidualDataAbstract> createData(
91 DataCollectorAbstract* const data) override;
92
93 template <typename NewScalar>
95
99 const std::shared_ptr<Base>& get_model() const;
100
105 const Scalar get_disturbance() const;
106
111 void set_disturbance(const Scalar disturbance);
112
120 void set_reevals(const std::vector<ReevaluationFunction>& reevals);
121
122 protected:
123 using Base::nu_;
124 using Base::state_;
125 using Base::unone_;
126
127 private:
139 void assertStableStateFD(const Eigen::Ref<const VectorXs>& /*x*/);
140
141 std::shared_ptr<Base> model_;
143 Scalar e_jac_;
145 std::vector<ReevaluationFunction>
146 reevals_;
147};
148
149template <typename _Scalar>
151 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
152
153 typedef _Scalar Scalar;
158 typedef typename MathBaseTpl<Scalar>::VectorXs VectorXs;
159
166 template <template <typename Scalar> class Model>
167 explicit ResidualDataNumDiffTpl(Model<Scalar>* const model,
168 DataCollectorAbstract* const shared_data)
169 : Base(model, shared_data),
170 dx(model->get_state()->get_ndx()),
171 xp(model->get_state()->get_nx()),
172 du(model->get_nu()),
173 up(model->get_nu()) {
174 dx.setZero();
175 xp.setZero();
176 du.setZero();
177 up.setZero();
178
179 const std::size_t& ndx = model->get_model()->get_state()->get_ndx();
180 const std::size_t& nu = model->get_model()->get_nu();
181 data_0 = model->get_model()->createData(shared_data);
182 for (std::size_t i = 0; i < ndx; ++i) {
183 data_x.push_back(model->get_model()->createData(shared_data));
184 }
185 for (std::size_t i = 0; i < nu; ++i) {
186 data_u.push_back(model->get_model()->createData(shared_data));
187 }
188 }
189
190 virtual ~ResidualDataNumDiffTpl() {}
191
192 using Base::r;
193 using Base::Ru;
194 using Base::Rx;
195 using Base::shared;
196
197 Scalar x_norm;
198 Scalar
200 Scalar
202 VectorXs dx;
203 VectorXs xp;
205 VectorXs du;
206 VectorXs up;
208 std::shared_ptr<Base> data_0;
209 std::vector<std::shared_ptr<Base> >
211 std::vector<std::shared_ptr<Base> >
213};
214
215} // namespace crocoddyl
216
217/* --- Details -------------------------------------------------------------- */
218/* --- Details -------------------------------------------------------------- */
219/* --- Details -------------------------------------------------------------- */
220#include "crocoddyl/core/numdiff/residual.hxx"
221
222#endif // CROCODDYL_CORE_NUMDIFF_RESIDUAL_HPP_
Abstract class for residual models.
std::shared_ptr< StateAbstract > state_
State description.
std::size_t nu_
Control dimension.
VectorXs unone_
No control vector.
This class computes the numerical differentiation of a residual model.
Definition residual.hpp:29
const std::shared_ptr< Base > & get_model() const
Return the original residual model.
const Scalar get_disturbance() const
Return the disturbance constant used by the numerical differentiation routine.
virtual void calc(const std::shared_ptr< ResidualDataAbstract > &data, const Eigen::Ref< const VectorXs > &x) override
void set_reevals(const std::vector< ReevaluationFunction > &reevals)
Register functions that updates the shared data computed for a system rollout The updated data is use...
void set_disturbance(const Scalar disturbance)
Modify the disturbance constant used by the numerical differentiation routine.
virtual void calcDiff(const std::shared_ptr< ResidualDataAbstract > &data, const Eigen::Ref< const VectorXs > &x, const Eigen::Ref< const VectorXs > &u) override
Compute the Jacobian of the residual vector.
virtual ~ResidualModelNumDiffTpl()=default
Initialize the numdiff residual model.
ResidualModelNumDiffTpl(const std::shared_ptr< Base > &model)
Initialize the numdiff residual model.
virtual std::shared_ptr< ResidualDataAbstract > createData(DataCollectorAbstract *const data) override
Create the 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 residual vector.
virtual void calcDiff(const std::shared_ptr< ResidualDataAbstract > &data, const Eigen::Ref< const VectorXs > &x) override
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.
Scalar x_norm
Norm of the state vector.
Definition residual.hpp:197
std::vector< std::shared_ptr< Base > > data_x
The temporary data associated with the state variation.
Definition residual.hpp:210
Scalar uh_jac
Disturbance value used for computing .
Definition residual.hpp:201
std::vector< std::shared_ptr< Base > > data_u
The temporary data associated with the control variation.
Definition residual.hpp:212
Scalar xh_jac
Disturbance value used for computing .
Definition residual.hpp:199
ResidualDataNumDiffTpl(Model< Scalar > *const model, DataCollectorAbstract *const shared_data)
Initialize the numdiff residual data.
Definition residual.hpp:167
VectorXs du
Control disturbance.
Definition residual.hpp:205
std::shared_ptr< Base > data_0
The data at the approximation point.
Definition residual.hpp:208
VectorXs dx
State disturbance.
Definition residual.hpp:202