sot-torque-control  1.6.2
Collection of dynamic-graph entities aimed at implementing torque control on different robots.
SimpleInverseDyn Class Reference

#include <sot/torque_control/simple-inverse-dyn.hh>

Inheritance diagram for SimpleInverseDyn:
Collaboration diagram for SimpleInverseDyn:

Public Member Functions

EIGEN_MAKE_ALIGNED_OPERATOR_NEW SimpleInverseDyn (const std::string &name)
 
 DECLARE_SIGNAL_IN (posture_ref_pos, dynamicgraph::Vector)
 
 DECLARE_SIGNAL_IN (posture_ref_vel, dynamicgraph::Vector)
 
 DECLARE_SIGNAL_IN (posture_ref_acc, dynamicgraph::Vector)
 
 DECLARE_SIGNAL_IN (kp_posture, dynamicgraph::Vector)
 
 DECLARE_SIGNAL_IN (kd_posture, dynamicgraph::Vector)
 
 DECLARE_SIGNAL_IN (w_posture, double)
 
 DECLARE_SIGNAL_IN (kp_pos, dynamicgraph::Vector)
 
 DECLARE_SIGNAL_IN (kd_pos, dynamicgraph::Vector)
 
 DECLARE_SIGNAL_IN (com_ref_pos, dynamicgraph::Vector)
 
 DECLARE_SIGNAL_IN (com_ref_vel, dynamicgraph::Vector)
 
 DECLARE_SIGNAL_IN (com_ref_acc, dynamicgraph::Vector)
 
 DECLARE_SIGNAL_IN (kp_com, dynamicgraph::Vector)
 
 DECLARE_SIGNAL_IN (kd_com, dynamicgraph::Vector)
 
 DECLARE_SIGNAL_IN (w_com, double)
 
 DECLARE_SIGNAL_IN (kp_contact, dynamicgraph::Vector)
 
 DECLARE_SIGNAL_IN (kd_contact, dynamicgraph::Vector)
 
 DECLARE_SIGNAL_IN (w_forces, double)
 
 DECLARE_SIGNAL_IN (mu, double)
 
 DECLARE_SIGNAL_IN (contact_points, dynamicgraph::Matrix)
 
 DECLARE_SIGNAL_IN (contact_normal, dynamicgraph::Vector)
 
 DECLARE_SIGNAL_IN (f_min, double)
 
 DECLARE_SIGNAL_IN (f_max, double)
 
 DECLARE_SIGNAL_IN (waist_ref_pos, dynamicgraph::Vector)
 
 DECLARE_SIGNAL_IN (waist_ref_vel, dynamicgraph::Vector)
 
 DECLARE_SIGNAL_IN (waist_ref_acc, dynamicgraph::Vector)
 
 DECLARE_SIGNAL_IN (kp_waist, dynamicgraph::Vector)
 
 DECLARE_SIGNAL_IN (kd_waist, dynamicgraph::Vector)
 
 DECLARE_SIGNAL_IN (w_waist, double)
 
 DECLARE_SIGNAL_IN (q, dynamicgraph::Vector)
 
 DECLARE_SIGNAL_IN (v, dynamicgraph::Vector)
 
 DECLARE_SIGNAL_IN (active_joints, dynamicgraph::Vector)
 
 DECLARE_SIGNAL_INNER (active_joints_checked, dynamicgraph::Vector)
 mask with 1 for controlled joints, 0 otherwise More...
 
 DECLARE_SIGNAL_OUT (tau_des, dynamicgraph::Vector)
 
 DECLARE_SIGNAL_OUT (dv_des, dynamicgraph::Vector)
 
 DECLARE_SIGNAL_OUT (v_des, dynamicgraph::Vector)
 
 DECLARE_SIGNAL_OUT (q_des, dynamicgraph::Vector)
 
 DECLARE_SIGNAL_OUT (u, dynamicgraph::Vector)
 
virtual void display (std::ostream &os) const
 
void init (const double &dt, const std::string &robotRef)
 
void sendMsg (const std::string &msg, MsgType t=MSG_TYPE_INFO, const char *="", int=0)
 
virtual void setControlOutputType (const std::string &type)
 
void updateComOffset ()
 

Protected Attributes

tsid::math::Vector3 m_com_offset
 desired torques (sot order) More...
 
tsid::contacts::Contact6d * m_contactLF
 
tsid::contacts::Contact6d * m_contactRF
 TASKS. More...
 
ControlOutput m_ctrlMode
 Share pointer to the robot utils methods. More...
 
double m_dt
 
tsid::math::Vector m_dv_sot
 Computed solutions (accelerations and torques) and their derivatives. More...
 
