Crocoddyl
ActuationModelFloatingBaseThrustersTpl< _Scalar > Class Template Reference

Actuation models for floating base systems actuated with thrusters. More...

#include <floating-base-thrusters.hpp>

Inheritance diagram for ActuationModelFloatingBaseThrustersTpl< _Scalar >:
ActuationModelAbstractTpl< _Scalar >

Public Types

typedef ActuationModelAbstractTpl< Scalar > Base
 
typedef ActuationDataAbstractTpl< Scalar > Data
 
typedef MathBaseTpl< Scalar > MathBase
 
typedef MathBase::MatrixXs MatrixXs
 
typedef _Scalar Scalar
 
typedef StateMultibodyTpl< Scalar > StateMultibody
 
typedef ThrusterTpl< Scalar > Thruster
 
typedef MathBase::Vector3s Vector3s
 
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

 ActuationModelFloatingBaseThrustersTpl (boost::shared_ptr< StateMultibody > state, const std::vector< Thruster > &thrusters)
 Initialize the floating base actuation model equipped with thrusters. More...
 
virtual void calc (const boost::shared_ptr< Data > &data, const Eigen::Ref< const VectorXs > &, const Eigen::Ref< const VectorXs > &u)
 Compute the actuation signal and actuation set from its thrust and joint torque inputs \(\mathbf{u}\in\mathbb{R}^{nu}\). More...
 
virtual void calcDiff (const boost::shared_ptr< Data > &data, const Eigen::Ref< const VectorXs > &, const Eigen::Ref< const VectorXs > &)
 Compute the Jacobians of the floating base thruster actuation function. More...
 
virtual void commands (const boost::shared_ptr< Data > &data, const Eigen::Ref< const VectorXs > &, const Eigen::Ref< const VectorXs > &tau)
 Compute the joint torque input from the generalized torques. More...
 
virtual boost::shared_ptr< DatacreateData ()
 Create the floating base thruster actuation data. More...
 
std::size_t get_nthrusters () const
 Return the number of thrusters.
 
const MatrixXs & get_S () const
 
const std::vector< Thruster > & get_thrusters () const
 Return the vector of thrusters.
 
const MatrixXs & get_Wthrust () const
 
void print (std::ostream &os) const
 Print relevant information of the residual model. More...
 
void set_thrusters (const std::vector< Thruster > &thrusters)
 Modify the vector of thrusters. More...
 
