17 #ifndef __sot_torque_control_simple_inverse_dyn_H__ 18 #define __sot_torque_control_simple_inverse_dyn_H__ 25 #if defined(simple_inverse_dyn_EXPORTS) 26 #define SOTSIMPLEINVERSEDYN_EXPORT __declspec(dllexport) 28 #define SOTSIMPLEINVERSEDYN_EXPORT __declspec(dllimport) 31 #define SOTSIMPLEINVERSEDYN_EXPORT 39 #include "boost/assign.hpp" 42 #include <pinocchio/multibody/model.hpp> 43 #include <pinocchio/parsers/urdf.hpp> 44 #include "pinocchio/algorithm/joint-configuration.hpp" 46 #include <tsid/robots/robot-wrapper.hpp> 47 #include <tsid/solvers/solver-HQP-base.hpp> 48 #include <tsid/contacts/contact-6d.hpp> 49 #include <tsid/formulations/inverse-dynamics-formulation-acc-force.hpp> 50 #include <tsid/tasks/task-com-equality.hpp> 51 #include <tsid/tasks/task-joint-posture.hpp> 52 #include <tsid/trajectories/trajectory-euclidian.hpp> 55 #include <dynamic-graph/signal-helper.h> 56 #include <sot/core/matrix-geometry.hh> 57 #include <sot/core/robot-utils.hh> 75 DYNAMIC_GRAPH_ENTITY_DECL();
78 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
85 DECLARE_SIGNAL_IN(posture_ref_pos, dynamicgraph::Vector);
86 DECLARE_SIGNAL_IN(posture_ref_vel, dynamicgraph::Vector);
87 DECLARE_SIGNAL_IN(posture_ref_acc, dynamicgraph::Vector);
88 DECLARE_SIGNAL_IN(kp_posture, dynamicgraph::Vector);
89 DECLARE_SIGNAL_IN(kd_posture, dynamicgraph::Vector);
90 DECLARE_SIGNAL_IN(w_posture,
double);
92 DECLARE_SIGNAL_IN(kp_pos, dynamicgraph::Vector);
93 DECLARE_SIGNAL_IN(kd_pos, dynamicgraph::Vector);
95 DECLARE_SIGNAL_IN(com_ref_pos, dynamicgraph::Vector);
96 DECLARE_SIGNAL_IN(com_ref_vel, dynamicgraph::Vector);
97 DECLARE_SIGNAL_IN(com_ref_acc, dynamicgraph::Vector);
98 DECLARE_SIGNAL_IN(kp_com, dynamicgraph::Vector);
99 DECLARE_SIGNAL_IN(kd_com, dynamicgraph::Vector);
100 DECLARE_SIGNAL_IN(w_com,
double);
102 DECLARE_SIGNAL_IN(kp_contact, dynamicgraph::Vector);
103 DECLARE_SIGNAL_IN(kd_contact, dynamicgraph::Vector);
104 DECLARE_SIGNAL_IN(w_forces,
double);
105 DECLARE_SIGNAL_IN(mu,
double);
106 DECLARE_SIGNAL_IN(contact_points, dynamicgraph::Matrix);
107 DECLARE_SIGNAL_IN(contact_normal, dynamicgraph::Vector);
108 DECLARE_SIGNAL_IN(f_min,
double);
109 DECLARE_SIGNAL_IN(f_max,
double);
111 DECLARE_SIGNAL_IN(waist_ref_pos, dynamicgraph::Vector);
112 DECLARE_SIGNAL_IN(waist_ref_vel, dynamicgraph::Vector);
113 DECLARE_SIGNAL_IN(waist_ref_acc, dynamicgraph::Vector);
114 DECLARE_SIGNAL_IN(kp_waist, dynamicgraph::Vector);
115 DECLARE_SIGNAL_IN(kd_waist, dynamicgraph::Vector);
116 DECLARE_SIGNAL_IN(w_waist,
double);
118 DECLARE_SIGNAL_IN(q, dynamicgraph::Vector);
119 DECLARE_SIGNAL_IN(v, dynamicgraph::Vector);
120 DECLARE_SIGNAL_IN(active_joints, dynamicgraph::Vector);
121 DECLARE_SIGNAL_INNER(active_joints_checked, dynamicgraph::Vector);
123 DECLARE_SIGNAL_OUT(tau_des, dynamicgraph::Vector);
124 DECLARE_SIGNAL_OUT(dv_des, dynamicgraph::Vector);
125 DECLARE_SIGNAL_OUT(v_des, dynamicgraph::Vector);
126 DECLARE_SIGNAL_OUT(q_des, dynamicgraph::Vector);
127 DECLARE_SIGNAL_OUT(u, dynamicgraph::Vector);
131 void init(
const double& dt,
const std::string& robotRef);
132 virtual void setControlOutputType(
const std::string& type);
133 void updateComOffset();
136 virtual void display(std::ostream& os)
const;
138 void sendMsg(
const std::string& msg, MsgType t = MSG_TYPE_INFO,
const char* =
"",
int = 0) {
139 logger_.stream(t) << (
"[" + name +
"] " + msg) <<
'\n';
155 tsid::InverseDynamicsFormulationAccForce*
m_invDyn;
195 #endif // #ifndef __sot_torque_control_simple_inverse_dyn_H__ tsid::math::Vector m_dv_urdf
desired accelerations (sot order)
tsid::tasks::TaskComEquality * m_taskCom
tsid::tasks::TaskSE3Equality * m_taskWaist
double m_t
control loop time period
tsid::InverseDynamicsFormulationAccForce * m_invDyn
tsid::solvers::SolverHQPBase * m_hqpSolver
Solver and problem formulation.
Eigen::Matrix< double, 3, 1 > Vector3
tsid::trajectories::TrajectorySample m_sampleWaist
tsid::tasks::TaskJointPosture * m_taskBlockedJoints
unsigned int m_timeLast
3d CoM offset
bool m_firstTime
True if controler is enabled.
tsid::contacts::Contact6d * m_contactRF
TASKS.
bool m_enabled
true if the entity has been successfully initialized
tsid::math::Vector3 m_com_offset
desired torques (sot order)
tsid::tasks::TaskJointPosture * m_taskPosture
tsid::math::Vector m_v_sot
desired accelerations (urdf order)
bool m_initSucceeded
current time
#define SOTSIMPLEINVERSEDYN_EXPORT
tsid::trajectories::TrajectorySample m_sampleCom
Trajectories of the tasks.
RobotUtilShrPtr m_robot_util
Final time of the control loop.
tsid::math::Vector m_tau_sot
desired and current positions (urdf order) (close the TSID loop on it)
tsid::math::Vector m_v_urdf
desired velocities (sot order)
tsid::trajectories::TrajectorySample m_samplePosture
double m_w_com
Weights of the Tasks (which can be changed)
tsid::math::Vector m_q_sot
desired and current velocities (urdf order) (close the TSID loop on it)
void sendMsg(const std::string &msg, MsgType t=MSG_TYPE_INFO, const char *="", int=0)
AdmittanceController EntityClassName
const std::string ControlOutput_s[]
ControlOutput m_ctrlMode
Share pointer to the robot utils methods.
tsid::robots::RobotWrapper * m_robot
True at the first iteration of the controller.
tsid::math::Vector m_q_urdf
desired positions (sot order)
tsid::math::Vector m_dv_sot
Computed solutions (accelerations and torques) and their derivatives.
tsid::contacts::Contact6d * m_contactLF