tsid::math::Vector m_dv_urdf
 desired accelerations (sot order) More...
 
bool m_enabled
 true if the entity has been successfully initialized More...
 
bool m_firstTime
 True if controler is enabled. More...
 
tsid::solvers::SolverHQPBase * m_hqpSolver
 Solver and problem formulation. More...
 
bool m_initSucceeded
 current time More...
 
tsid::InverseDynamicsFormulationAccForce * m_invDyn
 
tsid::math::Vector m_q_sot
 desired and current velocities (urdf order) (close the TSID loop on it) More...
 
tsid::math::Vector m_q_urdf
 desired positions (sot order) More...
 
tsid::robots::RobotWrapper * m_robot
 True at the first iteration of the controller. More...
 
RobotUtilShrPtr m_robot_util
 Final time of the control loop. More...
 
tsid::trajectories::TrajectorySample m_sampleCom
 Trajectories of the tasks. More...
 
tsid::trajectories::TrajectorySample m_samplePosture
 
tsid::trajectories::TrajectorySample m_sampleWaist
 
double m_t
 control loop time period More...
 
tsid::tasks::TaskJointPosture * m_taskBlockedJoints
 
tsid::tasks::TaskComEquality * m_taskCom
 
tsid::tasks::TaskJointPosture * m_taskPosture
 
tsid::tasks::TaskSE3Equality * m_taskWaist
 
tsid::math::Vector m_tau_sot
 desired and current positions (urdf order) (close the TSID loop on it) More...
 
unsigned int m_timeLast
 3d CoM offset More...
 
tsid::math::Vector m_v_sot
 desired accelerations (urdf order) More...
 
tsid::math::Vector m_v_urdf
 desired velocities (sot order) More...
 
double m_w_com
 Weights of the Tasks (which can be changed) More...
 
double m_w_posture
 
double m_w_waist
 

Detailed Description

Definition at line 73 of file simple-inverse-dyn.hh.

Constructor & Destructor Documentation

◆ SimpleInverseDyn()

SimpleInverseDyn ( const std::string &  name)

Definition at line 134 of file simple-inverse-dyn.cpp.

Member Function Documentation

◆ DECLARE_SIGNAL_IN() [1/31]

DECLARE_SIGNAL_IN ( posture_ref_pos  ,
dynamicgraph::Vector   
)

◆ DECLARE_SIGNAL_IN() [2/31]

DECLARE_SIGNAL_IN ( posture_ref_vel  ,
dynamicgraph::Vector   
)

◆ DECLARE_SIGNAL_IN() [3/31]

DECLARE_SIGNAL_IN ( posture_ref_acc  ,
dynamicgraph::Vector   
)

◆ DECLARE_SIGNAL_IN() [4/31]

DECLARE_SIGNAL_IN ( kp_posture  ,
dynamicgraph::Vector   
)

◆ DECLARE_SIGNAL_IN() [5/31]

DECLARE_SIGNAL_IN ( kd_posture  ,
dynamicgraph::Vector   
)

◆ DECLARE_SIGNAL_IN() [6/31]

DECLARE_SIGNAL_IN ( w_posture  ,
double   
)

◆ DECLARE_SIGNAL_IN() [7/31]

DECLARE_SIGNAL_IN ( kp_pos  ,
dynamicgraph::Vector   
)

◆ DECLARE_SIGNAL_IN() [8/31]

DECLARE_SIGNAL_IN ( kd_pos  ,
dynamicgraph::Vector   
)

◆ DECLARE_SIGNAL_IN() [9/31]

DECLARE_SIGNAL_IN ( com_ref_pos  ,
dynamicgraph::Vector   
)

◆ DECLARE_SIGNAL_IN() [10/31]

DECLARE_SIGNAL_IN ( com_ref_vel  ,
dynamicgraph::Vector   
)

◆ DECLARE_SIGNAL_IN() [11/31]

DECLARE_SIGNAL_IN ( com_ref_acc  ,
dynamicgraph::Vector   
)

◆ DECLARE_SIGNAL_IN() [12/31]

DECLARE_SIGNAL_IN ( kp_com  ,
dynamicgraph::Vector   
)

◆ DECLARE_SIGNAL_IN() [13/31]

DECLARE_SIGNAL_IN ( kd_com  ,
dynamicgraph::Vector   
)

◆ DECLARE_SIGNAL_IN() [14/31]

DECLARE_SIGNAL_IN ( w_com  ,
double   
)

◆ DECLARE_SIGNAL_IN() [15/31]

DECLARE_SIGNAL_IN ( kp_contact  ,
dynamicgraph::Vector   
)

◆ DECLARE_SIGNAL_IN() [16/31]

DECLARE_SIGNAL_IN ( kd_contact  ,
dynamicgraph::Vector   
)

◆ DECLARE_SIGNAL_IN() [17/31]

DECLARE_SIGNAL_IN ( w_forces  ,
double   
)

◆ DECLARE_SIGNAL_IN() [18/31]

DECLARE_SIGNAL_IN ( mu  ,
double   
)

◆ DECLARE_SIGNAL_IN() [19/31]

DECLARE_SIGNAL_IN ( contact_points  ,
dynamicgraph::Matrix   
)

◆ DECLARE_SIGNAL_IN() [20/31]

DECLARE_SIGNAL_IN ( contact_normal  ,
dynamicgraph::Vector   
)

◆ DECLARE_SIGNAL_IN() [21/31]

DECLARE_SIGNAL_IN ( f_min  ,
double   
)

◆ DECLARE_SIGNAL_IN() [22/31]

DECLARE_SIGNAL_IN ( f_max  ,
double   
)

◆ DECLARE_SIGNAL_IN() [23/31]

DECLARE_SIGNAL_IN ( waist_ref_pos  ,
dynamicgraph::Vector   
)

◆ DECLARE_SIGNAL_IN() [24/31]

DECLARE_SIGNAL_IN ( waist_ref_vel  ,
dynamicgraph::Vector   
)

◆ DECLARE_SIGNAL_IN() [25/31]

DECLARE_SIGNAL_IN ( waist_ref_acc  ,
dynamicgraph::Vector   
)

◆ DECLARE_SIGNAL_IN() [26/31]

DECLARE_SIGNAL_IN ( kp_waist  ,
dynamicgraph::Vector   
)

◆ DECLARE_SIGNAL_IN() [27/31]

DECLARE_SIGNAL_IN ( kd_waist  ,
dynamicgraph::Vector   
)

◆ DECLARE_SIGNAL_IN() [28/31]

DECLARE_SIGNAL_IN ( w_waist  ,
double   
)

◆ DECLARE_SIGNAL_IN() [29/31]

DECLARE_SIGNAL_IN ( ,
dynamicgraph::Vector   
)

◆ DECLARE_SIGNAL_IN() [30/31]

DECLARE_SIGNAL_IN ( ,
dynamicgraph::Vector   
)

◆ DECLARE_SIGNAL_IN() [31/31]

DECLARE_SIGNAL_IN ( active_joints  ,
dynamicgraph::Vector   
)

◆ DECLARE_SIGNAL_INNER()

DECLARE_SIGNAL_INNER ( active_joints_checked  ,
dynamicgraph::Vector   
)

mask with 1 for controlled joints, 0 otherwise

◆ DECLARE_SIGNAL_OUT() [1/5]

DECLARE_SIGNAL_OUT ( tau_des  ,
dynamicgraph::Vector   
)

◆ DECLARE_SIGNAL_OUT() [2/5]

DECLARE_SIGNAL_OUT ( dv_des  ,
dynamicgraph::Vector   
)

◆ DECLARE_SIGNAL_OUT() [3/5]

DECLARE_SIGNAL_OUT ( v_des  ,
dynamicgraph::Vector   
)

◆ DECLARE_SIGNAL_OUT() [4/5]

DECLARE_SIGNAL_OUT ( q_des  ,
dynamicgraph::Vector   
)

◆ DECLARE_SIGNAL_OUT() [5/5]

DECLARE_SIGNAL_OUT ( ,
dynamicgraph::Vector   
)

◆ display()

void display ( std::ostream &  os) const
virtual

Definition at line 613 of file simple-inverse-dyn.cpp.

◆ init()

void init ( const double &  dt,
const std::string &  robotRef 
)

Definition at line 220 of file simple-inverse-dyn.cpp.

◆ sendMsg()

void sendMsg ( const std::string &  msg,
MsgType  t = MSG_TYPE_INFO,
const char *  = "",
int  = 0 
)
inline

Definition at line 138 of file simple-inverse-dyn.hh.

◆ setControlOutputType()

void setControlOutputType ( const std::string &  type)
virtual

Definition at line 210 of file simple-inverse-dyn.cpp.

◆ updateComOffset()

void updateComOffset ( )

Definition at line 204 of file simple-inverse-dyn.cpp.

Member Data Documentation

◆ m_com_offset

tsid::math::Vector3 m_com_offset
protected

desired torques (sot order)

Definition at line 184 of file simple-inverse-dyn.hh.

◆ m_contactLF

tsid::contacts::Contact6d* m_contactLF
protected

Definition at line 159 of file simple-inverse-dyn.hh.

◆ m_contactRF

