Crocoddyl
 
Loading...
Searching...
No Matches
floating-base-thrusters.hpp
1
2// BSD 3-Clause License
3//
4// Copyright (C) 2014-2025, Heriot-Watt University
5// Copyright note valid unless otherwise stated in individual files.
6// All rights reserved.
8
9#ifndef CROCODDYL_MULTIBODY_ACTUATIONS_PROPELLERS_HPP_
10#define CROCODDYL_MULTIBODY_ACTUATIONS_PROPELLERS_HPP_
11
12#include "crocoddyl/core/actuation-base.hpp"
13#include "crocoddyl/multibody/states/multibody.hpp"
14
15namespace crocoddyl {
16
17enum ThrusterType { CW = 0, CCW };
18
19template <typename _Scalar>
21 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
22
23 typedef _Scalar Scalar;
24 typedef pinocchio::SE3Tpl<Scalar> SE3;
26
37 ThrusterTpl(const SE3& pose, const Scalar ctorque,
38 const ThrusterType type = CW,
39 const Scalar min_thrust = Scalar(0.),
40 const Scalar max_thrust = std::numeric_limits<Scalar>::infinity())
41 : pose(pose),
43 type(type),
46
57 ThrusterTpl(const Scalar ctorque, const ThrusterType type = CW,
58 const Scalar min_thrust = Scalar(0.),
59 const Scalar max_thrust = std::numeric_limits<Scalar>::infinity())
60 : pose(SE3::Identity()),
62 type(type),
66 : pose(clone.pose),
67 ctorque(clone.ctorque),
68 type(clone.type),
70 max_thrust(clone.max_thrust) {}
71
72 template <typename NewScalar>
73 ThrusterTpl<NewScalar> cast() const {
74 typedef ThrusterTpl<NewScalar> ReturnType;
75 ReturnType ret(
76 pose.template cast<NewScalar>(), scalar_cast<NewScalar>(ctorque), type,
77 scalar_cast<NewScalar>(min_thrust), scalar_cast<NewScalar>(max_thrust));
78 return ret;
79 }
80
81 ThrusterTpl& operator=(const ThrusterTpl<Scalar>& other) {
82 if (this != &other) {
83 pose = other.pose;
84 ctorque = other.ctorque;
85 type = other.type;
86 min_thrust = other.min_thrust;
87 max_thrust = other.max_thrust;
88 }
89 return *this;
90 }
91
92 template <typename OtherScalar>
93 bool operator==(const ThrusterTpl<OtherScalar>& other) const {
94 return (pose == other.pose && ctorque == other.ctorque &&
95 type == other.type && min_thrust == other.min_thrust &&
96 max_thrust == other.max_thrust);
97 }
98
99 friend std::ostream& operator<<(std::ostream& os,
100 const ThrusterTpl<Scalar>& X) {
101 os << " pose:" << std::endl
102 << X.pose << " ctorque: " << X.ctorque << std::endl
103 << " type: " << X.type << std::endl
104 << "min_thrust: " << X.min_thrust << std::endl
105 << "max_thrust: " << X.max_thrust << std::endl;
106 return os;
107 }
108
109 SE3 pose;
110 Scalar ctorque;
111 ThrusterType type;
113 Scalar min_thrust;
114 Scalar max_thrust;
115};
116
135template <typename _Scalar>
137 : public ActuationModelAbstractTpl<_Scalar> {
138 public:
139 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
140 CROCODDYL_DERIVED_CAST(ActuationModelBase,
142
143 typedef _Scalar Scalar;
149 typedef typename MathBase::Vector3s Vector3s;
150 typedef typename MathBase::VectorXs VectorXs;
151 typedef typename MathBase::MatrixXs MatrixXs;
152
160 ActuationModelFloatingBaseThrustersTpl(std::shared_ptr<StateMultibody> state,
161 const std::vector<Thruster>& thrusters)
162 : Base(state,
163 state->get_nv() -
164 state->get_pinocchio()
165 ->joints[(
166 state->get_pinocchio()->existJointName("root_joint")
167 ? state->get_pinocchio()->getJointId("root_joint")
168 : 0)]
169 .nv() +
170 thrusters.size()),
171 thrusters_(thrusters),
172 n_thrusters_(thrusters.size()),
173 W_thrust_(state_->get_nv(), nu_),
174 update_data_(true) {
175 if (!state->get_pinocchio()->existJointName("root_joint")) {
176 throw_pretty(
177 "Invalid argument: "
178 << "the first joint has to be a root one (e.g., free-flyer joint)");
179 }
180 // Update the joint actuation part
181 W_thrust_.setZero();
182 if (nu_ > n_thrusters_) {
183 W_thrust_.bottomRightCorner(nu_ - n_thrusters_, nu_ - n_thrusters_)
184 .diagonal()
185 .setOnes();
186 }
187 // Update the floating base actuation part
189 }
190 virtual ~ActuationModelFloatingBaseThrustersTpl() = default;
191
200 virtual void calc(const std::shared_ptr<Data>& data,
201 const Eigen::Ref<const VectorXs>&,
202 const Eigen::Ref<const VectorXs>& u) override {
203 if (static_cast<std::size_t>(u.size()) != nu_) {
204 throw_pretty(
205 "Invalid argument: " << "u has wrong dimension (it should be " +
206 std::to_string(nu_) + ")");
207 }
208 if (update_data_) {
209 updateData(data);
210 }
211 data->tau.noalias() = data->dtau_du * u;
212 }
213
222#ifndef NDEBUG
223 virtual void calcDiff(const std::shared_ptr<Data>& data,
224 const Eigen::Ref<const VectorXs>&,
225 const Eigen::Ref<const VectorXs>&) override {
226#else
227 virtual void calcDiff(const std::shared_ptr<Data>&,
228 const Eigen::Ref<const VectorXs>&,
229 const Eigen::Ref<const VectorXs>&) override {
230#endif
231 // The derivatives has constant values which were set in createData.
232 assert_pretty(MatrixXs(data->dtau_du).isApprox(W_thrust_),
233 "dtau_du has wrong value");
234 }
235
236 virtual void commands(const std::shared_ptr<Data>& data,
237 const Eigen::Ref<const VectorXs>&,
238 const Eigen::Ref<const VectorXs>& tau) override {
239 data->u.noalias() = data->Mtau * tau;
240 }
241
242#ifndef NDEBUG
243 virtual void torqueTransform(const std::shared_ptr<Data>& data,
244 const Eigen::Ref<const VectorXs>&,
245 const Eigen::Ref<const VectorXs>&) override {
246#else
247 virtual void torqueTransform(const std::shared_ptr<Data>&,
248 const Eigen::Ref<const VectorXs>&,
249 const Eigen::Ref<const VectorXs>&) override {
250#endif
251 // The torque transform has constant values which were set in createData.
252 assert_pretty(MatrixXs(data->Mtau).isApprox(Mtau_), "Mtau has wrong value");
253 }
254
260 virtual std::shared_ptr<Data> createData() override {
261 std::shared_ptr<Data> data =
262 std::allocate_shared<Data>(Eigen::aligned_allocator<Data>(), this);
263 updateData(data);
264 return data;
265 }
266
267 template <typename NewScalar>
270 typedef StateMultibodyTpl<NewScalar> StateType;
271 typedef ThrusterTpl<NewScalar> ThrusterType;
272 std::vector<ThrusterType> thrusters = vector_cast<NewScalar>(thrusters_);
273 ReturnType ret(
274 std::static_pointer_cast<StateType>(state_->template cast<NewScalar>()),
275 thrusters);
276 return ret;
277 }
278
282 const std::vector<Thruster>& get_thrusters() const { return thrusters_; }
283
287 std::size_t get_nthrusters() const { return n_thrusters_; }
288
297 void set_thrusters(const std::vector<Thruster>& thrusters) {
298 if (static_cast<std::size_t>(thrusters.size()) != n_thrusters_) {
299 throw_pretty("Invalid argument: "
300 << "the number of thrusters is wrong (it should be " +
301 std::to_string(n_thrusters_) + ")");
302 }
303 thrusters_ = thrusters;
304 // Update the mapping matrix from thrusters thrust to body force/moments
305 for (std::size_t i = 0; i < n_thrusters_; ++i) {
306 const Thruster& p = thrusters_[i];
307 const Vector3s& f_z = p.pose.rotation() * Vector3s::UnitZ();
308 W_thrust_.template topRows<3>().col(i) += f_z;
309 W_thrust_.template middleRows<3>(3).col(i).noalias() +=
310 p.pose.translation().cross(f_z);
311 switch (p.type) {
312 case CW:
313 W_thrust_.template middleRows<3>(3).col(i) += p.ctorque * f_z;
314 break;
315 case CCW:
316 W_thrust_.template middleRows<3>(3).col(i) -= p.ctorque * f_z;
317 break;
318 }
319 }
320 // Compute the torque transform matrix from generalized torques to joint
321 // torque inputs
322 Mtau_ = pseudoInverse(W_thrust_);
323 S_.noalias() = W_thrust_ * Mtau_;
324 update_data_ = true;
325 }
326
327 const MatrixXs& get_Wthrust() const { return W_thrust_; }
328
329 const MatrixXs& get_S() const { return S_; }
330
331 void print(std::ostream& os) const override {
332 os << "ActuationModelFloatingBaseThrusters {nu=" << nu_
333 << ", nthrusters=" << n_thrusters_ << ", thrusters=" << std::endl;
334 for (std::size_t i = 0; i < n_thrusters_; ++i) {
335 os << std::to_string(i) << ": " << thrusters_[i];
336 }
337 os << "}";
338 }
339
340 protected:
341 std::vector<Thruster> thrusters_;
342 std::size_t n_thrusters_;
343 MatrixXs W_thrust_;
344 MatrixXs Mtau_;
346 MatrixXs S_;
347
348 bool update_data_;
349 using Base::nu_;
350 using Base::state_;
351
352 private:
353 void updateData(const std::shared_ptr<Data>& data) {
354 data->dtau_du = W_thrust_;
355 data->Mtau = Mtau_;
356 const std::size_t nv = state_->get_nv();
357 for (std::size_t k = 0; k < nv; ++k) {
358 data->tau_set[k] = if_static(S_(k, k));
359 }
360 update_data_ = false;
361 }
362
363 // Use for floating-point types
364 template <typename Scalar>
365 typename std::enable_if<std::is_floating_point<Scalar>::value, bool>::type
366 if_static(const Scalar& condition) {
367 return (fabs(condition) < std::numeric_limits<Scalar>::epsilon()) ? false
368 : true;
369 }
370
371#ifdef CROCODDYL_WITH_CODEGEN
372 // Use for CppAD types
373 template <typename Scalar>
374 typename std::enable_if<!std::is_floating_point<Scalar>::value, bool>::type
375 if_static(const Scalar& condition) {
376 return CppAD::Value(CppAD::fabs(condition)) >=
377 CppAD::numeric_limits<Scalar>::epsilon();
378 }
379#endif
380};
381
382} // namespace crocoddyl
383
384CROCODDYL_DECLARE_EXTERN_TEMPLATE_STRUCT(crocoddyl::ThrusterTpl)
385CROCODDYL_DECLARE_EXTERN_TEMPLATE_CLASS(
387
388#endif // CROCODDYL_MULTIBODY_ACTUATIONS_PROPELLERS_HPP_
Abstract class for the actuation-mapping model.
std::shared_ptr< StateAbstract > state_
Model of the state.
std::size_t nu_
Dimension of joint torque inputs.
Actuation models for floating base systems actuated with thrusters.
MatrixXs S_
Selection matrix for under-actuation part.
std::shared_ptr< StateAbstract > state_
Model of the state.
virtual void calc(const std::shared_ptr< Data > &data, const Eigen::Ref< const VectorXs > &, const Eigen::Ref< const VectorXs > &u) override
Compute the actuation signal and actuation set from its thrust and joint torque inputs .
virtual std::shared_ptr< Data > createData() override
Create the floating base thruster actuation data.
ActuationModelFloatingBaseThrustersTpl(std::shared_ptr< StateMultibody > state, const std::vector< Thruster > &thrusters)
Initialize the floating base actuation model equipped with thrusters.
virtual void commands(const std::shared_ptr< Data > &data, const Eigen::Ref< const VectorXs > &, const Eigen::Ref< const VectorXs > &tau) override
Compute the joint torque input from the generalized torques.
std::vector< Thruster > thrusters_
Vector of thrusters.
virtual void torqueTransform(const std::shared_ptr< Data > &data, const Eigen::Ref< const VectorXs > &, const Eigen::Ref< const VectorXs > &) override
Compute the torque transform from generalized torques to joint torque inputs.
std::size_t nu_
Dimension of joint torque inputs.
std::size_t get_nthrusters() const
Return the number of thrusters.
virtual void calcDiff(const std::shared_ptr< Data > &data, const Eigen::Ref< const VectorXs > &, const Eigen::Ref< const VectorXs > &) override
Compute the Jacobians of the floating base thruster actuation function.
MatrixXs W_thrust_
Matrix from thrusters thrusts to body wrench.
void set_thrusters(const std::vector< Thruster > &thrusters)
Modify the vector of thrusters.
void print(std::ostream &os) const override
Print relevant information of the residual model.
const std::vector< Thruster > & get_thrusters() const
Return the vector of thrusters.
State multibody representation.
Definition multibody.hpp:34
ThrusterTpl(const SE3 &pose, const Scalar ctorque, const ThrusterType type=CW, const Scalar min_thrust=Scalar(0.), const Scalar max_thrust=std::numeric_limits< Scalar >::infinity())
Initialize the thruster in a give pose from the root joint.
ThrusterTpl(const Scalar ctorque, const ThrusterType type=CW, const Scalar min_thrust=Scalar(0.), const Scalar max_thrust=std::numeric_limits< Scalar >::infinity())
Initialize the thruster in a pose in the origin of the root joint.
Scalar ctorque
Coefficient of generated torque per thrust.