floating-base-thrusters.hpp

// BSD 3-Clause License
//
// Copyright (C) 2014-2024, Heriot-Watt University
// Copyright note valid unless otherwise stated in individual files.
// All rights reserved.
 
#ifndef CROCODDYL_MULTIBODY_ACTUATIONS_PROPELLERS_HPP_
#define CROCODDYL_MULTIBODY_ACTUATIONS_PROPELLERS_HPP_
 
#include <pinocchio/multibody/fwd.hpp>
#include <pinocchio/spatial/se3.hpp>
#include <vector>
 
#include "crocoddyl/core/actuation-base.hpp"
#include "crocoddyl/core/utils/exception.hpp"
#include "crocoddyl/multibody/states/multibody.hpp"
 
namespace crocoddyl {
 
enum ThrusterType { CW = 0, CCW };
 
template <typename _Scalar>
struct ThrusterTpl {
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 
  typedef _Scalar Scalar;
  typedef pinocchio::SE3Tpl<Scalar> SE3;
  typedef ThrusterTpl<Scalar> Thruster;
 
  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())
      : pose(pose),
        ctorque(ctorque),
        type(type),
        min_thrust(min_thrust),
        max_thrust(max_thrust) {}
 
  ThrusterTpl(const Scalar ctorque, const ThrusterType type = CW,
              const Scalar min_thrust = Scalar(0.),
              const Scalar max_thrust = std::numeric_limits<Scalar>::infinity())
      : pose(SE3::Identity()),
        ctorque(ctorque),
        type(type),
        min_thrust(min_thrust),
        max_thrust(max_thrust) {}
  ThrusterTpl(const ThrusterTpl<Scalar>& clone)
      : pose(clone.pose),
        ctorque(clone.ctorque),
        type(clone.type),
        min_thrust(clone.min_thrust),
        max_thrust(clone.max_thrust) {}
 
  ThrusterTpl& operator=(const ThrusterTpl<Scalar>& other) {
    if (this != &other) {
      pose = other.pose;
      ctorque = other.ctorque;
      type = other.type;
      min_thrust = other.min_thrust;
      max_thrust = other.max_thrust;
    }
    return *this;
  }
 
  template <typename OtherScalar>
  bool operator==(const ThrusterTpl<OtherScalar>& other) const {
    return (pose == other.pose && ctorque == other.ctorque &&
            type == other.type && min_thrust == other.min_thrust &&
            max_thrust == other.max_thrust);
  }
 
  friend std::ostream& operator<<(std::ostream& os,
                                  const ThrusterTpl<Scalar>& X) {
    os << "      pose:" << std::endl
       << X.pose << "   ctorque: " << X.ctorque << std::endl
       << "      type: " << X.type << std::endl
       << "min_thrust: " << X.min_thrust << std::endl
       << "max_thrust: " << X.max_thrust << std::endl;
    return os;
  }
 
  SE3 pose;           
  Scalar ctorque;     
  ThrusterType type;  
  Scalar min_thrust;  
  Scalar max_thrust;  
};
 
