sot-torque-control  1.6.5
Collection of dynamic-graph entities aimed at implementing torque control on different robots.
 
Loading...
Searching...
No Matches
inverse-dynamics-balance-controller.hh
Go to the documentation of this file.
1/*
2 * Copyright 2017, Andrea Del Prete, LAAS-CNRS
3 *
4 */
5
6#ifndef __sot_torque_control_inverse_dynamics_balance_controller_H__
7#define __sot_torque_control_inverse_dynamics_balance_controller_H__
8
9/* --------------------------------------------------------------------- */
10/* --- API ------------------------------------------------------------- */
11/* --------------------------------------------------------------------- */
12
13#if defined(WIN32)
14#if defined(inverse_dynamics_balance_controller_EXPORTS)
15#define SOTINVERSEDYNAMICSBALANCECONTROLLER_EXPORT __declspec(dllexport)
16#else
17#define SOTINVERSEDYNAMICSBALANCECONTROLLER_EXPORT __declspec(dllimport)
18#endif
19#else
20#define SOTINVERSEDYNAMICSBALANCECONTROLLER_EXPORT
21#endif
22
23/* --------------------------------------------------------------------- */
24/* --- INCLUDE --------------------------------------------------------- */
25/* --------------------------------------------------------------------- */
26
27#include <map>
28
29#include "boost/assign.hpp"
30
31/* Pinocchio */
32#include <pinocchio/multibody/model.hpp>
33#include <pinocchio/parsers/urdf.hpp>
34#include <tsid/contacts/contact-6d.hpp>
35#include <tsid/formulations/inverse-dynamics-formulation-acc-force.hpp>
36#include <tsid/robots/robot-wrapper.hpp>
37#include <tsid/solvers/solver-HQP-base.hpp>
38#include <tsid/tasks/task-com-equality.hpp>
39#include <tsid/tasks/task-joint-posture.hpp>
40#include <tsid/trajectories/trajectory-euclidian.hpp>
41
42/* HELPER */
43#include <dynamic-graph/signal-helper.h>
44
45#include <sot/core/matrix-geometry.hh>
46#include <sot/core/robot-utils.hh>
48
49namespace dynamicgraph {
50namespace sot {
51namespace torque_control {
52
53/* --------------------------------------------------------------------- */
54/* --- CLASS ----------------------------------------------------------- */
55/* --------------------------------------------------------------------- */
56
58 InverseDynamicsBalanceController : public ::dynamicgraph::Entity {
60 DYNAMIC_GRAPH_ENTITY_DECL();
61
62 public:
63 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
64
65 /* --- CONSTRUCTOR ---- */
66 InverseDynamicsBalanceController(const std::string& name);
67
68 void init(const double& dt, const std::string& robotRef);
69 void updateComOffset();
70 void removeRightFootContact(const double& transitionTime);
71 void removeLeftFootContact(const double& transitionTime);
72 void addRightFootContact(const double& transitionTime);
73 void addLeftFootContact(const double& transitionTime);
74 void addTaskRightHand(/*const double& transitionTime*/);
75 void removeTaskRightHand(const double& transitionTime);
76 void addTaskLeftHand(/*const double& transitionTime*/);
77 void removeTaskLeftHand(const double& transitionTime);
78
79 /* --- SIGNALS --- */
80 DECLARE_SIGNAL_IN(com_ref_pos, dynamicgraph::Vector);
81 DECLARE_SIGNAL_IN(com_ref_vel, dynamicgraph::Vector);
82 DECLARE_SIGNAL_IN(com_ref_acc, dynamicgraph::Vector);
83 DECLARE_SIGNAL_IN(rf_ref_pos, dynamicgraph::Vector);
84 DECLARE_SIGNAL_IN(rf_ref_vel, dynamicgraph::Vector);
85 DECLARE_SIGNAL_IN(rf_ref_acc, dynamicgraph::Vector);
86 DECLARE_SIGNAL_IN(lf_ref_pos, dynamicgraph::Vector);
87 DECLARE_SIGNAL_IN(lf_ref_vel, dynamicgraph::Vector);
88 DECLARE_SIGNAL_IN(lf_ref_acc, dynamicgraph::Vector);
89 DECLARE_SIGNAL_IN(rh_ref_pos, dynamicgraph::Vector);
90 DECLARE_SIGNAL_IN(rh_ref_vel, dynamicgraph::Vector);
91 DECLARE_SIGNAL_IN(rh_ref_acc, dynamicgraph::Vector);
92 DECLARE_SIGNAL_IN(lh_ref_pos, dynamicgraph::Vector);
93 DECLARE_SIGNAL_IN(lh_ref_vel, dynamicgraph::Vector);
94 DECLARE_SIGNAL_IN(lh_ref_acc, dynamicgraph::Vector);
95 DECLARE_SIGNAL_IN(posture_ref_pos, dynamicgraph::Vector);
96 DECLARE_SIGNAL_IN(posture_ref_vel, dynamicgraph::Vector);
97 DECLARE_SIGNAL_IN(posture_ref_acc, dynamicgraph::Vector);
98 DECLARE_SIGNAL_IN(base_orientation_ref_pos, dynamicgraph::Vector);
99 DECLARE_SIGNAL_IN(base_orientation_ref_vel, dynamicgraph::Vector);
100 DECLARE_SIGNAL_IN(base_orientation_ref_acc, dynamicgraph::Vector);
101 DECLARE_SIGNAL_IN(f_ref_right_foot, dynamicgraph::Vector);
102 DECLARE_SIGNAL_IN(f_ref_left_foot, dynamicgraph::Vector);
103
104 DECLARE_SIGNAL_IN(kp_base_orientation, dynamicgraph::Vector);
105 DECLARE_SIGNAL_IN(kd_base_orientation, dynamicgraph::Vector);
106 DECLARE_SIGNAL_IN(kp_constraints, dynamicgraph::Vector);
107 DECLARE_SIGNAL_IN(kd_constraints, dynamicgraph::Vector);
108 DECLARE_SIGNAL_IN(kp_com, dynamicgraph::Vector);
109 DECLARE_SIGNAL_IN(kd_com, dynamicgraph::Vector);
110 DECLARE_SIGNAL_IN(kp_feet, dynamicgraph::Vector);
111 DECLARE_SIGNAL_IN(kd_feet, dynamicgraph::Vector);
112 DECLARE_SIGNAL_IN(kp_hands, dynamicgraph::Vector);
113 DECLARE_SIGNAL_IN(kd_hands, dynamicgraph::Vector);
114 DECLARE_SIGNAL_IN(kp_posture, dynamicgraph::Vector);
115 DECLARE_SIGNAL_IN(kd_posture, dynamicgraph::Vector);
116 DECLARE_SIGNAL_IN(kp_pos, dynamicgraph::Vector);
117 DECLARE_SIGNAL_IN(kd_pos, dynamicgraph::Vector);
118
119 DECLARE_SIGNAL_IN(w_com, double);
120 DECLARE_SIGNAL_IN(w_feet, double);
121 DECLARE_SIGNAL_IN(w_hands, double);
122 DECLARE_SIGNAL_IN(w_posture, double);
123 DECLARE_SIGNAL_IN(w_base_orientation, double);
124 DECLARE_SIGNAL_IN(w_torques, double);
125 DECLARE_SIGNAL_IN(w_forces, double);
126 DECLARE_SIGNAL_IN(weight_contact_forces, dynamicgraph::Vector);
127
128 DECLARE_SIGNAL_IN(mu, double);
129 DECLARE_SIGNAL_IN(contact_points, dynamicgraph::Matrix);
130 DECLARE_SIGNAL_IN(contact_normal, dynamicgraph::Vector);
131 DECLARE_SIGNAL_IN(f_min, double);
132 DECLARE_SIGNAL_IN(f_max_right_foot, double);
133 DECLARE_SIGNAL_IN(f_max_left_foot, double);
134
135 DECLARE_SIGNAL_IN(rotor_inertias, dynamicgraph::Vector);
136 DECLARE_SIGNAL_IN(gear_ratios, dynamicgraph::Vector);
137 DECLARE_SIGNAL_IN(tau_max, dynamicgraph::Vector);
138 DECLARE_SIGNAL_IN(q_min, dynamicgraph::Vector);
139 DECLARE_SIGNAL_IN(q_max, dynamicgraph::Vector);
140 DECLARE_SIGNAL_IN(dq_max, dynamicgraph::Vector);
141 DECLARE_SIGNAL_IN(ddq_max, dynamicgraph::Vector);
142 DECLARE_SIGNAL_IN(dt_joint_pos_limits, double);
143
144 DECLARE_SIGNAL_IN(tau_estimated, dynamicgraph::Vector);
145 DECLARE_SIGNAL_IN(q, dynamicgraph::Vector);
146 DECLARE_SIGNAL_IN(v, dynamicgraph::Vector);
147 DECLARE_SIGNAL_IN(wrench_base, dynamicgraph::Vector);
148 DECLARE_SIGNAL_IN(wrench_left_foot, dynamicgraph::Vector);
149 DECLARE_SIGNAL_IN(wrench_right_foot, dynamicgraph::Vector);
150
152 active_joints,
153 dynamicgraph::Vector);
154
155 DECLARE_SIGNAL_OUT(tau_des, dynamicgraph::Vector);
156 DECLARE_SIGNAL_OUT(M, dynamicgraph::Matrix);
157 DECLARE_SIGNAL_OUT(dv_des, dynamicgraph::Vector);
158 DECLARE_SIGNAL_OUT(f_des_right_foot, dynamicgraph::Vector);
159 DECLARE_SIGNAL_OUT(f_des_left_foot, dynamicgraph::Vector);
160 DECLARE_SIGNAL_OUT(zmp_des_right_foot, dynamicgraph::Vector);
161 DECLARE_SIGNAL_OUT(zmp_des_left_foot, dynamicgraph::Vector);
162 DECLARE_SIGNAL_OUT(zmp_des_right_foot_local, dynamicgraph::Vector);
163 DECLARE_SIGNAL_OUT(zmp_des_left_foot_local, dynamicgraph::Vector);
164 DECLARE_SIGNAL_OUT(zmp_des, dynamicgraph::Vector);
165 DECLARE_SIGNAL_OUT(zmp_ref, dynamicgraph::Vector);
166 DECLARE_SIGNAL_OUT(zmp_right_foot, dynamicgraph::Vector);
167 DECLARE_SIGNAL_OUT(zmp_left_foot, dynamicgraph::Vector);
168 DECLARE_SIGNAL_OUT(zmp, dynamicgraph::Vector);
169 DECLARE_SIGNAL_OUT(com, dynamicgraph::Vector);
170 DECLARE_SIGNAL_OUT(com_vel, dynamicgraph::Vector);
171 DECLARE_SIGNAL_OUT(com_acc, dynamicgraph::Vector);
172 DECLARE_SIGNAL_OUT(com_acc_des, dynamicgraph::Vector);
173 DECLARE_SIGNAL_OUT(base_orientation, dynamicgraph::Vector);
174 DECLARE_SIGNAL_OUT(right_foot_pos, dynamicgraph::Vector);
175 DECLARE_SIGNAL_OUT(left_foot_pos, dynamicgraph::Vector);
176 DECLARE_SIGNAL_OUT(right_hand_pos, dynamicgraph::Vector);
177 DECLARE_SIGNAL_OUT(left_hand_pos, dynamicgraph::Vector);
178 DECLARE_SIGNAL_OUT(right_foot_vel, dynamicgraph::Vector);
179 DECLARE_SIGNAL_OUT(left_foot_vel, dynamicgraph::Vector);
180 DECLARE_SIGNAL_OUT(right_hand_vel, dynamicgraph::Vector);
181 DECLARE_SIGNAL_OUT(left_hand_vel, dynamicgraph::Vector);
182 DECLARE_SIGNAL_OUT(right_foot_acc, dynamicgraph::Vector);
183 DECLARE_SIGNAL_OUT(left_foot_acc, dynamicgraph::Vector);
184 DECLARE_SIGNAL_OUT(right_hand_acc, dynamicgraph::Vector);
185 DECLARE_SIGNAL_OUT(left_hand_acc, dynamicgraph::Vector);
186 DECLARE_SIGNAL_OUT(right_foot_acc_des, dynamicgraph::Vector);
187 DECLARE_SIGNAL_OUT(left_foot_acc_des, dynamicgraph::Vector);
188
191 DECLARE_SIGNAL_INNER(active_joints_checked, dynamicgraph::Vector);
192
193 /* --- COMMANDS --- */
194 /* --- ENTITY INHERITANCE --- */
195 virtual void display(std::ostream& os) const;
196
197 void sendMsg(const std::string& msg, MsgType t = MSG_TYPE_INFO,
198 const char* = "", int = 0) {
199 logger_.stream(t) << ("[" + name + "] " + msg) << '\n';
200 }
201
202 protected:
203 double m_dt;
204 double m_t;
205 bool
209
211 DOUBLE_SUPPORT = 0,
212 LEFT_SUPPORT = 1,
213 LEFT_SUPPORT_TRANSITION = 2, // transition towards left support
214 RIGHT_SUPPORT = 3,
215 RIGHT_SUPPORT_TRANSITION = 4
216 };
220
222 TASK_RIGHT_HAND_ON = 0,
223 /*TASK_RIGHT_HAND_TRANSITION = 1,*/
224 TASK_RIGHT_HAND_OFF = 1
225 };
227
229 TASK_LEFT_HAND_ON = 0,
230 /*TASK_LEFT_HAND_TRANSITION = 1,*/
231 TASK_LEFT_HAND_OFF = 1
232 };
234 /*double m_handsTransitionTime;*/
236
239
242
244 tsid::robots::RobotWrapper* m_robot;
245 tsid::solvers::SolverHQPBase* m_hqpSolver;
246 tsid::solvers::SolverHQPBase* m_hqpSolver_60_36_34;
247 tsid::solvers::SolverHQPBase* m_hqpSolver_48_30_17;
248 tsid::InverseDynamicsFormulationAccForce* m_invDyn;
249 tsid::contacts::Contact6d* m_contactRF;
250 tsid::contacts::Contact6d* m_contactLF;
251 tsid::contacts::Contact6d* m_contactRH;
252 tsid::contacts::Contact6d* m_contactLH;
253 tsid::tasks::TaskComEquality* m_taskCom;
254 tsid::tasks::TaskSE3Equality* m_taskRF;
255 tsid::tasks::TaskSE3Equality* m_taskLF;
256 tsid::tasks::TaskSE3Equality* m_taskRH;
257 tsid::tasks::TaskSE3Equality* m_taskLH;
258 tsid::tasks::TaskJointPosture* m_taskPosture;
259 tsid::tasks::TaskJointPosture* m_taskBlockedJoints;
260
261 tsid::trajectories::TrajectorySample m_sampleCom;
262 tsid::trajectories::TrajectorySample m_sampleRF;
263 tsid::trajectories::TrajectorySample m_sampleLF;
264 tsid::trajectories::TrajectorySample m_sampleRH;
265 tsid::trajectories::TrajectorySample m_sampleLH;
266 tsid::trajectories::TrajectorySample m_samplePosture;
267
268 double m_w_com;
270 double m_w_hands;
271
272 tsid::math::Vector m_dv_sot;
273 tsid::math::Vector m_dv_urdf;
274 tsid::math::Vector m_f;
275 tsid::math::Vector6 m_f_RF;
276 tsid::math::Vector6 m_f_LF;
277 tsid::math::Vector3 m_com_offset;
278 tsid::math::Vector3 m_zmp_des_LF;
279 tsid::math::Vector3 m_zmp_des_RF;
280 tsid::math::Vector3
282 tsid::math::Vector3
284 tsid::math::Vector3 m_zmp_des;
285 tsid::math::Vector3 m_zmp_LF;
286 tsid::math::Vector3 m_zmp_RF;
287 tsid::math::Vector3 m_zmp;
288 tsid::math::Vector m_tau_sot;
289 tsid::math::Vector m_q_urdf;
290 tsid::math::Vector m_v_urdf;
291
292 typedef pinocchio::Data::Matrix6x Matrix6x;
295 Eigen::ColPivHouseholderQR<Matrix6x> m_J_RF_QR;
296 Eigen::ColPivHouseholderQR<Matrix6x> m_J_LF_QR;
297 tsid::math::Vector6 m_v_RF_int;
298 tsid::math::Vector6 m_v_LF_int;
299
300 unsigned int m_timeLast;
301 RobotUtilShrPtr m_robot_util;
302
303}; // class InverseDynamicsBalanceController
304} // namespace torque_control
305} // namespace sot
306} // namespace dynamicgraph
307
308#endif // #ifndef __sot_torque_control_inverse_dynamics_balance_controller_H__
void sendMsg(const std::string &msg, MsgType t=MSG_TYPE_INFO, const char *="", int=0)
DECLARE_SIGNAL_IN(base_orientation_ref_acc, dynamicgraph::Vector)
DECLARE_SIGNAL_OUT(zmp_des_left_foot, dynamicgraph::Vector)
tsid::math::Vector3 m_zmp_des
3d desired zmp left foot expressed in local frame
DECLARE_SIGNAL_OUT(zmp_des_left_foot_local, dynamicgraph::Vector)
DECLARE_SIGNAL_OUT(f_des_right_foot, dynamicgraph::Vector)
DECLARE_SIGNAL_OUT(left_foot_acc_des, dynamicgraph::Vector)
DECLARE_SIGNAL_IN(base_orientation_ref_vel, dynamicgraph::Vector)
DECLARE_SIGNAL_INNER(active_joints_checked, dynamicgraph::Vector)
DECLARE_SIGNAL_IN(base_orientation_ref_pos, dynamicgraph::Vector)
DECLARE_SIGNAL_OUT(base_orientation, dynamicgraph::Vector)
DECLARE_SIGNAL_OUT(tau_des, dynamicgraph::Vector)
mask with 1 for controlled joints, 0 otherwise
tsid::math::Vector3 m_zmp_des_RF_local
3d desired zmp left foot expressed in local frame
DECLARE_SIGNAL_OUT(zmp_des_right_foot, dynamicgraph::Vector)
DECLARE_SIGNAL_OUT(zmp_des_right_foot_local, dynamicgraph::Vector)
DECLARE_SIGNAL_OUT(right_foot_acc_des, dynamicgraph::Vector)
DECLARE_SIGNAL_IN(kd_base_orientation, dynamicgraph::Vector)
DECLARE_SIGNAL_IN(wrench_right_foot, dynamicgraph::Vector)
DECLARE_SIGNAL_IN(weight_contact_forces, dynamicgraph::Vector)
DECLARE_SIGNAL_IN(kp_base_orientation, dynamicgraph::Vector)
#define SOTINVERSEDYNAMICSBALANCECONTROLLER_EXPORT
to read text file
Definition treeview.dox:22