6 #ifndef __sot_torque_control_inverse_dynamics_balance_controller_H__ 7 #define __sot_torque_control_inverse_dynamics_balance_controller_H__ 14 #if defined(inverse_dynamics_balance_controller_EXPORTS) 15 #define SOTINVERSEDYNAMICSBALANCECONTROLLER_EXPORT __declspec(dllexport) 17 #define SOTINVERSEDYNAMICSBALANCECONTROLLER_EXPORT __declspec(dllimport) 20 #define SOTINVERSEDYNAMICSBALANCECONTROLLER_EXPORT 28 #include "boost/assign.hpp" 31 #include <pinocchio/multibody/model.hpp> 32 #include <pinocchio/parsers/urdf.hpp> 34 #include <tsid/robots/robot-wrapper.hpp> 35 #include <tsid/solvers/solver-HQP-base.hpp> 36 #include <tsid/contacts/contact-6d.hpp> 37 #include <tsid/formulations/inverse-dynamics-formulation-acc-force.hpp> 38 #include <tsid/tasks/task-com-equality.hpp> 39 #include <tsid/tasks/task-joint-posture.hpp> 40 #include <tsid/trajectories/trajectory-euclidian.hpp> 43 #include <dynamic-graph/signal-helper.h> 44 #include <sot/core/matrix-geometry.hh> 45 #include <sot/core/robot-utils.hh> 59 DYNAMIC_GRAPH_ENTITY_DECL();
62 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
67 void init(
const double& dt,
const std::string& robotRef);
68 void updateComOffset();
69 void removeRightFootContact(
const double& transitionTime);
70 void removeLeftFootContact(
const double& transitionTime);
71 void addRightFootContact(
const double& transitionTime);
72 void addLeftFootContact(
const double& transitionTime);
73 void addTaskRightHand();
74 void removeTaskRightHand(
const double& transitionTime);
75 void addTaskLeftHand();
76 void removeTaskLeftHand(
const double& transitionTime);
79 DECLARE_SIGNAL_IN(com_ref_pos, dynamicgraph::Vector);
80 DECLARE_SIGNAL_IN(com_ref_vel, dynamicgraph::Vector);
81 DECLARE_SIGNAL_IN(com_ref_acc, dynamicgraph::Vector);
82 DECLARE_SIGNAL_IN(rf_ref_pos, dynamicgraph::Vector);
83 DECLARE_SIGNAL_IN(rf_ref_vel, dynamicgraph::Vector);
84 DECLARE_SIGNAL_IN(rf_ref_acc, dynamicgraph::Vector);
85 DECLARE_SIGNAL_IN(lf_ref_pos, dynamicgraph::Vector);
86 DECLARE_SIGNAL_IN(lf_ref_vel, dynamicgraph::Vector);
87 DECLARE_SIGNAL_IN(lf_ref_acc, dynamicgraph::Vector);
88 DECLARE_SIGNAL_IN(rh_ref_pos, dynamicgraph::Vector);
89 DECLARE_SIGNAL_IN(rh_ref_vel, dynamicgraph::Vector);
90 DECLARE_SIGNAL_IN(rh_ref_acc, dynamicgraph::Vector);
91 DECLARE_SIGNAL_IN(lh_ref_pos, dynamicgraph::Vector);
92 DECLARE_SIGNAL_IN(lh_ref_vel, dynamicgraph::Vector);
93 DECLARE_SIGNAL_IN(lh_ref_acc, dynamicgraph::Vector);
94 DECLARE_SIGNAL_IN(posture_ref_pos, dynamicgraph::Vector);
95 DECLARE_SIGNAL_IN(posture_ref_vel, dynamicgraph::Vector);
96 DECLARE_SIGNAL_IN(posture_ref_acc, dynamicgraph::Vector);
97 DECLARE_SIGNAL_IN(base_orientation_ref_pos, dynamicgraph::Vector);
98 DECLARE_SIGNAL_IN(base_orientation_ref_vel, dynamicgraph::Vector);
99 DECLARE_SIGNAL_IN(base_orientation_ref_acc, dynamicgraph::Vector);
100 DECLARE_SIGNAL_IN(f_ref_right_foot, dynamicgraph::Vector);
101 DECLARE_SIGNAL_IN(f_ref_left_foot, dynamicgraph::Vector);
103 DECLARE_SIGNAL_IN(kp_base_orientation, dynamicgraph::Vector);
104 DECLARE_SIGNAL_IN(kd_base_orientation, dynamicgraph::Vector);
105 DECLARE_SIGNAL_IN(kp_constraints, dynamicgraph::Vector);
106 DECLARE_SIGNAL_IN(kd_constraints, dynamicgraph::Vector);
107 DECLARE_SIGNAL_IN(kp_com, dynamicgraph::Vector);
108 DECLARE_SIGNAL_IN(kd_com, dynamicgraph::Vector);
109 DECLARE_SIGNAL_IN(kp_feet, dynamicgraph::Vector);
110 DECLARE_SIGNAL_IN(kd_feet, dynamicgraph::Vector);
111 DECLARE_SIGNAL_IN(kp_hands, dynamicgraph::Vector);
112 DECLARE_SIGNAL_IN(kd_hands, dynamicgraph::Vector);
113 DECLARE_SIGNAL_IN(kp_posture, dynamicgraph::Vector);
114 DECLARE_SIGNAL_IN(kd_posture, dynamicgraph::Vector);
115 DECLARE_SIGNAL_IN(kp_pos, dynamicgraph::Vector);
116 DECLARE_SIGNAL_IN(kd_pos, dynamicgraph::Vector);
118 DECLARE_SIGNAL_IN(w_com,
double);
119 DECLARE_SIGNAL_IN(w_feet,
double);
120 DECLARE_SIGNAL_IN(w_hands,
double);
121 DECLARE_SIGNAL_IN(w_posture,
double);
122 DECLARE_SIGNAL_IN(w_base_orientation,
double);
123 DECLARE_SIGNAL_IN(w_torques,
double);
124 DECLARE_SIGNAL_IN(w_forces,
double);
125 DECLARE_SIGNAL_IN(weight_contact_forces, dynamicgraph::Vector);
127 DECLARE_SIGNAL_IN(mu,
double);
128 DECLARE_SIGNAL_IN(contact_points, dynamicgraph::Matrix);
129 DECLARE_SIGNAL_IN(contact_normal, dynamicgraph::Vector);
130 DECLARE_SIGNAL_IN(f_min,
double);
131 DECLARE_SIGNAL_IN(f_max_right_foot,
double);
132 DECLARE_SIGNAL_IN(f_max_left_foot,
double);
134 DECLARE_SIGNAL_IN(rotor_inertias, dynamicgraph::Vector);
135 DECLARE_SIGNAL_IN(gear_ratios, dynamicgraph::Vector);
136 DECLARE_SIGNAL_IN(tau_max, dynamicgraph::Vector);
137 DECLARE_SIGNAL_IN(q_min, dynamicgraph::Vector);
138 DECLARE_SIGNAL_IN(q_max, dynamicgraph::Vector);
139 DECLARE_SIGNAL_IN(dq_max, dynamicgraph::Vector);
140 DECLARE_SIGNAL_IN(ddq_max, dynamicgraph::Vector);
141 DECLARE_SIGNAL_IN(dt_joint_pos_limits,
double);
143 DECLARE_SIGNAL_IN(tau_estimated, dynamicgraph::Vector);
144 DECLARE_SIGNAL_IN(q, dynamicgraph::Vector);
145 DECLARE_SIGNAL_IN(v, dynamicgraph::Vector);
146 DECLARE_SIGNAL_IN(wrench_base, dynamicgraph::Vector);
147 DECLARE_SIGNAL_IN(wrench_left_foot, dynamicgraph::Vector);
148 DECLARE_SIGNAL_IN(wrench_right_foot, dynamicgraph::Vector);
150 DECLARE_SIGNAL_IN(active_joints, dynamicgraph::Vector);
152 DECLARE_SIGNAL_OUT(tau_des, dynamicgraph::Vector);
153 DECLARE_SIGNAL_OUT(M, dynamicgraph::Matrix);
154 DECLARE_SIGNAL_OUT(dv_des, dynamicgraph::Vector);
155 DECLARE_SIGNAL_OUT(f_des_right_foot, dynamicgraph::Vector);
156 DECLARE_SIGNAL_OUT(f_des_left_foot, dynamicgraph::Vector);
157 DECLARE_SIGNAL_OUT(zmp_des_right_foot, dynamicgraph::Vector);
158 DECLARE_SIGNAL_OUT(zmp_des_left_foot, dynamicgraph::Vector);
159 DECLARE_SIGNAL_OUT(zmp_des_right_foot_local, dynamicgraph::Vector);
160 DECLARE_SIGNAL_OUT(zmp_des_left_foot_local, dynamicgraph::Vector);
161 DECLARE_SIGNAL_OUT(zmp_des, dynamicgraph::Vector);
162 DECLARE_SIGNAL_OUT(zmp_ref, dynamicgraph::Vector);
163 DECLARE_SIGNAL_OUT(zmp_right_foot, dynamicgraph::Vector);
164 DECLARE_SIGNAL_OUT(zmp_left_foot, dynamicgraph::Vector);
165 DECLARE_SIGNAL_OUT(zmp, dynamicgraph::Vector);
166 DECLARE_SIGNAL_OUT(com, dynamicgraph::Vector);
167 DECLARE_SIGNAL_OUT(com_vel, dynamicgraph::Vector);
168 DECLARE_SIGNAL_OUT(com_acc, dynamicgraph::Vector);
169 DECLARE_SIGNAL_OUT(com_acc_des, dynamicgraph::Vector);
170 DECLARE_SIGNAL_OUT(base_orientation, dynamicgraph::Vector);
171 DECLARE_SIGNAL_OUT(right_foot_pos, dynamicgraph::Vector);
172 DECLARE_SIGNAL_OUT(left_foot_pos, dynamicgraph::Vector);
173 DECLARE_SIGNAL_OUT(right_hand_pos, dynamicgraph::Vector);
174 DECLARE_SIGNAL_OUT(left_hand_pos, dynamicgraph::Vector);
175 DECLARE_SIGNAL_OUT(right_foot_vel, dynamicgraph::Vector);
176 DECLARE_SIGNAL_OUT(left_foot_vel, dynamicgraph::Vector);
177 DECLARE_SIGNAL_OUT(right_hand_vel, dynamicgraph::Vector);
178 DECLARE_SIGNAL_OUT(left_hand_vel, dynamicgraph::Vector);
179 DECLARE_SIGNAL_OUT(right_foot_acc, dynamicgraph::Vector);
180 DECLARE_SIGNAL_OUT(left_foot_acc, dynamicgraph::Vector);
181 DECLARE_SIGNAL_OUT(right_hand_acc, dynamicgraph::Vector);
182 DECLARE_SIGNAL_OUT(left_hand_acc, dynamicgraph::Vector);
183 DECLARE_SIGNAL_OUT(right_foot_acc_des, dynamicgraph::Vector);
184 DECLARE_SIGNAL_OUT(left_foot_acc_des, dynamicgraph::Vector);
187 DECLARE_SIGNAL_INNER(active_joints_checked, dynamicgraph::Vector);
191 virtual void display(std::ostream& os)
const;
193 void sendMsg(
const std::string& msg, MsgType t = MSG_TYPE_INFO,
const char* =
"",
int = 0) {
194 logger_.stream(t) << (
"[" + name +
"] " + msg) <<
'\n';
207 LEFT_SUPPORT_TRANSITION = 2,
209 RIGHT_SUPPORT_TRANSITION = 4
215 TASK_RIGHT_HAND_ON = 0,
217 TASK_RIGHT_HAND_OFF = 1
222 TASK_LEFT_HAND_ON = 0,
224 TASK_LEFT_HAND_OFF = 1
240 tsid::InverseDynamicsFormulationAccForce*
m_invDyn;
298 #endif // #ifndef __sot_torque_control_inverse_dynamics_balance_controller_H__ int m_frame_id_rf
end time of the current transition (if any)
tsid::trajectories::TrajectorySample m_sampleCom
int m_frame_id_lf
frame id of right foot
Eigen::Matrix< double, 3, 1 > Vector3
tsid::math::Vector m_dv_sot
tsid::math::Vector3 m_zmp_des_LF
3d CoM offset
tsid::math::Vector3 m_zmp_des
3d desired zmp left foot expressed in local frame
RightHandState m_rightHandState
tsid::solvers::SolverHQPBase * m_hqpSolver_60_36_34
Eigen::ColPivHouseholderQR< Matrix6x > m_J_RF_QR
tsid::math::Vector3 m_zmp_des_RF_local
3d desired zmp left foot expressed in local frame
tsid::tasks::TaskJointPosture * m_taskBlockedJoints
tsid::math::Vector3 m_com_offset
desired 6d wrench left foot
tsid::InverseDynamicsFormulationAccForce * m_invDyn
tsid::contacts::Contact6d * m_contactLH
tsid::math::Vector3 m_zmp_RF
3d zmp left foot
double m_t
control loop time period
bool m_enabled
true if the entity has been successfully initialized
tsid::trajectories::TrajectorySample m_samplePosture
RobotUtilShrPtr m_robot_util
pinocchio::Data::Matrix6x Matrix6x
tsid::math::Vector m_dv_urdf
desired accelerations (sot order)
tsid::tasks::TaskComEquality * m_taskCom
tsid::math::Vector m_tau_sot
3d global zmp
ContactState m_contactState
tsid::math::Vector3 m_zmp_LF
3d desired global zmp
tsid::tasks::TaskSE3Equality * m_taskLF
tsid::math::Vector m_q_urdf
tsid::math::Vector3 m_zmp_des_RF
3d desired zmp left foot
tsid::math::Vector6 m_v_LF_int
tsid::tasks::TaskJointPosture * m_taskPosture
tsid::contacts::Contact6d * m_contactRF
Eigen::Matrix< double, 6, 1 > Vector6
tsid::math::Vector m_v_urdf
tsid::tasks::TaskSE3Equality * m_taskLH
bool m_firstTime
True if controler is enabled.
ContactState
True at the first iteration of the controller.
tsid::trajectories::TrajectorySample m_sampleRH
tsid::solvers::SolverHQPBase * m_hqpSolver
int m_frame_id_lh
frame id of right hand
tsid::math::Vector3 m_zmp_des_LF_local
3d desired zmp left foot
RightHandState
end time of the current contact transition (if any)
LeftHandState m_leftHandState
tsid::contacts::Contact6d * m_contactRH
void sendMsg(const std::string &msg, MsgType t=MSG_TYPE_INFO, const char *="", int=0)
double m_contactTransitionTime
tsid::trajectories::TrajectorySample m_sampleLF
tsid::robots::RobotWrapper * m_robot
frame id of left hand
Eigen::ColPivHouseholderQR< Matrix6x > m_J_LF_QR
tsid::math::Vector3 m_zmp
3d zmp left foot
AdmittanceController EntityClassName
#define SOTINVERSEDYNAMICSBALANCECONTROLLER_EXPORT
tsid::contacts::Contact6d * m_contactLF
tsid::math::Vector6 m_f_RF
desired force coefficients (24d)
tsid::tasks::TaskSE3Equality * m_taskRH
tsid::math::Vector6 m_v_RF_int
int m_frame_id_rh
frame id of left foot
tsid::trajectories::TrajectorySample m_sampleLH
tsid::math::Vector6 m_f_LF
desired 6d wrench right foot
tsid::solvers::SolverHQPBase * m_hqpSolver_48_30_17
tsid::trajectories::TrajectorySample m_sampleRF
tsid::math::Vector m_f
desired accelerations (urdf order)
tsid::tasks::TaskSE3Equality * m_taskRF