crocoddyl 1.9.0
Contact RObot COntrol by Differential DYnamic programming Library (Crocoddyl)
 
Loading...
Searching...
No Matches
actuation.hpp
1
2// BSD 3-Clause License
3//
4// Copyright (C) 2019-2021, University of Edinburgh, LAAS-CNRS
5// Copyright note valid unless otherwise stated in individual files.
6// All rights reserved.
8
9#ifndef CROCODDYL_CORE_NUMDIFF_ACTUATION_HPP_
10#define CROCODDYL_CORE_NUMDIFF_ACTUATION_HPP_
11
12#include <vector>
13#include <iostream>
14
15#include "crocoddyl/core/fwd.hpp"
16#include "crocoddyl/core/actuation-base.hpp"
17
18namespace crocoddyl {
19
30template <typename _Scalar>
32 public:
33 typedef _Scalar Scalar;
38 typedef typename MathBase::VectorXs VectorXs;
39 typedef typename MathBase::MatrixXs MatrixXs;
40
46 explicit ActuationModelNumDiffTpl(boost::shared_ptr<Base> model);
47
52
56 virtual void calc(const boost::shared_ptr<ActuationDataAbstract>& data, const Eigen::Ref<const VectorXs>& x,
57 const Eigen::Ref<const VectorXs>& u);
58
63 virtual void calc(const boost::shared_ptr<ActuationDataAbstract>& data, const Eigen::Ref<const VectorXs>& x);
64
68 virtual void calcDiff(const boost::shared_ptr<ActuationDataAbstract>& data, const Eigen::Ref<const VectorXs>& x,
69 const Eigen::Ref<const VectorXs>& u);
70
75 virtual void calcDiff(const boost::shared_ptr<ActuationDataAbstract>& data, const Eigen::Ref<const VectorXs>& x);
76
80 virtual boost::shared_ptr<ActuationDataAbstract> createData();
81
85 const boost::shared_ptr<Base>& get_model() const;
86
90 const Scalar get_disturbance() const;
91
95 void set_disturbance(const Scalar disturbance);
96
97 private:
98 boost::shared_ptr<Base> model_;
99 Scalar disturbance_;
100
101 protected:
102 using Base::nu_;
103};
104
105template <typename _Scalar>
107 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
108
109 typedef _Scalar Scalar;
111 typedef typename MathBase::VectorXs VectorXs;
113
120 template <template <typename Scalar> class Model>
121 explicit ActuationDataNumDiffTpl(Model<Scalar>* const model)
122 : Base(model),
123 dx(model->get_model()->get_state()->get_ndx()),
124 du(model->get_model()->get_nu()),
125 xp(model->get_model()->get_state()->get_nx()) {
126 dx.setZero();
127 du.setZero();
128 xp.setZero();
129 const std::size_t ndx = model->get_model()->get_state()->get_ndx();
130 const std::size_t nu = model->get_model()->get_nu();
131 data_0 = model->get_model()->createData();
132 for (std::size_t i = 0; i < ndx; ++i) {
133 data_x.push_back(model->get_model()->createData());
134 }
135 for (std::size_t i = 0; i < nu; ++i) {
136 data_u.push_back(model->get_model()->createData());
137 }
138 }
139
140 VectorXs dx;
141 VectorXs du;
142 VectorXs xp;
143 boost::shared_ptr<Base> data_0;
144 std::vector<boost::shared_ptr<Base> > data_x;
145 std::vector<boost::shared_ptr<Base> > data_u;
146
147 using Base::dtau_du;
148 using Base::dtau_dx;
149 using Base::tau;
150};
151
152} // namespace crocoddyl
153
154/* --- Details -------------------------------------------------------------- */
155/* --- Details -------------------------------------------------------------- */
156/* --- Details -------------------------------------------------------------- */
157#include "crocoddyl/core/numdiff/actuation.hxx"
158
159#endif // CROCODDYL_CORE_NUMDIFF_ACTUATION_HPP_
Abstract class for the actuation-mapping model.
std::size_t nu_
Control dimension.
This class computes the numerical differentiation of an actuation model.
Definition: actuation.hpp:31
virtual boost::shared_ptr< ActuationDataAbstract > createData()
Create the actuation data.
const boost::shared_ptr< Base > & get_model() const
Return the original actuation model.
virtual void calcDiff(const boost::shared_ptr< ActuationDataAbstract > &data, const Eigen::Ref< const VectorXs > &x)
virtual void calcDiff(const boost::shared_ptr< ActuationDataAbstract > &data, const Eigen::Ref< const VectorXs > &x, const Eigen::Ref< const VectorXs > &u)
Compute the Jacobians of the actuation function.
virtual void calc(const boost::shared_ptr< ActuationDataAbstract > &data, const Eigen::Ref< const VectorXs > &x, const Eigen::Ref< const VectorXs > &u)
Compute the actuation signal from the state point and control input .
const Scalar get_disturbance() const
Return the disturbance value used by the numdiff routine.
void set_disturbance(const Scalar disturbance)
Modify the disturbance value used by the numdiff routine.
virtual void calc(const boost::shared_ptr< ActuationDataAbstract > &data, const Eigen::Ref< const VectorXs > &x)
Ignore the computation of the actuation signal.
ActuationModelNumDiffTpl(boost::shared_ptr< Base > model)
Initialize the numdiff residual model.
virtual ~ActuationModelNumDiffTpl()
Destroy the numdiff actuation model.
MatrixXs dtau_dx
Partial derivatives of the actuation model w.r.t. the state point.
VectorXs tau
Actuation (generalized force) signal.
MatrixXs dtau_du
Partial derivatives of the actuation model w.r.t. the control input.
std::vector< boost::shared_ptr< Base > > data_u
The temporary data associated with the control variation.
Definition: actuation.hpp:145
boost::shared_ptr< Base > data_0
The data that contains the final results.
Definition: actuation.hpp:143
VectorXs xp
The integrated state from the disturbance on one DoF "\f$ \int x dx_i \f$".
Definition: actuation.hpp:142
VectorXs du
Control disturbance.
Definition: actuation.hpp:141
std::vector< boost::shared_ptr< Base > > data_x
The temporary data associated with the state variation.
Definition: actuation.hpp:144
ActuationDataNumDiffTpl(Model< Scalar > *const model)
Initialize the numdiff actuation data.
Definition: actuation.hpp:121
VectorXs dx
State disturbance.
Definition: actuation.hpp:140