Head

GCC Code Coverage Report

Directory:	./		Exec	Total	Coverage
File:	include/crocoddyl/multibody/actuations/floating-base-thrusters.hpp	Lines:	73	108	67.6 %
Date:	2024-02-13 11:12:33	Branches:	70	213	32.9 %


///////////////////////////////////////////////////////////////////////////////
// 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;

  /**
   * @brief Initialize the thruster in a give pose from the root joint.
   *
   * @param pose[in]     Pose from root joint
   * @param ctorque[in]  Coefficient of generated torque per thrust
   * @param type[in]     Type of thruster (clockwise or counterclockwise,
   * default clockwise)
   * @param[in] min_thrust[in]  Minimum thrust (default 0.)
   * @param[in] max_thrust[in]  Maximum thrust (default inf number))
   */
  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) {}

  /**
   * @brief Initialize the thruster in a pose in the origin of the root joint.
   *
   * @param pose[in]     Pose from root joint
   * @param ctorque[in]  Coefficient of generated torque per thrust
   * @param type[in]     Type of thruster (clockwise or counterclockwise,
   * default clockwise)
   * @param[in] min_thrust[in]  Minimum thrust (default 0.)
   * @param[in] max_thrust[in]  Maximum thrust (default inf number))
   */
  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;           //!< Thruster pose
  Scalar ctorque;     //!< Coefficient of generated torque per thrust
  ThrusterType type;  //!< Type of thruster (CW and CCW for clockwise and
                      //!< counterclockwise, respectively)
  Scalar min_thrust;  //!< Minimum thrust
  Scalar max_thrust;  //!< Minimum thrust
};

/**
 * @brief Actuation models for floating base systems actuated with thrusters
 *
 * This actuation model models floating base robots equipped with thrusters,
 * e.g., multicopters or marine robots equipped with manipulators. It control
 * inputs are the thrusters' thrust (i.e., forces) and joint torques.
 *
 * Both actuation and Jacobians are computed analytically by `calc` and
 * `calcDiff`, respectively.
 *
 * We assume the robot velocity to zero for easily related square thruster
 * velocities with thrust and torque generated. This approach is similarly
 * implemented in M. Geisert and N. Mansard, "Trajectory generation for
 * quadrotor based systems using numerical optimal control", (ICRA). See Section
 * III.C.
 *
 * \sa `ActuationModelAbstractTpl`, `calc()`, `calcDiff()`, `createData()`
 */
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;

  /**
   * @brief Initialize the floating base actuation model equipped with
   * thrusters
   *
   * @param[in] state      State of the dynamical system
   * @param[in] thrusters  Vector of thrusters
   */
  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() {}

  /**
   * @brief Compute the actuation signal and actuation set from its thrust
   * and joint torque inputs \f$\mathbf{u}\in\mathbb{R}^{nu}\f$
   *
   * @param[in] data  Floating base thrusters actuation data
   * @param[in] x     State point \f$\mathbf{x}\in\mathbb{R}^{ndx}\f$
   * @param[in] u     Joint-torque input \f$\mathbf{u}\in\mathbb{R}^{nu}\f$
   */
  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;
  }

  /**
   * @brief Compute the Jacobians of the floating base thruster actuation
   * function
   *
   * @param[in] data  Floating base thrusters actuation data
   * @param[in] x     State point \f$\mathbf{x}\in\mathbb{R}^{ndx}\f$
   * @param[in] u     Joint-torque input \f$\mathbf{u}\in\mathbb{R}^{nu}\f$
   */
