crocoddyl 1.9.0
Contact RObot COntrol by Differential DYnamic programming Library (Crocoddyl)
 
Loading...
Searching...
No Matches
control.hpp
1
2// BSD 3-Clause License
3//
4// Copyright (C) 2021, LAAS-CNRS, New York University, Max Planck Gesellschaft,
5// University of Edinburgh, University of Trento
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 <boost/make_shared.hpp>
14#include <boost/shared_ptr.hpp>
15
16#include "crocoddyl/core/fwd.hpp"
17#include "crocoddyl/core/control-base.hpp"
18
19namespace crocoddyl {
20
21template <typename _Scalar>
23 public:
24 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
25
26 typedef _Scalar Scalar;
31 typedef typename MathBase::VectorXs VectorXs;
32 typedef typename MathBase::MatrixXs MatrixXs;
33
39 explicit ControlParametrizationModelNumDiffTpl(boost::shared_ptr<Base> model);
41
49 void calc(const boost::shared_ptr<ControlParametrizationDataAbstract>& data, const Scalar t,
50 const Eigen::Ref<const VectorXs>& u) const;
51
59 void calcDiff(const boost::shared_ptr<ControlParametrizationDataAbstract>& data, const Scalar t,
60 const Eigen::Ref<const VectorXs>& u) const;
61
67 virtual boost::shared_ptr<ControlParametrizationDataAbstract> createData();
68
77 void params(const boost::shared_ptr<ControlParametrizationDataAbstract>& data, const Scalar t,
78 const Eigen::Ref<const VectorXs>& w) const;
79
88 void convertBounds(const Eigen::Ref<const VectorXs>& w_lb, const Eigen::Ref<const VectorXs>& w_ub,
89 Eigen::Ref<VectorXs> u_lb, Eigen::Ref<VectorXs> u_ub) const;
90
100 void multiplyByJacobian(const boost::shared_ptr<ControlParametrizationDataAbstract>& data,
101 const Eigen::Ref<const MatrixXs>& A, Eigen::Ref<MatrixXs> out,
102 const AssignmentOp = setto) const;
103
114 void multiplyJacobianTransposeBy(const boost::shared_ptr<ControlParametrizationDataAbstract>& data,
115 const Eigen::Ref<const MatrixXs>& A, Eigen::Ref<MatrixXs> out,
116 const AssignmentOp = setto) const;
117
123 const boost::shared_ptr<Base>& get_model() const;
124
130 const Scalar get_disturbance() const;
131
137 void set_disturbance(const Scalar disturbance);
138
139 private:
144 boost::shared_ptr<Base> model_;
145
149 Scalar disturbance_;
150
151 protected:
152 using Base::nu_;
153 using Base::nw_;
154};
155
156template <typename _Scalar>
158 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
159
160 typedef _Scalar Scalar;
162 typedef typename MathBase::VectorXs VectorXs;
163 typedef typename MathBase::MatrixXs MatrixXs;
165
166 template <template <typename Scalar> class Model>
167 explicit ControlParametrizationDataNumDiffTpl(Model<Scalar>* const model) : Base(model), du(model->get_nu()) {
168 du.setZero();
169
170 const std::size_t nu = model->get_model()->get_nu();
171 data_0 = model->get_model()->createData();
172 for (std::size_t i = 0; i < nu; ++i) {
173 data_u.push_back(model->get_model()->createData());
174 }
175 }
176
178
179 VectorXs du;
180 boost::shared_ptr<Base> data_0;
181 std::vector<boost::shared_ptr<Base> > data_u;
182};
183
184} // namespace crocoddyl
185
186/* --- Details -------------------------------------------------------------- */
187/* --- Details -------------------------------------------------------------- */
188/* --- Details -------------------------------------------------------------- */
189#include "crocoddyl/core/numdiff/control.hxx"
190
191#endif // CROCODDYL_CORE_NUMDIFF_CONTROL_HPP_
Abstract class for the control trajectory parametrization.
std::size_t nu_
Control parameters dimension.
const boost::shared_ptr< Base > & get_model() const
Get the model_ object.
const Scalar get_disturbance() const
Get the disturbance_ object.
ControlParametrizationModelNumDiffTpl(boost::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
Convert the bounds on the control to bounds on the control parameters.
void set_disturbance(const Scalar disturbance)
Set the disturbance_ object.
virtual boost::shared_ptr< ControlParametrizationDataAbstract > createData()
Create the control-parametrization data.
void calc(const boost::shared_ptr< ControlParametrizationDataAbstract > &data, const Scalar t, const Eigen::Ref< const VectorXs > &u) const
Get the value of the control at the specified time.
void calcDiff(const boost::shared_ptr< ControlParametrizationDataAbstract > &data, const Scalar t, const Eigen::Ref< const VectorXs > &u) const
Get the value of the Jacobian of the control with respect to the parameters.
void multiplyByJacobian(const boost::shared_ptr< ControlParametrizationDataAbstract > &data, const Eigen::Ref< const MatrixXs > &A, Eigen::Ref< MatrixXs > out, const AssignmentOp=setto) const
Compute the product between a specified matrix and the Jacobian of the control (with respect to the p...
void multiplyJacobianTransposeBy(const boost::shared_ptr< ControlParametrizationDataAbstract > &data, const Eigen::Ref< const MatrixXs > &A, Eigen::Ref< MatrixXs > out, const AssignmentOp=setto) const
Compute the product between the transposed Jacobian of the control (with respect to the parameters) a...
void params(const boost::shared_ptr< ControlParametrizationDataAbstract > &data, const Scalar t, const Eigen::Ref< const VectorXs > &w) const
Get a value of the control parameters such that the control at the specified time t is equal to the s...
std::vector< boost::shared_ptr< Base > > data_u
The temporary data associated with the control variation.
Definition: control.hpp:181
boost::shared_ptr< Base > data_0
The data that contains the final results.
Definition: control.hpp:180
VectorXs du
temporary variable used for finite differencing
Definition: control.hpp:179