Multicopter actuation model. More...
#include <multicopter-base.hpp>
Public Types | |
| typedef ActuationModelAbstractTpl< Scalar > | Base |
| typedef ActuationDataAbstractTpl< Scalar > | Data |
| typedef MathBaseTpl< Scalar > | MathBase |
| typedef MathBase::Matrix6xs | Matrix6xs |
| typedef MathBase::MatrixXs | MatrixXs |
| typedef _Scalar | Scalar |
| typedef StateMultibodyTpl< Scalar > | StateMultibody |
| typedef MathBase::VectorXs | VectorXs |
Public Types inherited from ActuationModelAbstractTpl< _Scalar > | |
| typedef ActuationDataAbstractTpl< Scalar > | ActuationDataAbstract |
| typedef MathBaseTpl< Scalar > | MathBase |
| typedef MathBase::MatrixXs | MatrixXs |
| typedef StateAbstractTpl< Scalar > | StateAbstract |
| typedef MathBase::VectorXs | VectorXs |
Public Member Functions | |
| virtual void | calc (const std::shared_ptr< Data > &data, const Eigen::Ref< const VectorXs > &, const Eigen::Ref< const VectorXs > &u) |
| Compute the actuation signal from the state point \(\mathbf{x}\in\mathbb{R}^{ndx}\) and joint torque inputs \(\mathbf{u}\in\mathbb{R}^{nu}\). | |
| virtual void | calcDiff (const std::shared_ptr< Data > &data, const Eigen::Ref< const VectorXs > &, const Eigen::Ref< const VectorXs > &) |
| Compute the Jacobians of the actuation function. | |
| virtual void | commands (const std::shared_ptr< Data > &data, const Eigen::Ref< const VectorXs > &, const Eigen::Ref< const VectorXs > &tau) |
| Compute the joint torque input from the generalized torques. | |
| std::shared_ptr< Data > | createData () |
| Create the actuation data. | |
| DEPRECATED ("Use constructor ActuationModelFloatingBaseThrustersTpl", ActuationModelMultiCopterBaseTpl(std::shared_ptr< StateMultibody > state, const Eigen::Ref< const Matrix6xs > &tau_f)) | |
| Initialize the multicopter actuation model. | |
| DEPRECATED ("Use constructor without n_rotors", ActuationModelMultiCopterBaseTpl(std::shared_ptr< StateMultibody > state, const std::size_t n_rotors, const Eigen::Ref< const Matrix6xs > &tau_f)) | |
| std::size_t | get_nrotors () const |
| const MatrixXs & | get_tauf () const |
| void | set_tauf (const Eigen::Ref< const MatrixXs > &tau_f) |
| virtual void | torqueTransform (const std::shared_ptr< Data > &data, const Eigen::Ref< const VectorXs > &, const Eigen::Ref< const VectorXs > &) |
| Compute the torque transform from generalized torques to joint torque inputs. | |
Public Member Functions inherited from ActuationModelAbstractTpl< _Scalar > | |
| ActuationModelAbstractTpl (std::shared_ptr< StateAbstract > state, const std::size_t nu) | |
| Initialize the actuation model. | |
| void | calc (const std::shared_ptr< ActuationDataAbstract > &data, const Eigen::Ref< const VectorXs > &x) |
| Ignore the computation of the actuation signal. | |
| void | calcDiff (const std::shared_ptr< ActuationDataAbstract > &data, const Eigen::Ref< const VectorXs > &x) |
| Ignore the computation of the Jacobians of the actuation function. | |
| std::size_t | get_nu () const |
| Return the dimension of the joint-torque input. | |
| const std::shared_ptr< StateAbstract > & | get_state () const |
| Return the state. | |
| virtual void | print (std::ostream &os) const |
| Print relevant information of the residual model. | |
Protected Attributes | |
| MatrixXs | Mtau_ |
| std::size_t | n_rotors_ |
| Number of rotors. | |
| std::size_t | nu_ |
| Dimension of joint torque inputs. | |
| std::shared_ptr< StateAbstract > | state_ |
| Model of the state. | |
| MatrixXs | tau_f_ |
| Matrix from rotors thrust to body force/moments. | |
Protected Attributes inherited from ActuationModelAbstractTpl< _Scalar > | |
| std::size_t | nu_ |
| Dimension of joint torque inputs. | |
| std::shared_ptr< StateAbstract > | state_ |
| Model of the state. | |
Additional Inherited Members | |
Public Attributes inherited from ActuationModelAbstractTpl< _Scalar > | |
| EIGEN_MAKE_ALIGNED_OPERATOR_NEW typedef _Scalar | Scalar |
Multicopter actuation model.
This actuation model is aimed for those robots whose base_link is actuated using a propulsion system, e.g., a multicopter or an aerial manipulator (multicopter with a robotic arm attached). Control input: the thrust (force) created by each propeller. tau_f matrix: this matrix relates the thrust of each propeller to the net force and torque that it causes to the base_link. For a simple quadrotor: tau_f.nrows = 6, tau_f.ncols = 4
Both actuation and Jacobians are computed analytically by calc and calcDiff, respectively.
Reference: M. Geisert and N. Mansard, "Trajectory generation for quadrotor based systems using numerical optimal control," 2016 IEEE International Conference on Robotics and Automation (ICRA), Stockholm, 2016, pp. 2958-2964. See Section III.C.
ActuationModelAbstractTpl, calc(), calcDiff(), createData() Definition at line 43 of file multicopter-base.hpp.
| typedef _Scalar Scalar |
Definition at line 46 of file multicopter-base.hpp.
| typedef MathBaseTpl<Scalar> MathBase |
Definition at line 47 of file multicopter-base.hpp.
| typedef ActuationModelAbstractTpl<Scalar> Base |
Definition at line 48 of file multicopter-base.hpp.
| typedef ActuationDataAbstractTpl<Scalar> Data |
Definition at line 49 of file multicopter-base.hpp.
| typedef StateMultibodyTpl<Scalar> StateMultibody |
Definition at line 50 of file multicopter-base.hpp.
| typedef MathBase::VectorXs VectorXs |
Definition at line 51 of file multicopter-base.hpp.
| typedef MathBase::MatrixXs MatrixXs |
Definition at line 52 of file multicopter-base.hpp.
| typedef MathBase::Matrix6xs Matrix6xs |
Definition at line 53 of file multicopter-base.hpp.
|
inlinevirtual |
Definition at line 72 of file multicopter-base.hpp.
| DEPRECATED | ( | "Use constructor ActuationModelFloatingBaseThrustersTpl" | , |
| ActuationModelMultiCopterBaseTpl< _Scalar >(std::shared_ptr< StateMultibody > state, const Eigen::Ref< const Matrix6xs > &tau_f) | |||
| ) |
Initialize the multicopter actuation model.
| [in] | state | State of the dynamical system |
| [in] | tau_f | Matrix that maps the thrust of each propeller to the net force and torque |
|
inlinevirtual |
Compute the actuation signal from the state point \(\mathbf{x}\in\mathbb{R}^{ndx}\) and joint torque inputs \(\mathbf{u}\in\mathbb{R}^{nu}\).
| [in] | data | Actuation data |
| [in] | x | State point \(\mathbf{x}\in\mathbb{R}^{ndx}\) |
| [in] | u | Joint-torque input \(\mathbf{u}\in\mathbb{R}^{nu}\) |
Implements ActuationModelAbstractTpl< _Scalar >.
Definition at line 74 of file multicopter-base.hpp.
|
inlinevirtual |
Compute the Jacobians of the actuation function.
| [in] | data | Actuation data |
| [in] | x | State point \(\mathbf{x}\in\mathbb{R}^{ndx}\) |
| [in] | u | Joint-torque input \(\mathbf{u}\in\mathbb{R}^{nu}\) |
Implements ActuationModelAbstractTpl< _Scalar >.
Definition at line 87 of file multicopter-base.hpp.
|
inlinevirtual |
Compute the joint torque input from the generalized torques.
It stores the results in ActuationDataAbstractTpl::u.
| [in] | data | Actuation data |
| [in] | x | State point \(\mathbf{x}\in\mathbb{R}^{ndx}\) |
| [in] | tau | Generalized torques \(\mathbf{u}\in\mathbb{R}^{nv}\) |
Implements ActuationModelAbstractTpl< _Scalar >.
Definition at line 100 of file multicopter-base.hpp.
|
inlinevirtual |
Compute the torque transform from generalized torques to joint torque inputs.
It stores the results in ActuationDataAbstractTpl::Mtau.
| [in] | data | Actuation data |
| [in] | x | State point \(\mathbf{x}\in\mathbb{R}^{ndx}\) |
| [in] | tau | Joint-torque inputs \(\mathbf{u}\in\mathbb{R}^{nu}\) |
Reimplemented from ActuationModelAbstractTpl< _Scalar >.
Definition at line 107 of file multicopter-base.hpp.
|
inlinevirtual |
Create the actuation data.
Reimplemented from ActuationModelAbstractTpl< _Scalar >.
Definition at line 119 of file multicopter-base.hpp.
|
inline |
Definition at line 130 of file multicopter-base.hpp.
|
inline |
Definition at line 131 of file multicopter-base.hpp.
|
inline |
Definition at line 132 of file multicopter-base.hpp.
|
protected |
Matrix from rotors thrust to body force/moments.
Definition at line 135 of file multicopter-base.hpp.
|
protected |
Constaint torque transform from generalized torques to joint torque inputs
Definition at line 136 of file multicopter-base.hpp.
|
protected |
Number of rotors.
Definition at line 138 of file multicopter-base.hpp.
|
protected |
Dimension of joint torque inputs.
Definition at line 172 of file actuation-base.hpp.
|
protected |
Model of the state.
Definition at line 173 of file actuation-base.hpp.