Crocoddyl
 
Loading...
Searching...
No Matches
constraint.hpp
1
2// BSD 3-Clause License
3//
4// Copyright (C) 2020-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_CONSTRAINT_HPP_
10#define CROCODDYL_CORE_NUMDIFF_CONSTRAINT_HPP_
11
12#include <boost/function.hpp>
13
14#include "crocoddyl/core/constraint-base.hpp"
15
16namespace crocoddyl {
17
29template <typename _Scalar>
31 public:
32 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
33 CROCODDYL_DERIVED_CAST(ConstraintModelBase, ConstraintModelNumDiffTpl)
34
35 typedef _Scalar Scalar;
41 typedef typename MathBaseTpl<Scalar>::VectorXs VectorXs;
42 typedef boost::function<void(const VectorXs&, const VectorXs&)>
43 ReevaluationFunction;
44
50 explicit ConstraintModelNumDiffTpl(const std::shared_ptr<Base>& model);
51
55 virtual ~ConstraintModelNumDiffTpl() = default;
56
60 virtual void calc(const std::shared_ptr<ConstraintDataAbstract>& data,
61 const Eigen::Ref<const VectorXs>& x,
62 const Eigen::Ref<const VectorXs>& u) override;
63
69 virtual void calc(const std::shared_ptr<ConstraintDataAbstract>& data,
70 const Eigen::Ref<const VectorXs>& x) override;
71
75 virtual void calcDiff(const std::shared_ptr<ConstraintDataAbstract>& 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<ConstraintDataAbstract>& data,
85 const Eigen::Ref<const VectorXs>& x) override;
86
90 virtual std::shared_ptr<ConstraintDataAbstract> createData(
91 DataCollectorAbstract* const data) override;
92
102 template <typename NewScalar>
104
108 const std::shared_ptr<Base>& get_model() const;
109
114 const Scalar get_disturbance() const;
115
120 void set_disturbance(const Scalar disturbance);
121
129 void set_reevals(const std::vector<ReevaluationFunction>& reevals);
130
131 protected:
132 using Base::nu_;
133 using Base::state_;
134 using Base::unone_;
135
136 private:
145 void assertStableStateFD(const Eigen::Ref<const VectorXs>& /*x*/);
146
147 std::shared_ptr<Base> model_;
149 Scalar e_jac_;
151 std::vector<ReevaluationFunction>
152 reevals_;
153};
154
155template <typename _Scalar>
157 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
158
159 typedef _Scalar Scalar;
164 typedef typename MathBaseTpl<Scalar>::VectorXs VectorXs;
165
166 template <template <typename Scalar> class Model>
167 explicit ConstraintDataNumDiffTpl(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 ~ConstraintDataNumDiffTpl() {}
191
192 using Base::Gu;
193 using Base::Gx;
194 using Base::Hu;
195 using Base::Hx;
196 using Base::shared;
197
198 Scalar x_norm;
199 Scalar
201 Scalar
203 VectorXs dx;
204 VectorXs xp;
206 VectorXs du;
207 VectorXs up;
209 std::shared_ptr<Base> data_0;
210 std::vector<std::shared_ptr<Base> >
212 std::vector<std::shared_ptr<Base> >
214};
215
216} // namespace crocoddyl
217
218/* --- Details -------------------------------------------------------------- */
219/* --- Details -------------------------------------------------------------- */
220/* --- Details -------------------------------------------------------------- */
221#include "crocoddyl/core/numdiff/constraint.hxx"
222
223#endif // CROCODDYL_CORE_NUMDIFF_CONSTRAINT_HPP_
Abstract class for constraint models.
std::shared_ptr< StateAbstract > state_
State description.
std::size_t nu_
Control dimension.
This class computes the numerical differentiation of a constraint model.
const std::shared_ptr< Base > & get_model() const
Return the original constraint model.
virtual void calc(const std::shared_ptr< ConstraintDataAbstract > &data, const Eigen::Ref< const VectorXs > &x, const Eigen::Ref< const VectorXs > &u) override
Compute the constraint value.
const Scalar get_disturbance() const
Return the disturbance constant used by the numerical differentiation 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...
void set_disturbance(const Scalar disturbance)
Modify the disturbance constant used by the numerical differentiation routine.
ConstraintModelNumDiffTpl< NewScalar > cast() const
Cast the constraint numdiff model to a different scalar type.
virtual ~ConstraintModelNumDiffTpl()=default
Initialize the numdiff constraint model.
virtual void calcDiff(const std::shared_ptr< ConstraintDataAbstract > &data, const Eigen::Ref< const VectorXs > &x) override
virtual std::shared_ptr< ConstraintDataAbstract > createData(DataCollectorAbstract *const data) override
Create the constraint data.
ConstraintModelNumDiffTpl(const std::shared_ptr< Base > &model)
Initialize the numdiff constraint model.
virtual void calcDiff(const std::shared_ptr< ConstraintDataAbstract > &data, const Eigen::Ref< const VectorXs > &x, const Eigen::Ref< const VectorXs > &u) override
Compute the Jacobian of the constraint.
virtual void calc(const std::shared_ptr< ConstraintDataAbstract > &data, const Eigen::Ref< const VectorXs > &x) override
MatrixXs Gx
Jacobian of the inequality constraint.
MatrixXs Hx
Jacobian of the equality constraint.
MatrixXs Hu
Jacobian of the equality constraint.
DataCollectorAbstract * shared
Shared data.
MatrixXs Gu
Jacobian of the inequality constraint.
Scalar x_norm
Norm of the state vector.
std::vector< std::shared_ptr< Base > > data_x
The temporary data associated with the state variation.
Scalar uh_jac
Disturbance value used for computing .
std::vector< std::shared_ptr< Base > > data_u
The temporary data associated with the control variation.
Scalar xh_jac
Disturbance value used for computing .
VectorXs du
Control disturbance.
std::shared_ptr< Base > data_0
The data at the approximation point.
VectorXs dx
State disturbance.