template <typename _Scalar>
class ActuationModelFloatingBaseThrustersTpl
    : public ActuationModelAbstractTpl<_Scalar> {
 public:
  typedef _Scalar Scalar;
  typedef MathBaseTpl<Scalar> MathBase;
  typedef ActuationModelAbstractTpl<Scalar> Base;
  typedef ActuationDataAbstractTpl<Scalar> Data;
  typedef StateMultibodyTpl<Scalar> StateMultibody;
  typedef ThrusterTpl<Scalar> Thruster;
  typedef typename MathBase::Vector3s Vector3s;
  typedef typename MathBase::VectorXs VectorXs;
  typedef typename MathBase::MatrixXs MatrixXs;
 
  ActuationModelFloatingBaseThrustersTpl(
      boost::shared_ptr<StateMultibody> state,
      const std::vector<Thruster>& thrusters)
      : Base(state,
             state->get_nv() -
                 state->get_pinocchio()
                     ->joints[(
                         state->get_pinocchio()->existJointName("root_joint")
                             ? state->get_pinocchio()->getJointId("root_joint")
                             : 0)]
                     .nv() +
                 thrusters.size()),
        thrusters_(thrusters),
        n_thrusters_(thrusters.size()),
        W_thrust_(state_->get_nv(), nu_),
        update_data_(true) {
    if (!state->get_pinocchio()->existJointName("root_joint")) {
      throw_pretty(
          "Invalid argument: "
          << "the first joint has to be a root one (e.g., free-flyer joint)");
    }
    // Update the joint actuation part
    W_thrust_.setZero();
    if (nu_ > n_thrusters_) {
      W_thrust_.bottomRightCorner(nu_ - n_thrusters_, nu_ - n_thrusters_)
          .diagonal()
          .setOnes();
    }
    // Update the floating base actuation part
    set_thrusters(thrusters_);
  }
  virtual ~ActuationModelFloatingBaseThrustersTpl() {}
 
  virtual void calc(const boost::shared_ptr<Data>& data,
                    const Eigen::Ref<const VectorXs>&,
                    const Eigen::Ref<const VectorXs>& u) {
    if (static_cast<std::size_t>(u.size()) != nu_) {
      throw_pretty("Invalid argument: "
                   << "u has wrong dimension (it should be " +
                          std::to_string(nu_) + ")");
    }
    if (update_data_) {
      updateData(data);
    }
    data->tau.noalias() = data->dtau_du * u;
  }
 
#ifndef NDEBUG
  virtual void calcDiff(const boost::shared_ptr<Data>& data,
                        const Eigen::Ref<const VectorXs>&,
                        const Eigen::Ref<const VectorXs>&) {
#else
  virtual void calcDiff(const boost::shared_ptr<Data>&,
                        const Eigen::Ref<const VectorXs>&,
                        const Eigen::Ref<const VectorXs>&) {
#endif
    // The derivatives has constant values which were set in createData.
    assert_pretty(MatrixXs(data->dtau_du).isApprox(W_thrust_),
                  "dtau_du has wrong value");
  }
 
  virtual void commands(const boost::shared_ptr<Data>& data,
                        const Eigen::Ref<const VectorXs>&,
                        const Eigen::Ref<const VectorXs>& tau) {
    data->u.noalias() = data->Mtau * tau;
  }
 
#ifndef NDEBUG
  virtual void torqueTransform(const boost::shared_ptr<Data>& data,
                               const Eigen::Ref<const VectorXs>&,
                               const Eigen::Ref<const VectorXs>&) {
#else
  virtual void torqueTransform(const boost::shared_ptr<Data>&,
                               const Eigen::Ref<const VectorXs>&,
                               const Eigen::Ref<const VectorXs>&) {
#endif
    // The torque transform has constant values which were set in createData.
    assert_pretty(MatrixXs(data->Mtau).isApprox(Mtau_), "Mtau has wrong value");
  }
 
  virtual boost::shared_ptr<Data> createData() {
    boost::shared_ptr<Data> data =
        boost::allocate_shared<Data>(Eigen::aligned_allocator<Data>(), this);
    updateData(data);
    return data;
  }
 
  const std::vector<Thruster>& get_thrusters() const { return thrusters_; }
 
  std::size_t get_nthrusters() const { return n_thrusters_; }
 
  void set_thrusters(const std::vector<Thruster>& thrusters) {
    if (static_cast<std::size_t>(thrusters.size()) != n_thrusters_) {
      throw_pretty("Invalid argument: "
                   << "the number of thrusters is wrong (it should be " +
                          std::to_string(n_thrusters_) + ")");
    }
    thrusters_ = thrusters;
    // Update the mapping matrix from thrusters thrust to body force/moments
    for (std::size_t i = 0; i < n_thrusters_; ++i) {
      const Thruster& p = thrusters_[i];
      const Vector3s& f_z = p.pose.rotation() * Vector3s::UnitZ();
      W_thrust_.template topRows<3>().col(i) += f_z;
      W_thrust_.template middleRows<3>(3).col(i).noalias() +=
          p.pose.translation().cross(Vector3s::UnitZ());
      switch (p.type) {
        case CW:
          W_thrust_.template middleRows<3>(3).col(i) += p.ctorque * f_z;
          break;
        case CCW:
          W_thrust_.template middleRows<3>(3).col(i) -= p.ctorque * f_z;
          break;
      }
    }
    // Compute the torque transform matrix from generalized torques to joint
    // torque inputs
    Mtau_ = pseudoInverse(MatrixXs(W_thrust_));
    S_.noalias() = W_thrust_ * Mtau_;
    update_data_ = true;
  }
 
  const MatrixXs& get_Wthrust() const { return W_thrust_; }
 
  const MatrixXs& get_S() const { return S_; }
 
  void print(std::ostream& os) const {
    os << "ActuationModelFloatingBaseThrusters {nu=" << nu_
       << ", nthrusters=" << n_thrusters_ << ", thrusters=" << std::endl;
    for (std::size_t i = 0; i < n_thrusters_; ++i) {
      os << std::to_string(i) << ": " << thrusters_[i];
    }
    os << "}";
  }
 
 protected:
  std::vector<Thruster> thrusters_;  
  std::size_t n_thrusters_;          
  MatrixXs W_thrust_;  
  MatrixXs Mtau_;  
  MatrixXs S_;     
 
  bool update_data_;
  using Base::nu_;
  using Base::state_;
 
 private:
  void updateData(const boost::shared_ptr<Data>& data) {
    data->dtau_du = W_thrust_;
    data->Mtau = Mtau_;
    const std::size_t nv = state_->get_nv();
    for (std::size_t k = 0; k < nv; ++k) {
      if (fabs(S_(k, k)) < std::numeric_limits<Scalar>::epsilon()) {
        data->tau_set[k] = false;
      } else {
        data->tau_set[k] = true;
      }
    }
    update_data_ = false;
  }
};
 
}  // namespace crocoddyl
 
#endif  // CROCODDYL_MULTIBODY_ACTUATIONS_PROPELLERS_HPP_