Crocoddyl
 
Loading...
Searching...
No Matches
control.hpp
1
2// BSD 3-Clause License
3//
4// Copyright (C) 2019-2025, University of Edinburgh, LAAS-CNRS,
5// Heriot-Watt University
6// Copyright note valid unless otherwise stated in individual files.
7// All rights reserved.
9
10#ifndef CROCODDYL_CORE_NUMDIFF_CONTROL_HPP_
11#define CROCODDYL_CORE_NUMDIFF_CONTROL_HPP_
12
13#include "crocoddyl/core/control-base.hpp"
14#include "crocoddyl/core/fwd.hpp"
15
16namespace crocoddyl {
17
18template <typename _Scalar>
21 public:
22 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
23 CROCODDYL_DERIVED_CAST(ControlParametrizationModelBase,
25
26 typedef _Scalar Scalar;
32 typedef typename MathBase::VectorXs VectorXs;
33 typedef typename MathBase::MatrixXs MatrixXs;
34
40 explicit ControlParametrizationModelNumDiffTpl(std::shared_ptr<Base> model);
42
50 void calc(const std::shared_ptr<ControlParametrizationDataAbstract>& data,
51 const Scalar t, const Eigen::Ref<const VectorXs>& u) const override;
52
61 void calcDiff(const std::shared_ptr<ControlParametrizationDataAbstract>& data,
62 const Scalar t,
63 const Eigen::Ref<const VectorXs>& u) const override;
64
70 virtual std::shared_ptr<ControlParametrizationDataAbstract> createData()
71 override;
72
81 void params(const std::shared_ptr<ControlParametrizationDataAbstract>& data,
82 const Scalar t,
83 const Eigen::Ref<const VectorXs>& w) const override;
84
94 void convertBounds(const Eigen::Ref<const VectorXs>& w_lb,
95 const Eigen::Ref<const VectorXs>& w_ub,
96 Eigen::Ref<VectorXs> u_lb,
97 Eigen::Ref<VectorXs> u_ub) const override;
98
111 const std::shared_ptr<ControlParametrizationDataAbstract>& data,
112 const Eigen::Ref<const MatrixXs>& A, Eigen::Ref<MatrixXs> out,
113 const AssignmentOp = setto) const override;
114
127 const std::shared_ptr<ControlParametrizationDataAbstract>& data,
128 const Eigen::Ref<const MatrixXs>& A, Eigen::Ref<MatrixXs> out,
129 const AssignmentOp = setto) const override;
130
131 template <typename NewScalar>
138 const std::shared_ptr<Base>& get_model() const;
139
144 const Scalar get_disturbance() const;
145
150 void set_disturbance(const Scalar disturbance);
151
152 private:
153 std::shared_ptr<Base>
154 model_;
155 Scalar e_jac_;
157
158 protected:
159 using Base::nu_;
160 using Base::nw_;
161};
162
163template <typename _Scalar>
165 : public ControlParametrizationDataAbstractTpl<_Scalar> {
166 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
167
168 typedef _Scalar Scalar;
170 typedef typename MathBase::VectorXs VectorXs;
171 typedef typename MathBase::MatrixXs MatrixXs;
173
174 template <template <typename Scalar> class Model>
175 explicit ControlParametrizationDataNumDiffTpl(Model<Scalar>* const model)
176 : Base(model), du(model->get_nu()) {
177 du.setZero();
178
179 const std::size_t nu = model->get_model()->get_nu();
180 data_0 = model->get_model()->createData();
181 for (std::size_t i = 0; i < nu; ++i) {
182 data_u.push_back(model->get_model()->createData());
183 }
184 }
185
187
188 VectorXs du;
189 std::shared_ptr<Base> data_0;
190 std::vector<std::shared_ptr<Base> >
192};
193
194} // namespace crocoddyl
195
196/* --- Details -------------------------------------------------------------- */
197/* --- Details -------------------------------------------------------------- */
198/* --- Details -------------------------------------------------------------- */
199#include "crocoddyl/core/numdiff/control.hxx"
200
201#endif // CROCODDYL_CORE_NUMDIFF_CONTROL_HPP_
Abstract class for the control trajectory parametrization.
std::size_t nu_
Control parameters dimension.
void params(const std::shared_ptr< ControlParametrizationDataAbstract > &data, const Scalar t, const Eigen::Ref< const VectorXs > &w) const override
Get a value of the control parameters such that the control at the specified time t is equal to the s...
const std::shared_ptr< Base > & get_model() const
Get the model_ object.
void calcDiff(const std::shared_ptr< ControlParametrizationDataAbstract > &data, const Scalar t, const Eigen::Ref< const VectorXs > &u) const override
Get the value of the Jacobian of the control with respect to the parameters.
void calc(const std::shared_ptr< ControlParametrizationDataAbstract > &data, const Scalar t, const Eigen::Ref< const VectorXs > &u) const override
Get the value of the control at the specified time.
const Scalar get_disturbance() const
Return the disturbance constant used in the numerical differentiation routine.
void set_disturbance(const Scalar disturbance)
Modify the disturbance constant used in the numerical differentiation routine.
virtual std::shared_ptr< ControlParametrizationDataAbstract > createData() override
Create the control-parametrization data.
ControlParametrizationModelNumDiffTpl(std::shared_ptr< Base > model)
Construct a new ControlParametrizationModelNumDiff object.
void convertBounds(const Eigen::Ref< const VectorXs > &w_lb, const Eigen::Ref< const VectorXs > &w_ub, Eigen::Ref< VectorXs > u_lb, Eigen::Ref< VectorXs > u_ub) const override
Convert the bounds on the control to bounds on the control parameters.
void multiplyByJacobian(const std::shared_ptr< ControlParametrizationDataAbstract > &data, const Eigen::Ref< const MatrixXs > &A, Eigen::Ref< MatrixXs > out, const AssignmentOp=setto) const override
Compute the product between a specified matrix and the Jacobian of the control (with respect to the p...
void multiplyJacobianTransposeBy(const std::shared_ptr< ControlParametrizationDataAbstract > &data, const Eigen::Ref< const MatrixXs > &A, Eigen::Ref< MatrixXs > out, const AssignmentOp=setto) const override
Compute the product between the transposed Jacobian of the control (with respect to the parameters) a...
std::vector< std::shared_ptr< Base > > data_u
The temporary data associated with the control variation.
Definition control.hpp:191
VectorXs du
temporary variable used for finite differencing
Definition control.hpp:188
std::shared_ptr< Base > data_0
The data that contains the final results.
Definition control.hpp:189