1 |
|
|
/////////////////////////////////////////////////////////////////////////////// |
2 |
|
|
// BSD 3-Clause License |
3 |
|
|
// |
4 |
|
|
// Copyright (C) 2019-2024, LAAS-CNRS, IRI: CSIC-UPC, University of Edinburgh |
5 |
|
|
// Copyright note valid unless otherwise stated in individual files. |
6 |
|
|
// All rights reserved. |
7 |
|
|
/////////////////////////////////////////////////////////////////////////////// |
8 |
|
|
|
9 |
|
|
#ifndef CROCODDYL_MULTIBODY_ACTUATIONS_MULTICOPTER_BASE_HPP_ |
10 |
|
|
#define CROCODDYL_MULTIBODY_ACTUATIONS_MULTICOPTER_BASE_HPP_ |
11 |
|
|
|
12 |
|
|
#include <iostream> |
13 |
|
|
|
14 |
|
|
#include "crocoddyl/core/actuation-base.hpp" |
15 |
|
|
#include "crocoddyl/core/utils/deprecate.hpp" |
16 |
|
|
#include "crocoddyl/core/utils/exception.hpp" |
17 |
|
|
#include "crocoddyl/multibody/fwd.hpp" |
18 |
|
|
#include "crocoddyl/multibody/states/multibody.hpp" |
19 |
|
|
|
20 |
|
|
namespace crocoddyl { |
21 |
|
|
|
22 |
|
|
/** |
23 |
|
|
* @brief Multicopter actuation model |
24 |
|
|
* |
25 |
|
|
* This actuation model is aimed for those robots whose base_link is actuated |
26 |
|
|
* using a propulsion system, e.g., a multicopter or an aerial manipulator |
27 |
|
|
* (multicopter with a robotic arm attached). Control input: the thrust (force) |
28 |
|
|
* created by each propeller. tau_f matrix: this matrix relates the thrust of |
29 |
|
|
* each propeller to the net force and torque that it causes to the base_link. |
30 |
|
|
* For a simple quadrotor: tau_f.nrows = 6, tau_f.ncols = 4 |
31 |
|
|
* |
32 |
|
|
* Both actuation and Jacobians are computed analytically by `calc` and |
33 |
|
|
* `calcDiff`, respectively. |
34 |
|
|
* |
35 |
|
|
* Reference: M. Geisert and N. Mansard, "Trajectory generation for quadrotor |
36 |
|
|
* based systems using numerical optimal control," 2016 IEEE International |
37 |
|
|
* Conference on Robotics and Automation (ICRA), Stockholm, 2016, pp. 2958-2964. |
38 |
|
|
* See Section III.C. |
39 |
|
|
* |
40 |
|
|
* \sa `ActuationModelAbstractTpl`, `calc()`, `calcDiff()`, `createData()` |
41 |
|
|
*/ |
42 |
|
|
template <typename _Scalar> |
43 |
|
|
class ActuationModelMultiCopterBaseTpl |
44 |
|
|
: public ActuationModelAbstractTpl<_Scalar> { |
45 |
|
|
public: |
46 |
|
|
typedef _Scalar Scalar; |
47 |
|
|
typedef MathBaseTpl<Scalar> MathBase; |
48 |
|
|
typedef ActuationModelAbstractTpl<Scalar> Base; |
49 |
|
|
typedef ActuationDataAbstractTpl<Scalar> Data; |
50 |
|
|
typedef StateMultibodyTpl<Scalar> StateMultibody; |
51 |
|
|
typedef typename MathBase::VectorXs VectorXs; |
52 |
|
|
typedef typename MathBase::MatrixXs MatrixXs; |
53 |
|
|
typedef typename MathBase::Matrix6xs Matrix6xs; |
54 |
|
|
|
55 |
|
|
/** |
56 |
|
|
* @brief Initialize the multicopter actuation model |
57 |
|
|
* |
58 |
|
|
* @param[in] state State of the dynamical system |
59 |
|
|
* @param[in] tau_f Matrix that maps the thrust of each propeller to the net |
60 |
|
|
* force and torque |
61 |
|
|
*/ |
62 |
|
|
DEPRECATED("Use constructor ActuationModelFloatingBaseThrustersTpl", |
63 |
|
|
ActuationModelMultiCopterBaseTpl( |
64 |
|
|
boost::shared_ptr<StateMultibody> state, |
65 |
|
|
const Eigen::Ref<const Matrix6xs>& tau_f)); |
66 |
|
|
|
67 |
|
|
DEPRECATED("Use constructor without n_rotors", |
68 |
|
|
ActuationModelMultiCopterBaseTpl( |
69 |
|
|
boost::shared_ptr<StateMultibody> state, |
70 |
|
|
const std::size_t n_rotors, |
71 |
|
|
const Eigen::Ref<const Matrix6xs>& tau_f)); |
72 |
|
|
virtual ~ActuationModelMultiCopterBaseTpl() {} |
73 |
|
|
|
74 |
|
|
virtual void calc(const boost::shared_ptr<Data>& data, |
75 |
|
|
const Eigen::Ref<const VectorXs>&, |
76 |
|
|
const Eigen::Ref<const VectorXs>& u) { |
77 |
|
|
if (static_cast<std::size_t>(u.size()) != nu_) { |
78 |
|
|
throw_pretty("Invalid argument: " |
79 |
|
|
<< "u has wrong dimension (it should be " + |
80 |
|
|
std::to_string(nu_) + ")"); |
81 |
|
|
} |
82 |
|
|
|
83 |
|
|
data->tau.noalias() = tau_f_ * u; |
84 |
|
|
} |
85 |
|
|
|
86 |
|
|
#ifndef NDEBUG |
87 |
|
|
virtual void calcDiff(const boost::shared_ptr<Data>& data, |
88 |
|
|
const Eigen::Ref<const VectorXs>&, |
89 |
|
|
const Eigen::Ref<const VectorXs>&) { |
90 |
|
|
#else |
91 |
|
|
virtual void calcDiff(const boost::shared_ptr<Data>&, |
92 |
|
|
const Eigen::Ref<const VectorXs>&, |
93 |
|
|
const Eigen::Ref<const VectorXs>&) { |
94 |
|
|
#endif |
95 |
|
|
// The derivatives has constant values which were set in createData. |
96 |
|
|
assert_pretty(MatrixXs(data->dtau_du).isApprox(tau_f_), |
97 |
|
|
"dtau_du has wrong value"); |
98 |
|
|
} |
99 |
|
|
|
100 |
|
|
virtual void commands(const boost::shared_ptr<Data>& data, |
101 |
|
|
const Eigen::Ref<const VectorXs>&, |
102 |
|
|
const Eigen::Ref<const VectorXs>& tau) { |
103 |
|
|
data->u.noalias() = Mtau_ * tau; |
104 |
|
|
} |
105 |
|
|
|
106 |
|
|
#ifndef NDEBUG |
107 |
|
|
virtual void torqueTransform(const boost::shared_ptr<Data>& data, |
108 |
|
|
const Eigen::Ref<const VectorXs>&, |
109 |
|
|
const Eigen::Ref<const VectorXs>&) { |
110 |
|
|
#else |
111 |
|
|
virtual void torqueTransform(const boost::shared_ptr<Data>&, |
112 |
|
|
const Eigen::Ref<const VectorXs>&, |
113 |
|
|
const Eigen::Ref<const VectorXs>&) { |
114 |
|
|
#endif |
115 |
|
|
// The torque transform has constant values which were set in createData. |
116 |
|
|
assert_pretty(MatrixXs(data->Mtau).isApprox(Mtau_), "Mtau has wrong value"); |
117 |
|
|
} |
118 |
|
|
|
119 |
|
|
boost::shared_ptr<Data> createData() { |
120 |
|
|
boost::shared_ptr<Data> data = |
121 |
|
|
boost::allocate_shared<Data>(Eigen::aligned_allocator<Data>(), this); |
122 |
|
|
data->dtau_du = tau_f_; |
123 |
|
|
data->Mtau = Mtau_; |
124 |
|
|
for (std::size_t i = 0; i < 2; ++i) { |
125 |
|
|
data->tau_set[i] = false; |
126 |
|
|
} |
127 |
|
|
return data; |
128 |
|
|
} |
129 |
|
|
|
130 |
|
|
std::size_t get_nrotors() const { return n_rotors_; }; |
131 |
|
|
const MatrixXs& get_tauf() const { return tau_f_; }; |
132 |
|
|
void set_tauf(const Eigen::Ref<const MatrixXs>& tau_f) { tau_f_ = tau_f; } |
133 |
|
|
|
134 |
|
|
protected: |
135 |
|
|
MatrixXs tau_f_; //!< Matrix from rotors thrust to body force/moments |
136 |
|
|
MatrixXs Mtau_; //!< Constaint torque transform from generalized torques to |
137 |
|
|
//!< joint torque inputs |
138 |
|
|
std::size_t n_rotors_; //!< Number of rotors |
139 |
|
|
|
140 |
|
|
using Base::nu_; |
141 |
|
|
using Base::state_; |
142 |
|
|
|
143 |
|
|
#ifndef NDEBUG |
144 |
|
|
private: |
145 |
|
|
MatrixXs S_; |
146 |
|
|
#endif |
147 |
|
|
}; |
148 |
|
|
|
149 |
|
|
template <typename Scalar> |
150 |
|
|
ActuationModelMultiCopterBaseTpl<Scalar>::ActuationModelMultiCopterBaseTpl( |
151 |
|
|
boost::shared_ptr<StateMultibody> state, |
152 |
|
|
const Eigen::Ref<const Matrix6xs>& tau_f) |
153 |
|
|
: Base(state, state->get_nv() - 6 + tau_f.cols()), n_rotors_(tau_f.cols()) { |
154 |
|
|
pinocchio::JointModelFreeFlyerTpl<Scalar> ff_joint; |
155 |
|
|
if (state->get_pinocchio()->joints[1].shortname() != ff_joint.shortname()) { |
156 |
|
|
throw_pretty("Invalid argument: " |
157 |
|
|
<< "the first joint has to be free-flyer"); |
158 |
|
|
} |
159 |
|
|
|
160 |
|
|
tau_f_ = MatrixXs::Zero(state_->get_nv(), nu_); |
161 |
|
|
tau_f_.block(0, 0, 6, n_rotors_) = tau_f; |
162 |
|
|
if (nu_ > n_rotors_) { |
163 |
|
|
tau_f_.bottomRightCorner(nu_ - n_rotors_, nu_ - n_rotors_) |
164 |
|
|
.diagonal() |
165 |
|
|
.setOnes(); |
166 |
|
|
} |
167 |
|
|
Mtau_ = pseudoInverse(MatrixXs(tau_f)); |
168 |
|
|
std::cerr << "Deprecated ActuationModelMultiCopterBase: Use " |
169 |
|
|
"ActuationModelFloatingBaseThrusters" |
170 |
|
|
<< std::endl; |
171 |
|
|
} |
172 |
|
|
|
173 |
|
|
template <typename Scalar> |
174 |
|
|
ActuationModelMultiCopterBaseTpl<Scalar>::ActuationModelMultiCopterBaseTpl( |
175 |
|
|
boost::shared_ptr<StateMultibody> state, const std::size_t n_rotors, |
176 |
|
|
const Eigen::Ref<const Matrix6xs>& tau_f) |
177 |
|
|
: Base(state, state->get_nv() - 6 + n_rotors), n_rotors_(n_rotors) { |
178 |
|
|
pinocchio::JointModelFreeFlyerTpl<Scalar> ff_joint; |
179 |
|
|
if (state->get_pinocchio()->joints[1].shortname() != ff_joint.shortname()) { |
180 |
|
|
throw_pretty("Invalid argument: " |
181 |
|
|
<< "the first joint has to be free-flyer"); |
182 |
|
|
} |
183 |
|
|
|
184 |
|
|
tau_f_ = MatrixXs::Zero(state_->get_nv(), nu_); |
185 |
|
|
tau_f_.block(0, 0, 6, n_rotors_) = tau_f; |
186 |
|
|
if (nu_ > n_rotors_) { |
187 |
|
|
tau_f_.bottomRightCorner(nu_ - n_rotors_, nu_ - n_rotors_) |
188 |
|
|
.diagonal() |
189 |
|
|
.setOnes(); |
190 |
|
|
} |
191 |
|
|
std::cerr << "Deprecated ActuationModelMultiCopterBase: Use constructor " |
192 |
|
|
"without n_rotors." |
193 |
|
|
<< std::endl; |
194 |
|
|
} |
195 |
|
|
|
196 |
|
|
} // namespace crocoddyl |
197 |
|
|
|
198 |
|
|
#endif // CROCODDYL_MULTIBODY_ACTUATIONS_MULTICOPTER_BASE_HPP_ |