crocoddyl 1.9.0
Contact RObot COntrol by Differential DYnamic programming Library (Crocoddyl)
 
Loading...
Searching...
No Matches
residual.hpp
1
2// BSD 3-Clause License
3//
4// Copyright (C) 2021, University of Edinburgh
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#include "crocoddyl/multibody/fwd.hpp"
14#include "crocoddyl/core/residual-base.hpp"
15
16namespace crocoddyl {
17
27template <typename _Scalar>
29 public:
30 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
31
32 typedef _Scalar Scalar;
38 typedef typename MathBaseTpl<Scalar>::VectorXs VectorXs;
39 typedef typename MathBaseTpl<Scalar>::MatrixXs MatrixXs;
40 typedef boost::function<void(const VectorXs&, const VectorXs&)> ReevaluationFunction;
41
47 explicit ResidualModelNumDiffTpl(const boost::shared_ptr<Base>& model);
48
53
57 virtual void calc(const boost::shared_ptr<ResidualDataAbstract>& data, const Eigen::Ref<const VectorXs>& x,
58 const Eigen::Ref<const VectorXs>& u);
59
64 virtual void calc(const boost::shared_ptr<ResidualDataAbstract>& data, const Eigen::Ref<const VectorXs>& x);
65
69 virtual void calcDiff(const boost::shared_ptr<ResidualDataAbstract>& data, const Eigen::Ref<const VectorXs>& x,
70 const Eigen::Ref<const VectorXs>& u);
71
76 virtual void calcDiff(const boost::shared_ptr<ResidualDataAbstract>& data, const Eigen::Ref<const VectorXs>& x);
77
81 virtual boost::shared_ptr<ResidualDataAbstract> createData(DataCollectorAbstract* const data);
82
86 const boost::shared_ptr<Base>& get_model() const;
87
91 const Scalar get_disturbance() const;
92
96 void set_disturbance(const Scalar disturbance);
97
104 void set_reevals(const std::vector<ReevaluationFunction>& reevals);
105
106 protected:
107 using Base::nu_;
108 using Base::state_;
109 using Base::unone_;
110
111 private:
123 void assertStableStateFD(const Eigen::Ref<const VectorXs>& /*x*/);
124
125 boost::shared_ptr<Base> model_;
126 Scalar disturbance_;
127 std::vector<ReevaluationFunction> reevals_;
128};
129
130template <typename _Scalar>
132 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
133
134 typedef _Scalar Scalar;
139 typedef typename MathBaseTpl<Scalar>::VectorXs VectorXs;
140
147 template <template <typename Scalar> class Model>
148 explicit ResidualDataNumDiffTpl(Model<Scalar>* const model, DataCollectorAbstract* const shared_data)
149 : Base(model, shared_data),
150 dx(model->get_state()->get_ndx()),
151 xp(model->get_state()->get_nx()),
152 du(model->get_nu()),
153 up(model->get_nu()) {
154 dx.setZero();
155 xp.setZero();
156 du.setZero();
157 up.setZero();
158
159 const std::size_t& ndx = model->get_model()->get_state()->get_ndx();
160 const std::size_t& nu = model->get_model()->get_nu();
161 data_0 = model->get_model()->createData(shared_data);
162 for (std::size_t i = 0; i < ndx; ++i) {
163 data_x.push_back(model->get_model()->createData(shared_data));
164 }
165 for (std::size_t i = 0; i < nu; ++i) {
166 data_u.push_back(model->get_model()->createData(shared_data));
167 }
168 }
169
170 virtual ~ResidualDataNumDiffTpl() {}
171
172 using Base::r;
173 using Base::Ru;
174 using Base::Rx;
175 using Base::shared;
176
177 VectorXs dx;
178 VectorXs xp;
179 VectorXs du;
180 VectorXs up;
181 boost::shared_ptr<Base> data_0;
182 std::vector<boost::shared_ptr<Base> > data_x;
183 std::vector<boost::shared_ptr<Base> > data_u;
184};
185
186} // namespace crocoddyl
187
188/* --- Details -------------------------------------------------------------- */
189/* --- Details -------------------------------------------------------------- */
190/* --- Details -------------------------------------------------------------- */
191#include "crocoddyl/core/numdiff/residual.hxx"
192
193#endif // CROCODDYL_CORE_NUMDIFF_RESIDUAL_HPP_
Abstract class for residual models.
boost::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:28
const boost::shared_ptr< Base > & get_model() const
Return the original residual model.
virtual void calc(const boost::shared_ptr< ResidualDataAbstract > &data, const Eigen::Ref< const VectorXs > &x)
const Scalar get_disturbance() const
Return the disturbance value used by the numdiff routine.
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...
virtual boost::shared_ptr< ResidualDataAbstract > createData(DataCollectorAbstract *const data)
Create the residual data.
virtual void calcDiff(const boost::shared_ptr< ResidualDataAbstract > &data, const Eigen::Ref< const VectorXs > &x)
void set_disturbance(const Scalar disturbance)
Modify the disturbance value used by the numdiff routine.
ResidualModelNumDiffTpl(const boost::shared_ptr< Base > &model)
Initialize the numdiff residual model.
virtual void calc(const boost::shared_ptr< ResidualDataAbstract > &data, const Eigen::Ref< const VectorXs > &x, const Eigen::Ref< const VectorXs > &u)
Compute the residual vector.
virtual void calcDiff(const boost::shared_ptr< ResidualDataAbstract > &data, const Eigen::Ref< const VectorXs > &x, const Eigen::Ref< const VectorXs > &u)
Compute the Jacobian of the residual vector.
virtual ~ResidualModelNumDiffTpl()
Initialize the numdiff residual model.
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.
std::vector< boost::shared_ptr< Base > > data_u
The temporary data associated with the control variation.
Definition: residual.hpp:183
boost::shared_ptr< Base > data_0
The data at the approximation point.
Definition: residual.hpp:181
VectorXs xp
The integrated state from the disturbance on one DoF "\f$ \int x dx_i \f$".
Definition: residual.hpp:178
ResidualDataNumDiffTpl(Model< Scalar > *const model, DataCollectorAbstract *const shared_data)
Initialize the numdiff residual data.
Definition: residual.hpp:148
VectorXs du
Control disturbance.
Definition: residual.hpp:179
std::vector< boost::shared_ptr< Base > > data_x
The temporary data associated with the state variation.
Definition: residual.hpp:182
VectorXs up
The integrated control from the disturbance on one DoF "\f$ \int u du_i = u + du \f$".
Definition: residual.hpp:180
VectorXs dx
State disturbance.
Definition: residual.hpp:177