virtual void torqueTransform (const boost::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. More...
 
- Public Member Functions inherited from ActuationModelAbstractTpl< _Scalar >
 ActuationModelAbstractTpl (boost::shared_ptr< StateAbstract > state, const std::size_t nu)
 Initialize the actuation model. More...
 
void calc (const boost::shared_ptr< ActuationDataAbstract > &data, const Eigen::Ref< const VectorXs > &x)
 Ignore the computation of the actuation signal. More...
 
void calcDiff (const boost::shared_ptr< ActuationDataAbstract > &data, const Eigen::Ref< const VectorXs > &x)
 Ignore the computation of the Jacobians of the actuation function. More...
 
std::size_t get_nu () const
 Return the dimension of the joint-torque input.
 
const boost::shared_ptr< StateAbstract > & get_state () const
 Return the state.
 

Protected Attributes

MatrixXs Mtau_
 
std::size_t n_thrusters_
 Number of thrusters.
 
std::size_t nu_
 Dimension of joint torque inputs.
 
MatrixXs S_
 Selection matrix for under-actuation part.
 
boost::shared_ptr< StateAbstractstate_
 Model of the state.
 
std::vector< Thrusterthrusters_
 Vector of thrusters.
 
bool update_data_
 
MatrixXs W_thrust_
 Matrix from thrusters thrusts to body wrench.
 
- Protected Attributes inherited from ActuationModelAbstractTpl< _Scalar >
std::size_t nu_
 Dimension of joint torque inputs.
 
boost::shared_ptr< StateAbstractstate_
 Model of the state.
 

Additional Inherited Members

- Public Attributes inherited from ActuationModelAbstractTpl< _Scalar >
EIGEN_MAKE_ALIGNED_OPERATOR_NEW typedef _Scalar Scalar
 

Detailed Description

template<typename _Scalar>
class crocoddyl::ActuationModelFloatingBaseThrustersTpl< _Scalar >

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.

See also
ActuationModelAbstractTpl, calc(), calcDiff(), createData()

Definition at line 132 of file floating-base-thrusters.hpp.

Constructor & Destructor Documentation

◆ ActuationModelFloatingBaseThrustersTpl()

ActuationModelFloatingBaseThrustersTpl ( boost::shared_ptr< StateMultibody state,
const std::vector< Thruster > &  thrusters 
)
inline

Initialize the floating base actuation model equipped with thrusters.

Parameters
[in]stateState of the dynamical system
[in]thrustersVector of thrusters

Definition at line 152 of file floating-base-thrusters.hpp.

Member Function Documentation

◆ calc()

virtual void calc ( const boost::shared_ptr< Data > &  data,
const Eigen::Ref< const VectorXs > &  ,
const Eigen::Ref< const VectorXs > &  u 
)
inlinevirtual

Compute the actuation signal and actuation set from its thrust and joint torque inputs \(\mathbf{u}\in\mathbb{R}^{nu}\).

Parameters
[in]dataFloating base thrusters actuation data
[in]xState point \(\mathbf{x}\in\mathbb{R}^{ndx}\)
[in]uJoint-torque input \(\mathbf{u}\in\mathbb{R}^{nu}\)

Implements ActuationModelAbstractTpl< _Scalar >.

Definition at line 193 of file floating-base-thrusters.hpp.

◆ calcDiff()

virtual void calcDiff ( const boost::shared_ptr< Data > &  data,
const Eigen::Ref< const VectorXs > &  ,
const Eigen::Ref< const VectorXs > &   
)
inlinevirtual

Compute the Jacobians of the floating base thruster actuation function.

Parameters
[in]dataFloating base thrusters actuation data
[in]xState point \(\mathbf{x}\in\mathbb{R}^{ndx}\)
[in]uJoint-torque input \(\mathbf{u}\in\mathbb{R}^{nu}\)

Implements ActuationModelAbstractTpl< _Scalar >.

Definition at line 216 of file floating-base-thrusters.hpp.

◆ commands()

virtual void commands ( const boost::shared_ptr< Data > &  data,
const Eigen::Ref< const VectorXs > &  x,
const Eigen::Ref< const VectorXs > &  tau 
)
inlinevirtual

Compute the joint torque input from the generalized torques.

It stores the results in ActuationDataAbstractTpl::u.

Parameters
[in]dataActuation data
[in]xState point \(\mathbf{x}\in\mathbb{R}^{ndx}\)
[in]tauGeneralized torques \(\mathbf{u}\in\mathbb{R}^{nv}\)

Implements ActuationModelAbstractTpl< _Scalar >.

Definition at line 229 of file floating-base-thrusters.hpp.

◆ torqueTransform()

virtual void torqueTransform ( const boost::shared_ptr< Data > &  data,
const Eigen::Ref< const VectorXs > &  x,
const Eigen::Ref< const VectorXs > &  u 
)
inlinevirtual

Compute the torque transform from generalized torques to joint torque inputs.

It stores the results in ActuationDataAbstractTpl::Mtau.

Parameters
[in]dataActuation data
[in]xState point \(\mathbf{x}\in\mathbb{R}^{ndx}\)
[in]tauJoint-torque inputs \(\mathbf{u}\in\mathbb{R}^{nu}\)

Reimplemented from ActuationModelAbstractTpl< _Scalar >.

Definition at line 236 of file floating-base-thrusters.hpp.

◆ createData()

virtual boost::shared_ptr<Data> createData ( )
inlinevirtual

Create the floating base thruster actuation data.

Returns
the actuation data

Reimplemented from ActuationModelAbstractTpl< _Scalar >.

Definition at line 253 of file floating-base-thrusters.hpp.

◆ set_thrusters()

void set_thrusters ( const std::vector< Thruster > &  thrusters)
inline

Modify the vector of thrusters.

Since we don't want to allocate data, we request to pass the same number of thrusters.

Parameters
[in]thrustersVector of thrusters

Definition at line 278 of file floating-base-thrusters.hpp.

◆ print()

void print ( std::ostream &  os) const
inlinevirtual

Print relevant information of the residual model.

Parameters
[out]osOutput stream object

Reimplemented from ActuationModelAbstractTpl< _Scalar >.

Definition at line 312 of file floating-base-thrusters.hpp.

Member Data Documentation

◆ Mtau_

MatrixXs Mtau_
protected

Constaint torque transform from generalized torques to joint torque inputs

Definition at line 325 of file floating-base-thrusters.hpp.


The documentation for this class was generated from the following file: