Crocoddyl
 
Loading...
Searching...
No Matches
actuation.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_ACTUATION_HPP_
11#define CROCODDYL_CORE_NUMDIFF_ACTUATION_HPP_
12
13#include "crocoddyl/core/actuation-base.hpp"
14#include "crocoddyl/core/fwd.hpp"
15
16namespace crocoddyl {
17
30template <typename _Scalar>
32 public:
33 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
34 CROCODDYL_DERIVED_CAST(ActuationModelBase, ActuationModelNumDiffTpl)
35
36 typedef _Scalar Scalar;
41 typedef typename MathBase::VectorXs VectorXs;
42 typedef typename MathBase::MatrixXs MatrixXs;
43
50 explicit ActuationModelNumDiffTpl(std::shared_ptr<Base> model);
51
55 virtual ~ActuationModelNumDiffTpl() = default;
56
60 virtual void calc(const std::shared_ptr<ActuationDataAbstract>& data,
61 const Eigen::Ref<const VectorXs>& x,
62 const Eigen::Ref<const VectorXs>& u) override;
63
68 virtual void calc(const std::shared_ptr<ActuationDataAbstract>& data,
69 const Eigen::Ref<const VectorXs>& x);
70
74 virtual void calcDiff(const std::shared_ptr<ActuationDataAbstract>& data,
75 const Eigen::Ref<const VectorXs>& x,
76 const Eigen::Ref<const VectorXs>& u) override;
77
83 virtual void calcDiff(const std::shared_ptr<ActuationDataAbstract>& data,
84 const Eigen::Ref<const VectorXs>& x);
85
89 virtual void commands(const std::shared_ptr<ActuationDataAbstract>& data,
90 const Eigen::Ref<const VectorXs>& x,
91 const Eigen::Ref<const VectorXs>& tau) override;
92
96 virtual void torqueTransform(
97 const std::shared_ptr<ActuationDataAbstract>& data,
98 const Eigen::Ref<const VectorXs>& x,
99 const Eigen::Ref<const VectorXs>& u) override;
100
104 virtual std::shared_ptr<ActuationDataAbstract> createData() override;
105
106 template <typename NewScalar>
108
112 const std::shared_ptr<Base>& get_model() const;
113
118 const Scalar get_disturbance() const;
119
124 void set_disturbance(const Scalar disturbance);
125
126 private:
127 std::shared_ptr<Base> model_;
129 Scalar e_jac_;
131
132 protected:
133 using Base::nu_;
134};
135
136template <typename _Scalar>
138 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
139
140 typedef _Scalar Scalar;
142 typedef typename MathBase::VectorXs VectorXs;
144
151 template <template <typename Scalar> class Model>
152 explicit ActuationDataNumDiffTpl(Model<Scalar>* const model)
153 : Base(model),
154 dx(model->get_model()->get_state()->get_ndx()),
155 du(model->get_model()->get_nu()),
156 xp(model->get_model()->get_state()->get_nx()) {
157 dx.setZero();
158 du.setZero();
159 xp.setZero();
160 const std::size_t ndx = model->get_model()->get_state()->get_ndx();
161 const std::size_t nu = model->get_model()->get_nu();
162 data_0 = model->get_model()->createData();
163 for (std::size_t i = 0; i < ndx; ++i) {
164 data_x.push_back(model->get_model()->createData());
165 }
166 for (std::size_t i = 0; i < nu; ++i) {
167 data_u.push_back(model->get_model()->createData());
168 }
169 }
170
171 Scalar x_norm;
172 Scalar
174 Scalar
176 VectorXs dx;
177 VectorXs du;
178 VectorXs xp;
180 std::shared_ptr<Base> data_0;
181 std::vector<std::shared_ptr<Base> >
183 std::vector<std::shared_ptr<Base> >
185
186 using Base::dtau_du;
187 using Base::dtau_dx;
188 using Base::tau;
189};
190
191} // namespace crocoddyl
192
193/* --- Details -------------------------------------------------------------- */
194/* --- Details -------------------------------------------------------------- */
195/* --- Details -------------------------------------------------------------- */
196#include "crocoddyl/core/numdiff/actuation.hxx"
197
198#endif // CROCODDYL_CORE_NUMDIFF_ACTUATION_HPP_
Abstract class for the actuation-mapping model.
std::size_t nu_
Dimension of joint torque inputs.
This class computes the numerical differentiation of an actuation model.
Definition actuation.hpp:31
virtual void calc(const std::shared_ptr< ActuationDataAbstract > &data, const Eigen::Ref< const VectorXs > &x, const Eigen::Ref< const VectorXs > &u) override
Compute the actuation signal from the state point and joint torque inputs .
const std::shared_ptr< Base > & get_model() const
Return the original actuation model.
virtual ~ActuationModelNumDiffTpl()=default
Destroy the numdiff actuation model.
virtual void commands(const std::shared_ptr< ActuationDataAbstract > &data, const Eigen::Ref< const VectorXs > &x, const Eigen::Ref< const VectorXs > &tau) override
Compute the joint torque input from the generalized torques.
const Scalar get_disturbance() const
Return the disturbance constant used by the numerical differentiation routine.
virtual void calcDiff(const std::shared_ptr< ActuationDataAbstract > &data, const Eigen::Ref< const VectorXs > &x, const Eigen::Ref< const VectorXs > &u) override
Compute the Jacobians of the actuation function.
void set_disturbance(const Scalar disturbance)
Modify the disturbance constant used by the numerical differentiation routine.
virtual std::shared_ptr< ActuationDataAbstract > createData() override
Create the actuation data.
ActuationModelNumDiffTpl(std::shared_ptr< Base > model)
Initialize the numdiff residual model.
virtual void torqueTransform(const std::shared_ptr< ActuationDataAbstract > &data, const Eigen::Ref< const VectorXs > &x, const Eigen::Ref< const VectorXs > &u) override
Compute the torque transform from generalized torques to joint torque inputs.
virtual void calcDiff(const std::shared_ptr< ActuationDataAbstract > &data, const Eigen::Ref< const VectorXs > &x)
virtual void calc(const std::shared_ptr< ActuationDataAbstract > &data, const Eigen::Ref< const VectorXs > &x)
Ignore the computation of the actuation signal.
VectorXs tau
Generalized torques.
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.
ActuationDataNumDiffTpl(Model< Scalar > *const model)
Initialize the numdiff actuation data.
std::shared_ptr< Base > data_0
The data that contains the final results.
VectorXs dx
State disturbance.