tsid::contacts::Contact6d* m_contactRF
protected

TASKS.

Definition at line 158 of file simple-inverse-dyn.hh.

◆ m_ctrlMode

ControlOutput m_ctrlMode
protected

Share pointer to the robot utils methods.

Definition at line 188 of file simple-inverse-dyn.hh.

◆ m_dt

double m_dt
protected

Definition at line 143 of file simple-inverse-dyn.hh.

◆ m_dv_sot

tsid::math::Vector m_dv_sot
protected

Computed solutions (accelerations and torques) and their derivatives.

Definition at line 176 of file simple-inverse-dyn.hh.

◆ m_dv_urdf

tsid::math::Vector m_dv_urdf
protected

desired accelerations (sot order)

Definition at line 177 of file simple-inverse-dyn.hh.

◆ m_enabled

bool m_enabled
protected

true if the entity has been successfully initialized

Definition at line 146 of file simple-inverse-dyn.hh.

◆ m_firstTime

bool m_firstTime
protected

True if controler is enabled.

Definition at line 147 of file simple-inverse-dyn.hh.

◆ m_hqpSolver

tsid::solvers::SolverHQPBase* m_hqpSolver
protected

Solver and problem formulation.

Definition at line 154 of file simple-inverse-dyn.hh.

◆ m_initSucceeded

bool m_initSucceeded
protected

current time

Definition at line 145 of file simple-inverse-dyn.hh.

◆ m_invDyn

tsid::InverseDynamicsFormulationAccForce* m_invDyn
protected

Definition at line 155 of file simple-inverse-dyn.hh.

◆ m_q_sot

tsid::math::Vector m_q_sot
protected

desired and current velocities (urdf order) (close the TSID loop on it)

Definition at line 180 of file simple-inverse-dyn.hh.

◆ m_q_urdf

tsid::math::Vector m_q_urdf
protected

desired positions (sot order)

Definition at line 181 of file simple-inverse-dyn.hh.

◆ m_robot

tsid::robots::RobotWrapper* m_robot
protected

True at the first iteration of the controller.

TSID Robot

Definition at line 151 of file simple-inverse-dyn.hh.

◆ m_robot_util

RobotUtilShrPtr m_robot_util
protected

Final time of the control loop.

Definition at line 187 of file simple-inverse-dyn.hh.

◆ m_sampleCom

tsid::trajectories::TrajectorySample m_sampleCom
protected

Trajectories of the tasks.

Definition at line 166 of file simple-inverse-dyn.hh.

◆ m_samplePosture

tsid::trajectories::TrajectorySample m_samplePosture
protected

Definition at line 168 of file simple-inverse-dyn.hh.

◆ m_sampleWaist

tsid::trajectories::TrajectorySample m_sampleWaist
protected

Definition at line 167 of file simple-inverse-dyn.hh.

◆ m_t

double m_t
protected

control loop time period

Definition at line 144 of file simple-inverse-dyn.hh.

◆ m_taskBlockedJoints

tsid::tasks::TaskJointPosture* m_taskBlockedJoints
protected

Definition at line 163 of file simple-inverse-dyn.hh.

◆ m_taskCom

tsid::tasks::TaskComEquality* m_taskCom
protected

Definition at line 160 of file simple-inverse-dyn.hh.

◆ m_taskPosture

tsid::tasks::TaskJointPosture* m_taskPosture
protected

Definition at line 162 of file simple-inverse-dyn.hh.

◆ m_taskWaist

tsid::tasks::TaskSE3Equality* m_taskWaist
protected

Definition at line 161 of file simple-inverse-dyn.hh.

◆ m_tau_sot

tsid::math::Vector m_tau_sot
protected

desired and current positions (urdf order) (close the TSID loop on it)

Definition at line 182 of file simple-inverse-dyn.hh.

◆ m_timeLast

unsigned int m_timeLast
protected

3d CoM offset

Definition at line 186 of file simple-inverse-dyn.hh.

◆ m_v_sot

tsid::math::Vector m_v_sot
protected

desired accelerations (urdf order)

Definition at line 178 of file simple-inverse-dyn.hh.

◆ m_v_urdf

tsid::math::Vector m_v_urdf
protected

desired velocities (sot order)

Definition at line 179 of file simple-inverse-dyn.hh.

◆ m_w_com

double m_w_com
protected

Weights of the Tasks (which can be changed)

Definition at line 171 of file simple-inverse-dyn.hh.

◆ m_w_posture

double m_w_posture
protected

Definition at line 172 of file simple-inverse-dyn.hh.

◆ m_w_waist

double m_w_waist
protected

Definition at line 173 of file simple-inverse-dyn.hh.


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