crocoddyl 1.9.0
Contact RObot COntrol by Differential DYnamic programming Library (Crocoddyl)
 
Loading...
Searching...
No Matches
actuation-squashing.hpp
1
2// BSD 3-Clause License
3//
4// Copyright (C) 2019-2021, University of Edinburgh, IRI: CSIC-UPC
5// Copyright note valid unless otherwise stated in individual files.
6// All rights reserved.
8
9#ifndef CROCODDYL_CORE_ACTUATION_SQUASHING_HPP_
10#define CROCODDYL_CORE_ACTUATION_SQUASHING_HPP_
11
12#include "crocoddyl/core/fwd.hpp"
13#include "crocoddyl/core/actuation-base.hpp"
14#include "crocoddyl/core/actuation/squashing-base.hpp"
15
16namespace crocoddyl {
17
18template <typename _Scalar>
20 public:
21 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
22
23 typedef _Scalar Scalar;
30 typedef typename MathBase::VectorXs VectorXs;
31 typedef typename MathBase::MatrixXs MatrixXs;
32
33 ActuationSquashingModelTpl(boost::shared_ptr<ActuationModelAbstract> actuation,
34 boost::shared_ptr<SquashingModelAbstract> squashing, const std::size_t nu)
35 : Base(actuation->get_state(), nu), squashing_(squashing), actuation_(actuation){};
36
38
39 virtual void calc(const boost::shared_ptr<ActuationDataAbstract>& data, const Eigen::Ref<const VectorXs>& x,
40 const Eigen::Ref<const VectorXs>& u) {
41 Data* d = static_cast<Data*>(data.get());
42
43 squashing_->calc(d->squashing, u);
44 actuation_->calc(d->actuation, x, d->squashing->u);
45 data->tau = d->actuation->tau;
46 };
47
48 virtual void calcDiff(const boost::shared_ptr<ActuationDataAbstract>& data, const Eigen::Ref<const VectorXs>& x,
49 const Eigen::Ref<const VectorXs>& u) {
50 Data* d = static_cast<Data*>(data.get());
51
52 squashing_->calcDiff(d->squashing, u);
53 actuation_->calcDiff(d->actuation, x, d->squashing->u);
54 data->dtau_du.noalias() = d->actuation->dtau_du * d->squashing->du_ds;
55 };
56
57 boost::shared_ptr<ActuationDataAbstract> createData() {
58 return boost::allocate_shared<Data>(Eigen::aligned_allocator<Data>(), this);
59 };
60
61 const boost::shared_ptr<SquashingModelAbstract>& get_squashing() const { return squashing_; };
62 const boost::shared_ptr<ActuationModelAbstract>& get_actuation() const { return actuation_; };
63
64 protected:
65 boost::shared_ptr<SquashingModelAbstract> squashing_;
66 boost::shared_ptr<ActuationModelAbstract> actuation_;
67};
68
69template <typename _Scalar>
71 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
72
73 typedef _Scalar Scalar;
77 typedef typename MathBase::VectorXs VectorXs;
78 typedef typename MathBase::MatrixXs MatrixXs;
79
80 template <template <typename Scalar> class Model>
81 explicit ActuationSquashingDataTpl(Model<Scalar>* const model)
82 : Base(model),
83 squashing(model->get_squashing()->createData()),
84 actuation(model->get_actuation()->createData()) {}
85
87
88 boost::shared_ptr<SquashingDataAbstract> squashing;
89 boost::shared_ptr<ActuationDataAbstract> actuation;
90
91 using Base::dtau_du;
92 using Base::dtau_dx;
93 using Base::tau;
94};
95
96} // namespace crocoddyl
97
98#endif // CROCODDYL_CORE_ACTIVATION_SQUASH_BASE_HPP_
Abstract class for the actuation-mapping model.
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 .
boost::shared_ptr< ActuationDataAbstract > createData()
Create the actuation data.
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.