Crocoddyl
 
Loading...
Searching...
No Matches
actuation-squashing.hpp
1
2// BSD 3-Clause License
3//
4// Copyright (C) 2019-2025, University of Edinburgh, IRI: CSIC-UPC,
5// Heriot-Watt University
6// Copyright note valid unless otherwise stated in individual files.
7// All rights reserved.
9
10#ifndef CROCODDYL_CORE_ACTUATION_SQUASHING_HPP_
11#define CROCODDYL_CORE_ACTUATION_SQUASHING_HPP_
12
13#include "crocoddyl/core/actuation-base.hpp"
14#include "crocoddyl/core/actuation/squashing-base.hpp"
15#include "crocoddyl/core/fwd.hpp"
16
17namespace crocoddyl {
18
19template <typename _Scalar>
21 public:
22 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
23 CROCODDYL_DERIVED_CAST(ActuationModelBase, ActuationSquashingModelTpl)
24
25 typedef _Scalar Scalar;
31 typedef typename MathBase::VectorXs VectorXs;
32 typedef typename MathBase::MatrixXs MatrixXs;
33
34 ActuationSquashingModelTpl(std::shared_ptr<Base> actuation,
35 std::shared_ptr<SquashingModelAbstract> squashing,
36 const std::size_t nu)
37 : Base(actuation->get_state(), nu),
38 squashing_(squashing),
39 actuation_(actuation) {};
40
41 virtual ~ActuationSquashingModelTpl() = default;
42
43 virtual void calc(const std::shared_ptr<ActuationDataAbstract>& data,
44 const Eigen::Ref<const VectorXs>& x,
45 const Eigen::Ref<const VectorXs>& u) override {
46 Data* d = static_cast<Data*>(data.get());
47
48 squashing_->calc(d->squashing, u);
49 actuation_->calc(d->actuation, x, d->squashing->u);
50 data->tau = d->actuation->tau;
51 data->tau_set = d->actuation->tau_set;
52 };
53
54 virtual void calcDiff(const std::shared_ptr<ActuationDataAbstract>& data,
55 const Eigen::Ref<const VectorXs>& x,
56 const Eigen::Ref<const VectorXs>& u) override {
57 Data* d = static_cast<Data*>(data.get());
58
59 squashing_->calcDiff(d->squashing, u);
60 actuation_->calcDiff(d->actuation, x, d->squashing->u);
61 data->dtau_du.noalias() = d->actuation->dtau_du * d->squashing->du_ds;
62 };
63
64 virtual void commands(const std::shared_ptr<ActuationDataAbstract>& data,
65 const Eigen::Ref<const VectorXs>& x,
66 const Eigen::Ref<const VectorXs>& tau) override {
67 if (static_cast<std::size_t>(tau.size()) != this->state_->get_nv()) {
68 throw_pretty("Invalid argument: "
69 << "tau has wrong dimension (it should be " +
70 std::to_string(this->state_->get_nv()) + ")");
71 }
72 torqueTransform(data, x, tau);
73 data->u.noalias() = data->Mtau * tau;
74 }
75
76 std::shared_ptr<ActuationDataAbstract> createData() override {
77 return std::allocate_shared<Data>(Eigen::aligned_allocator<Data>(), this);
78 };
79
80 template <typename NewScalar>
83 ReturnType ret(actuation_->template cast<NewScalar>(),
84 squashing_->template cast<NewScalar>(), nu_);
85 return ret;
86 }
87
88 const std::shared_ptr<SquashingModelAbstract>& get_squashing() const {
89 return squashing_;
90 };
91 const std::shared_ptr<Base>& get_actuation() const { return actuation_; };
92
93 protected:
94 std::shared_ptr<SquashingModelAbstract> squashing_;
95 std::shared_ptr<Base> actuation_;
96 using Base::nu_;
98};
99
100template <typename _Scalar>
102 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
103
104 typedef _Scalar Scalar;
108 typedef typename MathBase::VectorXs VectorXs;
109 typedef typename MathBase::MatrixXs MatrixXs;
110
111 template <template <typename Scalar> class Model>
112 explicit ActuationSquashingDataTpl(Model<Scalar>* const model)
113 : Base(model),
114 squashing(model->get_squashing()->createData()),
115 actuation(model->get_actuation()->createData()) {}
116
117 virtual ~ActuationSquashingDataTpl() = default;
118
119 std::shared_ptr<SquashingDataAbstract> squashing;
120 std::shared_ptr<Base> actuation;
121
122 using Base::dtau_du;
123 using Base::dtau_dx;
124 using Base::Mtau;
125 using Base::tau;
126 using Base::tau_set;
127 using Base::u;
128};
129
130} // namespace crocoddyl
131
132CROCODDYL_DECLARE_EXTERN_TEMPLATE_CLASS(crocoddyl::ActuationSquashingModelTpl)
133CROCODDYL_DECLARE_EXTERN_TEMPLATE_STRUCT(crocoddyl::ActuationSquashingDataTpl)
134
135#endif // CROCODDYL_CORE_ACTIVATION_SQUASH_BASE_HPP_
Abstract class for the actuation-mapping model.
std::shared_ptr< StateAbstract > state_
Model of the state.
virtual void torqueTransform(const std::shared_ptr< ActuationDataAbstract > &data, const Eigen::Ref< const VectorXs > &x, const Eigen::Ref< const VectorXs > &u)
Compute the torque transform from generalized torques to joint torque inputs.
std::size_t nu_
Dimension of joint torque inputs.
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 .
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.
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.
std::shared_ptr< ActuationDataAbstract > createData() override
Create the actuation data.
virtual void torqueTransform(const std::shared_ptr< ActuationDataAbstract > &data, const Eigen::Ref< const VectorXs > &x, const Eigen::Ref< const VectorXs > &u)
Compute the torque transform from generalized torques to joint torque inputs.
std::size_t nu_
Dimension of joint torque inputs.
std::vector< bool > tau_set
True for joints that are actuacted.
VectorXs tau
Generalized torques.