Crocoddyl
 
Loading...
Searching...
No Matches
poly-two-rk.hpp
1
2// BSD 3-Clause License
3//
4// Copyright (C) 2021-2025, University of Edinburgh, University of Trento,
5// Heriot-Watt University
6// Copyright note valid unless otherwise stated in individual files.
7// All rights reserved.
9
10#ifndef CROCODDYL_CORE_CONTROLS_POLY_TWO_RK_HPP_
11#define CROCODDYL_CORE_CONTROLS_POLY_TWO_RK_HPP_
12
13#include "crocoddyl/core/control-base.hpp"
14#include "crocoddyl/core/fwd.hpp"
15#include "crocoddyl/core/integrator/rk.hpp"
16
17namespace crocoddyl {
18
49template <typename _Scalar>
52 public:
53 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
54 CROCODDYL_DERIVED_CAST(ControlParametrizationModelBase,
56
57 typedef _Scalar Scalar;
63 typedef typename MathBase::VectorXs VectorXs;
64 typedef typename MathBase::MatrixXs MatrixXs;
65
72 explicit ControlParametrizationModelPolyTwoRKTpl(const std::size_t nw,
73 const RKType rktype);
75
83 virtual void calc(
84 const std::shared_ptr<ControlParametrizationDataAbstract>& data,
85 const Scalar t, const Eigen::Ref<const VectorXs>& u) const override;
86
97 virtual void calcDiff(
98 const std::shared_ptr<ControlParametrizationDataAbstract>& data,
99 const Scalar t, const Eigen::Ref<const VectorXs>& u) const override;
100
106 virtual std::shared_ptr<ControlParametrizationDataAbstract> createData()
107 override;
108
117 virtual void params(
118 const std::shared_ptr<ControlParametrizationDataAbstract>& data,
119 const Scalar t, const Eigen::Ref<const VectorXs>& w) const override;
120
130 virtual void convertBounds(const Eigen::Ref<const VectorXs>& w_lb,
131 const Eigen::Ref<const VectorXs>& w_ub,
132 Eigen::Ref<VectorXs> u_lb,
133 Eigen::Ref<VectorXs> u_ub) const override;
134
148 virtual void multiplyByJacobian(
149 const std::shared_ptr<ControlParametrizationDataAbstract>& data,
150 const Eigen::Ref<const MatrixXs>& A, Eigen::Ref<MatrixXs> out,
151 const AssignmentOp = setto) const override;
152
167 const std::shared_ptr<ControlParametrizationDataAbstract>& data,
168 const Eigen::Ref<const MatrixXs>& A, Eigen::Ref<MatrixXs> out,
169 const AssignmentOp = setto) const override;
170
171 template <typename NewScalar>
173
174 virtual void print(std::ostream& os) const override;
175
176 protected:
177 using Base::nu_;
178 using Base::nw_;
179
180 private:
181 RKType rktype_;
182};
183
184template <typename _Scalar>
186 : public ControlParametrizationDataAbstractTpl<_Scalar> {
187 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
188
189 typedef _Scalar Scalar;
192 typedef typename MathBase::Vector3s Vector3s;
193
194 template <template <typename Scalar> class Model>
195 explicit ControlParametrizationDataPolyTwoRKTpl(Model<Scalar>* const model)
196 : Base(model), tmp_t2(0.) {
197 c.setZero();
198 }
199 virtual ~ControlParametrizationDataPolyTwoRKTpl() = default;
200
201 Vector3s c;
203 Scalar tmp_t2;
204};
205
206} // namespace crocoddyl
207
208/* --- Details -------------------------------------------------------------- */
209/* --- Details -------------------------------------------------------------- */
210/* --- Details -------------------------------------------------------------- */
211#include "crocoddyl/core/controls/poly-two-rk.hxx"
212
213CROCODDYL_DECLARE_EXTERN_TEMPLATE_CLASS(
215CROCODDYL_DECLARE_EXTERN_TEMPLATE_STRUCT(
217
218#endif // CROCODDYL_CORE_CONTROLS_POLY_TWO_RK_HPP_
Abstract class for the control trajectory parametrization.
std::size_t nu_
Control parameters dimension.
A polynomial function of time of degree two, that is a quadratic function.
virtual 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.
virtual 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...
ControlParametrizationModelPolyTwoRKTpl(const std::size_t nw, const RKType rktype)
Initialize the poly-two RK control parametrization.
virtual 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.
virtual std::shared_ptr< ControlParametrizationDataAbstract > createData() override
Create the control-parametrization data.
virtual 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...
virtual 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 u such that the control at the specified time t is equal to the...
virtual 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
Map the specified bounds from the control space to the parameter space.
virtual void print(std::ostream &os) const override
Print relevant information of the control model.
Scalar tmp_t2
Temporary variable to store the square of the time.