#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");
  }

  /**
   * @brief Create the floating base thruster actuation data
   *
   * @return the actuation data
   */
  virtual boost::shared_ptr<Data> createData() {
    boost::shared_ptr<Data> data =
        boost::allocate_shared<Data>(Eigen::aligned_allocator<Data>(), this);
    updateData(data);
    return data;
  }

  /**
   * @brief Return the vector of thrusters
   */
  const std::vector<Thruster>& get_thrusters() const { return thrusters_; }

  /**
   * @brief Return the number of thrusters
   */
  std::size_t get_nthrusters() const { return n_thrusters_; }

  /**
   * @brief Modify the vector of thrusters
   *
   * Since we don't want to allocate data, we request to pass the same
   * number of thrusters.
   *
   * @param[in] thrusters  Vector of 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_;  //!< Vector of thrusters
  std::size_t n_thrusters_;          //!< Number of thrusters
  MatrixXs W_thrust_;  //!< Matrix from thrusters thrusts to body wrench
  MatrixXs Mtau_;  //!< Constaint torque transform from generalized torques to
                   //!< joint torque inputs
  MatrixXs S_;     //!< Selection matrix for under-actuation part

  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_


Generated by: GCOVR (Version 4.2)

Line	Branch	Exec	Source
1			///////////////////////////////////////////////////////////////////////////////
2			// BSD 3-Clause License
3			//
4			// Copyright (C) 2014-2024, Heriot-Watt University
5			// Copyright note valid unless otherwise stated in individual files.
6			// All rights reserved.
7			///////////////////////////////////////////////////////////////////////////////
8
9			#ifndef CROCODDYL_MULTIBODY_ACTUATIONS_PROPELLERS_HPP_
10			#define CROCODDYL_MULTIBODY_ACTUATIONS_PROPELLERS_HPP_
11
12			#include <pinocchio/multibody/fwd.hpp>
13			#include <pinocchio/spatial/se3.hpp>
14			#include <vector>
15
16			#include "crocoddyl/core/actuation-base.hpp"
17			#include "crocoddyl/core/utils/exception.hpp"
18			#include "crocoddyl/multibody/states/multibody.hpp"
19
20			namespace crocoddyl {
21
22			enum ThrusterType { CW = 0, CCW };
23
24			template <typename _Scalar>
25			struct ThrusterTpl {
26			EIGEN_MAKE_ALIGNED_OPERATOR_NEW
27
28			typedef _Scalar Scalar;
29			typedef pinocchio::SE3Tpl<Scalar> SE3;
30			typedef ThrusterTpl<Scalar> Thruster;
31
32			/**
33			* @brief Initialize the thruster in a give pose from the root joint.
34			*
35			* @param pose[in] Pose from root joint
36			* @param ctorque[in] Coefficient of generated torque per thrust
37			* @param type[in] Type of thruster (clockwise or counterclockwise,
38			* default clockwise)
39			* @param[in] min_thrust[in] Minimum thrust (default 0.)
40			* @param[in] max_thrust[in] Maximum thrust (default inf number))
41			*/
42		6840	ThrusterTpl(const SE3& pose, const Scalar ctorque,
43			const ThrusterType type = CW,
44			const Scalar min_thrust = Scalar(0.),
45			const Scalar max_thrust = std::numeric_limits<Scalar>::infinity())
46			: pose(pose),
47			ctorque(ctorque),
48			type(type),
49			min_thrust(min_thrust),
50		6840	max_thrust(max_thrust) {}
51
52			/**
53			* @brief Initialize the thruster in a pose in the origin of the root joint.
54			*
55			* @param pose[in] Pose from root joint
56			* @param ctorque[in] Coefficient of generated torque per thrust
57			* @param type[in] Type of thruster (clockwise or counterclockwise,
58			* default clockwise)
59			* @param[in] min_thrust[in] Minimum thrust (default 0.)
60			* @param[in] max_thrust[in] Maximum thrust (default inf number))
61			*/
62			ThrusterTpl(const Scalar ctorque, const ThrusterType type = CW,
63			const Scalar min_thrust = Scalar(0.),
64			const Scalar max_thrust = std::numeric_limits<Scalar>::infinity())
65			: pose(SE3::Identity()),
66			ctorque(ctorque),
67			type(type),
68			min_thrust(min_thrust),
69			max_thrust(max_thrust) {}
70		12793	ThrusterTpl(const ThrusterTpl<Scalar>& clone)
71		12793	: pose(clone.pose),
72		12793	ctorque(clone.ctorque),
73		12793	type(clone.type),
74		12793	min_thrust(clone.min_thrust),
75		12793	max_thrust(clone.max_thrust) {}
76
77			ThrusterTpl& operator=(const ThrusterTpl<Scalar>& other) {
78			if (this != &other) {
79			pose = other.pose;
80			ctorque = other.ctorque;
81			type = other.type;
82			min_thrust = other.min_thrust;
83			max_thrust = other.max_thrust;
84			}
85			return *this;
86			}
87
88			template <typename OtherScalar>
89			bool operator==(const ThrusterTpl<OtherScalar>& other) const {
90			return (pose == other.pose && ctorque == other.ctorque &&
91			type == other.type && min_thrust == other.min_thrust &&
92			max_thrust == other.max_thrust);
93			}
94
95			friend std::ostream& operator<<(std::ostream& os,
96			const ThrusterTpl<Scalar>& X) {
97			os << " pose:" << std::endl
98			<< X.pose << " ctorque: " << X.ctorque << std::endl
99			<< " type: " << X.type << std::endl
100			<< "min_thrust: " << X.min_thrust << std::endl
101			<< "max_thrust: " << X.max_thrust << std::endl;
102			return os;
103			}
104
105			SE3 pose; //!< Thruster pose
106			Scalar ctorque; //!< Coefficient of generated torque per thrust
107			ThrusterType type; //!< Type of thruster (CW and CCW for clockwise and
108			//!< counterclockwise, respectively)
109			Scalar min_thrust; //!< Minimum thrust
110			Scalar max_thrust; //!< Minimum thrust
111			};
112
113			/**
114			* @brief Actuation models for floating base systems actuated with thrusters
115			*
116			* This actuation model models floating base robots equipped with thrusters,
117			* e.g., multicopters or marine robots equipped with manipulators. It control
118			* inputs are the thrusters' thrust (i.e., forces) and joint torques.
119			*
120			* Both actuation and Jacobians are computed analytically by `calc` and
121			* `calcDiff`, respectively.
122			*
123			* We assume the robot velocity to zero for easily related square thruster
124			* velocities with thrust and torque generated. This approach is similarly
125			* implemented in M. Geisert and N. Mansard, "Trajectory generation for
126			* quadrotor based systems using numerical optimal control", (ICRA). See Section
127			* III.C.
128			*
129			* \sa `ActuationModelAbstractTpl`, `calc()`, `calcDiff()`, `createData()`
130			*/
131			template <typename _Scalar>
132			class ActuationModelFloatingBaseThrustersTpl
133			: public ActuationModelAbstractTpl<_Scalar> {
134			public:
135			typedef _Scalar Scalar;
136			typedef MathBaseTpl<Scalar> MathBase;
137			typedef ActuationModelAbstractTpl<Scalar> Base;
138			typedef ActuationDataAbstractTpl<Scalar> Data;
139			typedef StateMultibodyTpl<Scalar> StateMultibody;
140			typedef ThrusterTpl<Scalar> Thruster;
141			typedef typename MathBase::Vector3s Vector3s;
142			typedef typename MathBase::VectorXs VectorXs;
143			typedef typename MathBase::MatrixXs MatrixXs;
144
145			/**
146			* @brief Initialize the floating base actuation model equipped with
147			* thrusters
148			*
149			* @param[in] state State of the dynamical system
150			* @param[in] thrusters Vector of thrusters
151			*/
152		198	ActuationModelFloatingBaseThrustersTpl(
153			boost::shared_ptr<StateMultibody> state,
154			const std::vector<Thruster>& thrusters)
155			: Base(state,
156	✓✗	396	state->get_nv() -
157		198	state->get_pinocchio()
158		198	->joints[(
159		198	state->get_pinocchio()->existJointName("root_joint")
160	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✗✗✗	396	? state->get_pinocchio()->getJointId("root_joint")
161			: 0)]
162		198	.nv() +
163			thrusters.size()),
164			thrusters_(thrusters),
165			n_thrusters_(thrusters.size()),
166		198	W_thrust_(state_->get_nv(), nu_),
167	✓✗✓✗ ✓✗✓✗ ✓✗✓✗	594	update_data_(true) {
168	✓✗✓✗ ✗✓	198	if (!state->get_pinocchio()->existJointName("root_joint")) {
169			throw_pretty(
170			"Invalid argument: "
171			<< "the first joint has to be a root one (e.g., free-flyer joint)");
172			}
173			// Update the joint actuation part
174	✓✗	198	W_thrust_.setZero();
175	✓✓	198	if (nu_ > n_thrusters_) {
176	✓✗✓✗	92	W_thrust_.bottomRightCorner(nu_ - n_thrusters_, nu_ - n_thrusters_)
177	✓✗	184	.diagonal()
178			.setOnes();
179			}
180			// Update the floating base actuation part
181	✓✗	198	set_thrusters(thrusters_);
182		198	}
183		400	virtual ~ActuationModelFloatingBaseThrustersTpl() {}
184
185			/**
186			* @brief Compute the actuation signal and actuation set from its thrust
187			* and joint torque inputs \f$\mathbf{u}\in\mathbb{R}^{nu}\f$
188			*
189			* @param[in] data Floating base thrusters actuation data
190			* @param[in] x State point \f$\mathbf{x}\in\mathbb{R}^{ndx}\f$
191			* @param[in] u Joint-torque input \f$\mathbf{u}\in\mathbb{R}^{nu}\f$
192			*/
193		9310	virtual void calc(const boost::shared_ptr<Data>& data,
194			const Eigen::Ref<const VectorXs>&,
195			const Eigen::Ref<const VectorXs>& u) {
196	✗✓	9310	if (static_cast<std::size_t>(u.size()) != nu_) {
197			throw_pretty("Invalid argument: "
198			<< "u has wrong dimension (it should be " +
199			std::to_string(nu_) + ")");
200			}
201	✗✓	9310	if (update_data_) {
202			updateData(data);
203			}
204	✓✗✓✗	9310	data->tau.noalias() = data->dtau_du * u;
205		9310	}
206
207			/**
208			* @brief Compute the Jacobians of the floating base thruster actuation
209			* function
210			*
211			* @param[in] data Floating base thrusters actuation data
212			* @param[in] x State point \f$\mathbf{x}\in\mathbb{R}^{ndx}\f$
213			* @param[in] u Joint-torque input \f$\mathbf{u}\in\mathbb{R}^{nu}\f$
214			*/
215			#ifndef NDEBUG
216		1606	virtual void calcDiff(const boost::shared_ptr<Data>& data,
217			const Eigen::Ref<const VectorXs>&,
218			const Eigen::Ref<const VectorXs>&) {
219			#else
220			virtual void calcDiff(const boost::shared_ptr<Data>&,
221			const Eigen::Ref<const VectorXs>&,
222			const Eigen::Ref<const VectorXs>&) {
223			#endif
224			// The derivatives has constant values which were set in createData.
225	✓✗✗✓ ✗✗✗✗ ✗✗✗✗	1606	assert_pretty(MatrixXs(data->dtau_du).isApprox(W_thrust_),
226			"dtau_du has wrong value");
227		1606	}
228
229		2313	virtual void commands(const boost::shared_ptr<Data>& data,
230			const Eigen::Ref<const VectorXs>&,
231			const Eigen::Ref<const VectorXs>& tau) {
232	✓✗✓✗	2313	data->u.noalias() = data->Mtau * tau;
233		2313	}
234
235			#ifndef NDEBUG
236		629	virtual void torqueTransform(const boost::shared_ptr<Data>& data,
237			const Eigen::Ref<const VectorXs>&,
238			const Eigen::Ref<const VectorXs>&) {
239			#else
240			virtual void torqueTransform(const boost::shared_ptr<Data>&,
241			const Eigen::Ref<const VectorXs>&,
242			const Eigen::Ref<const VectorXs>&) {
243			#endif
244			// The torque transform has constant values which were set in createData.
245	✓✗✗✓ ✗✗✗✗ ✗✗✗✗	629	assert_pretty(MatrixXs(data->Mtau).isApprox(Mtau_), "Mtau has wrong value");
246		629	}
247
248			/**
249			* @brief Create the floating base thruster actuation data
250			*
251			* @return the actuation data
252			*/
253		6522	virtual boost::shared_ptr<Data> createData() {
254	✓✗	6522	boost::shared_ptr<Data> data =
255			boost::allocate_shared<Data>(Eigen::aligned_allocator<Data>(), this);
256	✓✗	6522	updateData(data);
257		6522	return data;
258			}
259
260			/**
261			* @brief Return the vector of thrusters
262			*/
263			const std::vector<Thruster>& get_thrusters() const { return thrusters_; }
264
265			/**
266			* @brief Return the number of thrusters
267			*/
268			std::size_t get_nthrusters() const { return n_thrusters_; }
269
270			/**
271			* @brief Modify the vector of thrusters
272			*
273			* Since we don't want to allocate data, we request to pass the same
274			* number of thrusters.
275			*
276			* @param[in] thrusters Vector of thrusters
277			*/
278		198	void set_thrusters(const std::vector<Thruster>& thrusters) {
279	✗✓	198	if (static_cast<std::size_t>(thrusters.size()) != n_thrusters_) {
280			throw_pretty("Invalid argument: "
281			<< "the number of thrusters is wrong (it should be " +
282			std::to_string(n_thrusters_) + ")");
283			}
284		198	thrusters_ = thrusters;
285			// Update the mapping matrix from thrusters thrust to body force/moments
286	✓✓	990	for (std::size_t i = 0; i < n_thrusters_; ++i) {
287		792	const Thruster& p = thrusters_[i];
288	✓✗✓✗ ✓✗✓✗	792	const Vector3s& f_z = p.pose.rotation() * Vector3s::UnitZ();
289	✓✗✓✗ ✓✗	792	W_thrust_.template topRows<3>().col(i) += f_z;
290	✓✗✓✗ ✓✗✓✗ ✓✗	792	W_thrust_.template middleRows<3>(3).col(i).noalias() +=
291	✓✗✓✗	792	p.pose.translation().cross(Vector3s::UnitZ());
292	✓✓✗	792	switch (p.type) {
293		396	case CW:
294	✓✗✓✗ ✓✗✓✗	396	W_thrust_.template middleRows<3>(3).col(i) += p.ctorque * f_z;
295		396	break;
296		396	case CCW:
297	✓✗✓✗ ✓✗✓✗	396	W_thrust_.template middleRows<3>(3).col(i) -= p.ctorque * f_z;
298		396	break;
299			}
300			}
301			// Compute the torque transform matrix from generalized torques to joint
302			// torque inputs
303	✓✗✓✗	198	Mtau_ = pseudoInverse(MatrixXs(W_thrust_));
304	✓✗✓✗	198	S_.noalias() = W_thrust_ * Mtau_;
305		198	update_data_ = true;
306		198	}
307
308			const MatrixXs& get_Wthrust() const { return W_thrust_; }
309
310			const MatrixXs& get_S() const { return S_; }
311
312			void print(std::ostream& os) const {
313			os << "ActuationModelFloatingBaseThrusters {nu=" << nu_
314			<< ", nthrusters=" << n_thrusters_ << ", thrusters=" << std::endl;
315			for (std::size_t i = 0; i < n_thrusters_; ++i) {
316			os << std::to_string(i) << ": " << thrusters_[i];
317			}
318			os << "}";
319			}
320
321			protected:
322			std::vector<Thruster> thrusters_; //!< Vector of thrusters
323			std::size_t n_thrusters_; //!< Number of thrusters
324			MatrixXs W_thrust_; //!< Matrix from thrusters thrusts to body wrench
325			MatrixXs Mtau_; //!< Constaint torque transform from generalized torques to
326			//!< joint torque inputs
327			MatrixXs S_; //!< Selection matrix for under-actuation part
328
329			bool update_data_;
330			using Base::nu_;
331			using Base::state_;
332
333			private:
334		6522	void updateData(const boost::shared_ptr<Data>& data) {
335		6522	data->dtau_du = W_thrust_;
336		6522	data->Mtau = Mtau_;
337		6522	const std::size_t nv = state_->get_nv();
338	✓✓	47806	for (std::size_t k = 0; k < nv; ++k) {
339	✓✓	41284	if (fabs(S_(k, k)) < std::numeric_limits<Scalar>::epsilon()) {
340		13044	data->tau_set[k] = false;
341			} else {
342		28240	data->tau_set[k] = true;
343			}
344			}
345		6522	update_data_ = false;
346		6522	}
347			};
348
349			} // namespace crocoddyl
350
351			#endif // CROCODDYL_MULTIBODY_ACTUATIONS_PROPELLERS_HPP_