crocoddyl  1.6.0
Contact RObot COntrol by Differential DYnamic programming Library (Crocoddyl)
multicopter-base.hpp
1 // BSD 3-Clause License
3 //
4 // Copyright (C) 2019-2020, LAAS-CNRS, IRI: CSIC-UPC
5 // Copyright note valid unless otherwise stated in individual files.
6 // All rights reserved.
8 
9 #ifndef CROCODDYL_MULTIBODY_ACTUATIONS_MULTICOPTER_BASE_HPP_
10 #define CROCODDYL_MULTIBODY_ACTUATIONS_MULTICOPTER_BASE_HPP_
11 
12 #include "crocoddyl/multibody/fwd.hpp"
13 #include "crocoddyl/core/utils/exception.hpp"
14 #include "crocoddyl/core/actuation-base.hpp"
15 #include "crocoddyl/multibody/states/multibody.hpp"
16 
17 /* This actuation model is aimed for those robots whose base_link is actuated using a propulsion system, e.g.
18  * a multicopter or an aerial manipulator (multicopter with a robotic arm attached).
19  * Control input: the thrust (force) created by each propeller.
20  * tau_f matrix: this matrix relates the thrust of each propeller to the net force and torque that it causes to the
21  * base_link. For a simple quadrotor: tau_f.nrows = 6, tau_f.ncols = 4
22  *
23  * Reference: M. Geisert and N. Mansard, "Trajectory generation for quadrotor based systems using numerical optimal
24  * control," 2016 IEEE International Conference on Robotics and Automation (ICRA), Stockholm, 2016, pp. 2958-2964. See
25  * Section III.C */
26 
27 namespace crocoddyl {
28 template <typename _Scalar>
30  public:
31  typedef _Scalar Scalar;
36  typedef typename MathBase::VectorXs VectorXs;
37  typedef typename MathBase::MatrixXs MatrixXs;
38 
39  ActuationModelMultiCopterBaseTpl(boost::shared_ptr<StateMultibody> state, const std::size_t& n_rotors,
40  const Eigen::Ref<const MatrixXs>& tau_f)
41  : Base(state, state->get_nv() - 6 + n_rotors), n_rotors_(n_rotors) {
42  pinocchio::JointModelFreeFlyerTpl<Scalar> ff_joint;
43  if (state->get_pinocchio()->joints[1].shortname() != ff_joint.shortname()) {
44  throw_pretty("Invalid argument: "
45  << "the first joint has to be free-flyer");
46  }
47 
48  tau_f_ = MatrixXs::Zero(state_->get_nv(), nu_);
49  tau_f_.block(0, 0, 6, n_rotors_) = tau_f;
50  if (nu_ > n_rotors_) {
51  tau_f_.bottomRightCorner(nu_ - n_rotors_, nu_ - n_rotors_) =
52  MatrixXs::Identity(nu_ - n_rotors_, nu_ - n_rotors_);
53  }
54  };
56 
57  virtual void calc(const boost::shared_ptr<Data>& data, const Eigen::Ref<const VectorXs>& /*x*/,
58  const Eigen::Ref<const VectorXs>& u) {
59  if (static_cast<std::size_t>(u.size()) != nu_) {
60  throw_pretty("Invalid argument: "
61  << "u has wrong dimension (it should be " + std::to_string(nu_) + ")");
62  }
63 
64  data->tau.noalias() = tau_f_ * u;
65  }
66 
67  virtual void calcDiff(const boost::shared_ptr<Data>& /*data*/, const Eigen::Ref<const VectorXs>& /*x*/,
68  const Eigen::Ref<const VectorXs>& /*u*/) {
69  // The derivatives has constant values which were set in createData.
70  }
71 
72  boost::shared_ptr<Data> createData() {
73  boost::shared_ptr<Data> data = boost::allocate_shared<Data>(Eigen::aligned_allocator<Data>(), this);
74  data->dtau_du = tau_f_;
75  return data;
76  }
77 
78  const std::size_t& get_nrotors() const { return n_rotors_; };
79  const MatrixXs& get_tauf() const { return tau_f_; };
80  void set_tauf(const Eigen::Ref<const MatrixXs>& tau_f) { tau_f_ = tau_f; }
81 
82  protected:
83  // Specific of multicopter
84  MatrixXs tau_f_; // Matrix from rotors thrust to body force/moments
85  std::size_t n_rotors_;
86 
87  using Base::nu_;
88  using Base::state_;
89 };
90 
91 } // namespace crocoddyl
92 
93 #endif // CROCODDYL_MULTIBODY_ACTUATIONS_MULTICOPTER_BASE_HPP_