1 |
|
|
/* |
2 |
|
|
* Copyright 2017, Andrea Del Prete, LAAS-CNRS |
3 |
|
|
* |
4 |
|
|
*/ |
5 |
|
|
|
6 |
|
|
#define EIGEN_RUNTIME_NO_MALLOC |
7 |
|
|
|
8 |
|
|
#include <dynamic-graph/factory.h> |
9 |
|
|
|
10 |
|
|
#include <boost/test/unit_test.hpp> |
11 |
|
|
#include <sot/core/debug.hh> |
12 |
|
|
#include <sot/torque_control/commands-helper.hh> |
13 |
|
|
#include <sot/torque_control/inverse-dynamics-balance-controller.hh> |
14 |
|
|
#include <tsid/math/utils.hpp> |
15 |
|
|
#include <tsid/solvers/solver-HQP-eiquadprog-rt.hpp> |
16 |
|
|
#include <tsid/solvers/solver-HQP-eiquadprog.hpp> |
17 |
|
|
#include <tsid/solvers/solver-HQP-factory.hxx> |
18 |
|
|
#include <tsid/solvers/utils.hpp> |
19 |
|
|
#include <tsid/utils/statistics.hpp> |
20 |
|
|
#include <tsid/utils/stop-watch.hpp> |
21 |
|
|
|
22 |
|
|
#if DEBUG |
23 |
|
|
#define ODEBUG(x) std::cout << x << std::endl |
24 |
|
|
#else |
25 |
|
|
#define ODEBUG(x) |
26 |
|
|
#endif |
27 |
|
|
#define ODEBUG3(x) std::cout << x << std::endl |
28 |
|
|
|
29 |
|
|
#define DBGFILE "/tmp/debug-sot-torqe-control.dat" |
30 |
|
|
|
31 |
|
|
#define RESETDEBUG5() \ |
32 |
|
|
{ \ |
33 |
|
|
std::ofstream DebugFile; \ |
34 |
|
|
DebugFile.open(DBGFILE, std::ofstream::out); \ |
35 |
|
|
DebugFile.close(); \ |
36 |
|
|
} |
37 |
|
|
#define ODEBUG5FULL(x) \ |
38 |
|
|
{ \ |
39 |
|
|
std::ofstream DebugFile; \ |
40 |
|
|
DebugFile.open(DBGFILE, std::ofstream::app); \ |
41 |
|
|
DebugFile << __FILE__ << ":" << __FUNCTION__ << "(#" << __LINE__ \ |
42 |
|
|
<< "):" << x << std::endl; \ |
43 |
|
|
DebugFile.close(); \ |
44 |
|
|
} |
45 |
|
|
#define ODEBUG5(x) \ |
46 |
|
|
{ \ |
47 |
|
|
std::ofstream DebugFile; \ |
48 |
|
|
DebugFile.open(DBGFILE, std::ofstream::app); \ |
49 |
|
|
DebugFile << x << std::endl; \ |
50 |
|
|
DebugFile.close(); \ |
51 |
|
|
} |
52 |
|
|
|
53 |
|
|
#define RESETDEBUG4() |
54 |
|
|
#define ODEBUG4FULL(x) |
55 |
|
|
#define ODEBUG4(x) |
56 |
|
|
|
57 |
|
|
namespace dynamicgraph { |
58 |
|
|
namespace sot { |
59 |
|
|
namespace torque_control { |
60 |
|
|
namespace dg = ::dynamicgraph; |
61 |
|
|
using namespace dg; |
62 |
|
|
using namespace dg::command; |
63 |
|
|
using namespace std; |
64 |
|
|
using namespace tsid; |
65 |
|
|
using namespace tsid::trajectories; |
66 |
|
|
using namespace tsid::math; |
67 |
|
|
using namespace tsid::contacts; |
68 |
|
|
using namespace tsid::tasks; |
69 |
|
|
using namespace tsid::solvers; |
70 |
|
|
using namespace dg::sot; |
71 |
|
|
|
72 |
|
|
typedef SolverHQuadProgRT<60, 36, 34> SolverHQuadProgRT60x36x34; |
73 |
|
|
typedef SolverHQuadProgRT<48, 30, 17> SolverHQuadProgRT48x30x17; |
74 |
|
|
|
75 |
|
|
#define REQUIRE_FINITE(A) assert(is_finite(A)) |
76 |
|
|
|
77 |
|
|
// Size to be aligned "-------------------------------------------------------" |
78 |
|
|
#define PROFILE_TAU_DES_COMPUTATION "InvDynBalCtrl: desired tau" |
79 |
|
|
#define PROFILE_HQP_SOLUTION "InvDynBalCtrl: HQP" |
80 |
|
|
#define PROFILE_PREPARE_INV_DYN "InvDynBalCtrl: prepare inv-dyn" |
81 |
|
|
#define PROFILE_READ_INPUT_SIGNALS "InvDynBalCtrl: read input signals" |
82 |
|
|
|
83 |
|
|
#define ZERO_FORCE_THRESHOLD 1e-3 |
84 |
|
|
|
85 |
|
|
#define INPUT_SIGNALS \ |
86 |
|
|
m_com_ref_posSIN << m_com_ref_velSIN << m_com_ref_accSIN << m_rf_ref_posSIN \ |
87 |
|
|
<< m_rf_ref_velSIN << m_rf_ref_accSIN << m_lf_ref_posSIN \ |
88 |
|
|
<< m_lf_ref_velSIN << m_lf_ref_accSIN << m_rh_ref_posSIN \ |
89 |
|
|
<< m_rh_ref_velSIN << m_rh_ref_accSIN << m_lh_ref_posSIN \ |
90 |
|
|
<< m_lh_ref_velSIN << m_lh_ref_accSIN \ |
91 |
|
|
<< m_posture_ref_posSIN << m_posture_ref_velSIN \ |
92 |
|
|
<< m_posture_ref_accSIN << m_base_orientation_ref_posSIN \ |
93 |
|
|
<< m_base_orientation_ref_velSIN \ |
94 |
|
|
<< m_base_orientation_ref_accSIN << m_f_ref_right_footSIN \ |
95 |
|
|
<< m_f_ref_left_footSIN << m_kp_base_orientationSIN \ |
96 |
|
|
<< m_kd_base_orientationSIN << m_kp_constraintsSIN \ |
97 |
|
|
<< m_kd_constraintsSIN << m_kp_comSIN << m_kd_comSIN \ |
98 |
|
|
<< m_kp_feetSIN << m_kd_feetSIN << m_kp_handsSIN \ |
99 |
|
|
<< m_kd_handsSIN << m_kp_postureSIN << m_kd_postureSIN \ |
100 |
|
|
<< m_kp_posSIN << m_kd_posSIN << m_w_comSIN << m_w_feetSIN \ |
101 |
|
|
<< m_w_handsSIN << m_w_postureSIN \ |
102 |
|
|
<< m_w_base_orientationSIN << m_w_torquesSIN \ |
103 |
|
|
<< m_w_forcesSIN << m_weight_contact_forcesSIN << m_muSIN \ |
104 |
|
|
<< m_contact_pointsSIN << m_contact_normalSIN << m_f_minSIN \ |
105 |
|
|
<< m_f_max_right_footSIN << m_f_max_left_footSIN \ |
106 |
|
|
<< m_rotor_inertiasSIN << m_gear_ratiosSIN << m_tau_maxSIN \ |
107 |
|
|
<< m_q_minSIN << m_q_maxSIN << m_dq_maxSIN << m_ddq_maxSIN \ |
108 |
|
|
<< m_dt_joint_pos_limitsSIN << m_tau_estimatedSIN << m_qSIN \ |
109 |
|
|
<< m_vSIN << m_wrench_baseSIN << m_wrench_left_footSIN \ |
110 |
|
|
<< m_wrench_right_footSIN << m_active_jointsSIN |
111 |
|
|
|
112 |
|
|
#define OUTPUT_SIGNALS \ |
113 |
|
|
m_tau_desSOUT << m_MSOUT << m_dv_desSOUT << m_f_des_right_footSOUT \ |
114 |
|
|
<< m_f_des_left_footSOUT << m_zmp_des_right_footSOUT \ |
115 |
|
|
<< m_zmp_des_left_footSOUT << m_zmp_des_right_foot_localSOUT \ |
116 |
|
|
<< m_zmp_des_left_foot_localSOUT << m_zmp_desSOUT \ |
117 |
|
|
<< m_zmp_refSOUT << m_zmp_right_footSOUT \ |
118 |
|
|
<< m_zmp_left_footSOUT << m_zmpSOUT << m_comSOUT \ |
119 |
|
|
<< m_com_velSOUT << m_com_accSOUT << m_com_acc_desSOUT \ |
120 |
|
|
<< m_base_orientationSOUT << m_left_foot_posSOUT \ |
121 |
|
|
<< m_right_foot_posSOUT << m_left_hand_posSOUT \ |
122 |
|
|
<< m_right_hand_posSOUT << m_left_foot_velSOUT \ |
123 |
|
|
<< m_right_foot_velSOUT << m_left_hand_velSOUT \ |
124 |
|
|
<< m_right_hand_velSOUT << m_left_foot_accSOUT \ |
125 |
|
|
<< m_right_foot_accSOUT << m_left_hand_accSOUT \ |
126 |
|
|
<< m_right_hand_accSOUT << m_right_foot_acc_desSOUT \ |
127 |
|
|
<< m_left_foot_acc_desSOUT |
128 |
|
|
|
129 |
|
|
/// Define EntityClassName here rather than in the header file |
130 |
|
|
/// so that it can be used by the macros DEFINE_SIGNAL_**_FUNCTION. |
131 |
|
|
typedef InverseDynamicsBalanceController EntityClassName; |
132 |
|
|
|
133 |
|
|
typedef Eigen::Matrix<double, 2, 1> Vector2; |
134 |
|
|
typedef Eigen::Matrix<double, Eigen::Dynamic, 1> VectorN; |
135 |
|
|
typedef Eigen::Matrix<double, Eigen::Dynamic, 1> VectorN6; |
136 |
|
|
/* --- DG FACTORY ---------------------------------------------------- */ |
137 |
|
|
DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(InverseDynamicsBalanceController, |
138 |
|
|
"InverseDynamicsBalanceController"); |
139 |
|
|
|
140 |
|
|
/* ------------------------------------------------------------------- */ |
141 |
|
|
/* --- CONSTRUCTION -------------------------------------------------- */ |
142 |
|
|
/* ------------------------------------------------------------------- */ |
143 |
|
|
InverseDynamicsBalanceController::InverseDynamicsBalanceController( |
144 |
|
|
const std::string& name) |
145 |
|
|
: Entity(name), |
146 |
|
|
CONSTRUCT_SIGNAL_IN(com_ref_pos, dynamicgraph::Vector), |
147 |
|
|
CONSTRUCT_SIGNAL_IN(com_ref_vel, dynamicgraph::Vector), |
148 |
|
|
CONSTRUCT_SIGNAL_IN(com_ref_acc, dynamicgraph::Vector), |
149 |
|
|
CONSTRUCT_SIGNAL_IN(rf_ref_pos, dynamicgraph::Vector), |
150 |
|
|
CONSTRUCT_SIGNAL_IN(rf_ref_vel, dynamicgraph::Vector), |
151 |
|
|
CONSTRUCT_SIGNAL_IN(rf_ref_acc, dynamicgraph::Vector), |
152 |
|
|
CONSTRUCT_SIGNAL_IN(lf_ref_pos, dynamicgraph::Vector), |
153 |
|
|
CONSTRUCT_SIGNAL_IN(lf_ref_vel, dynamicgraph::Vector), |
154 |
|
|
CONSTRUCT_SIGNAL_IN(lf_ref_acc, dynamicgraph::Vector), |
155 |
|
|
CONSTRUCT_SIGNAL_IN(rh_ref_pos, dynamicgraph::Vector), |
156 |
|
|
CONSTRUCT_SIGNAL_IN(rh_ref_vel, dynamicgraph::Vector), |
157 |
|
|
CONSTRUCT_SIGNAL_IN(rh_ref_acc, dynamicgraph::Vector), |
158 |
|
|
CONSTRUCT_SIGNAL_IN(lh_ref_pos, dynamicgraph::Vector), |
159 |
|
|
CONSTRUCT_SIGNAL_IN(lh_ref_vel, dynamicgraph::Vector), |
160 |
|
|
CONSTRUCT_SIGNAL_IN(lh_ref_acc, dynamicgraph::Vector), |
161 |
|
|
CONSTRUCT_SIGNAL_IN(posture_ref_pos, dynamicgraph::Vector), |
162 |
|
|
CONSTRUCT_SIGNAL_IN(posture_ref_vel, dynamicgraph::Vector), |
163 |
|
|
CONSTRUCT_SIGNAL_IN(posture_ref_acc, dynamicgraph::Vector), |
164 |
|
|
CONSTRUCT_SIGNAL_IN(base_orientation_ref_pos, dynamicgraph::Vector), |
165 |
|
|
CONSTRUCT_SIGNAL_IN(base_orientation_ref_vel, dynamicgraph::Vector), |
166 |
|
|
CONSTRUCT_SIGNAL_IN(base_orientation_ref_acc, dynamicgraph::Vector), |
167 |
|
|
CONSTRUCT_SIGNAL_IN(f_ref_right_foot, dynamicgraph::Vector), |
168 |
|
|
CONSTRUCT_SIGNAL_IN(f_ref_left_foot, dynamicgraph::Vector), |
169 |
|
|
CONSTRUCT_SIGNAL_IN(kp_base_orientation, dynamicgraph::Vector), |
170 |
|
|
CONSTRUCT_SIGNAL_IN(kd_base_orientation, dynamicgraph::Vector), |
171 |
|
|
CONSTRUCT_SIGNAL_IN(kp_constraints, dynamicgraph::Vector), |
172 |
|
|
CONSTRUCT_SIGNAL_IN(kd_constraints, dynamicgraph::Vector), |
173 |
|
|
CONSTRUCT_SIGNAL_IN(kp_com, dynamicgraph::Vector), |
174 |
|
|
CONSTRUCT_SIGNAL_IN(kd_com, dynamicgraph::Vector), |
175 |
|
|
CONSTRUCT_SIGNAL_IN(kp_feet, dynamicgraph::Vector), |
176 |
|
|
CONSTRUCT_SIGNAL_IN(kd_feet, dynamicgraph::Vector), |
177 |
|
|
CONSTRUCT_SIGNAL_IN(kp_hands, dynamicgraph::Vector), |
178 |
|
|
CONSTRUCT_SIGNAL_IN(kd_hands, dynamicgraph::Vector), |
179 |
|
|
CONSTRUCT_SIGNAL_IN(kp_posture, dynamicgraph::Vector), |
180 |
|
|
CONSTRUCT_SIGNAL_IN(kd_posture, dynamicgraph::Vector), |
181 |
|
|
CONSTRUCT_SIGNAL_IN(kp_pos, dynamicgraph::Vector), |
182 |
|
|
CONSTRUCT_SIGNAL_IN(kd_pos, dynamicgraph::Vector), |
183 |
|
|
CONSTRUCT_SIGNAL_IN(w_com, double), |
184 |
|
|
CONSTRUCT_SIGNAL_IN(w_feet, double), |
185 |
|
|
CONSTRUCT_SIGNAL_IN(w_hands, double), |
186 |
|
|
CONSTRUCT_SIGNAL_IN(w_posture, double), |
187 |
|
|
CONSTRUCT_SIGNAL_IN(w_base_orientation, double), |
188 |
|
|
CONSTRUCT_SIGNAL_IN(w_torques, double), |
189 |
|
|
CONSTRUCT_SIGNAL_IN(w_forces, double), |
190 |
|
|
CONSTRUCT_SIGNAL_IN(weight_contact_forces, dynamicgraph::Vector), |
191 |
|
|
CONSTRUCT_SIGNAL_IN(mu, double), |
192 |
|
|
CONSTRUCT_SIGNAL_IN(contact_points, dynamicgraph::Matrix), |
193 |
|
|
CONSTRUCT_SIGNAL_IN(contact_normal, dynamicgraph::Vector), |
194 |
|
|
CONSTRUCT_SIGNAL_IN(f_min, double), |
195 |
|
|
CONSTRUCT_SIGNAL_IN(f_max_right_foot, double), |
196 |
|
|
CONSTRUCT_SIGNAL_IN(f_max_left_foot, double), |
197 |
|
|
CONSTRUCT_SIGNAL_IN(rotor_inertias, dynamicgraph::Vector), |
198 |
|
|
CONSTRUCT_SIGNAL_IN(gear_ratios, dynamicgraph::Vector), |
199 |
|
|
CONSTRUCT_SIGNAL_IN(tau_max, dynamicgraph::Vector), |
200 |
|
|
CONSTRUCT_SIGNAL_IN(q_min, dynamicgraph::Vector), |
201 |
|
|
CONSTRUCT_SIGNAL_IN(q_max, dynamicgraph::Vector), |
202 |
|
|
CONSTRUCT_SIGNAL_IN(dq_max, dynamicgraph::Vector), |
203 |
|
|
CONSTRUCT_SIGNAL_IN(ddq_max, dynamicgraph::Vector), |
204 |
|
|
CONSTRUCT_SIGNAL_IN(dt_joint_pos_limits, double), |
205 |
|
|
CONSTRUCT_SIGNAL_IN(tau_estimated, dynamicgraph::Vector), |
206 |
|
|
CONSTRUCT_SIGNAL_IN(q, dynamicgraph::Vector), |
207 |
|
|
CONSTRUCT_SIGNAL_IN(v, dynamicgraph::Vector), |
208 |
|
|
CONSTRUCT_SIGNAL_IN(wrench_base, dynamicgraph::Vector), |
209 |
|
|
CONSTRUCT_SIGNAL_IN(wrench_left_foot, dynamicgraph::Vector), |
210 |
|
|
CONSTRUCT_SIGNAL_IN(wrench_right_foot, dynamicgraph::Vector), |
211 |
|
|
CONSTRUCT_SIGNAL_IN(active_joints, dynamicgraph::Vector), |
212 |
|
|
CONSTRUCT_SIGNAL_OUT(tau_des, dynamicgraph::Vector, INPUT_SIGNALS), |
213 |
|
|
CONSTRUCT_SIGNAL_OUT(M, dg::Matrix, m_tau_desSOUT), |
214 |
|
|
CONSTRUCT_SIGNAL_OUT(dv_des, dg::Vector, m_tau_desSOUT), |
215 |
|
|
CONSTRUCT_SIGNAL_OUT(f_des_right_foot, dynamicgraph::Vector, |
216 |
|
|
m_tau_desSOUT), |
217 |
|
|
CONSTRUCT_SIGNAL_OUT(f_des_left_foot, dynamicgraph::Vector, |
218 |
|
|
m_tau_desSOUT), |
219 |
|
|
CONSTRUCT_SIGNAL_OUT(zmp_des_right_foot, dynamicgraph::Vector, |
220 |
|
|
m_f_des_right_footSOUT), |
221 |
|
|
CONSTRUCT_SIGNAL_OUT(zmp_des_left_foot, dynamicgraph::Vector, |
222 |
|
|
m_f_des_left_footSOUT), |
223 |
|
|
CONSTRUCT_SIGNAL_OUT(zmp_des_right_foot_local, dynamicgraph::Vector, |
224 |
|
|
m_f_des_right_footSOUT), |
225 |
|
|
CONSTRUCT_SIGNAL_OUT(zmp_des_left_foot_local, dynamicgraph::Vector, |
226 |
|
|
m_f_des_left_footSOUT), |
227 |
|
|
CONSTRUCT_SIGNAL_OUT(zmp_des, dynamicgraph::Vector, |
228 |
|
|
m_zmp_des_left_footSOUT << m_zmp_des_right_footSOUT), |
229 |
|
|
CONSTRUCT_SIGNAL_OUT(zmp_ref, dynamicgraph::Vector, |
230 |
|
|
m_f_ref_left_footSIN << m_f_ref_right_footSIN), |
231 |
|
|
CONSTRUCT_SIGNAL_OUT(zmp_right_foot, dg::Vector, m_wrench_right_footSIN), |
232 |
|
|
CONSTRUCT_SIGNAL_OUT(zmp_left_foot, dg::Vector, m_wrench_left_footSIN), |
233 |
|
|
CONSTRUCT_SIGNAL_OUT(zmp, dg::Vector, |
234 |
|
|
m_wrench_left_footSIN << m_wrench_right_footSIN |
235 |
|
|
<< m_zmp_left_footSOUT |
236 |
|
|
<< m_zmp_right_footSOUT), |
237 |
|
|
CONSTRUCT_SIGNAL_OUT(com, dg::Vector, m_tau_desSOUT), |
238 |
|
|
CONSTRUCT_SIGNAL_OUT(com_vel, dg::Vector, m_tau_desSOUT), |
239 |
|
|
CONSTRUCT_SIGNAL_OUT(com_acc, dg::Vector, m_tau_desSOUT), |
240 |
|
|
CONSTRUCT_SIGNAL_OUT(com_acc_des, dg::Vector, m_tau_desSOUT), |
241 |
|
|
CONSTRUCT_SIGNAL_OUT(base_orientation, dg::Vector, m_tau_desSOUT), |
242 |
|
|
CONSTRUCT_SIGNAL_OUT(right_foot_pos, dg::Vector, m_tau_desSOUT), |
243 |
|
|
CONSTRUCT_SIGNAL_OUT(left_foot_pos, dg::Vector, m_tau_desSOUT), |
244 |
|
|
CONSTRUCT_SIGNAL_OUT(right_hand_pos, dg::Vector, m_tau_desSOUT), |
245 |
|
|
CONSTRUCT_SIGNAL_OUT(left_hand_pos, dg::Vector, m_tau_desSOUT), |
246 |
|
|
CONSTRUCT_SIGNAL_OUT(right_foot_vel, dg::Vector, m_tau_desSOUT), |
247 |
|
|
CONSTRUCT_SIGNAL_OUT(left_foot_vel, dg::Vector, m_tau_desSOUT), |
248 |
|
|
CONSTRUCT_SIGNAL_OUT(right_hand_vel, dg::Vector, m_tau_desSOUT), |
249 |
|
|
CONSTRUCT_SIGNAL_OUT(left_hand_vel, dg::Vector, m_tau_desSOUT), |
250 |
|
|
CONSTRUCT_SIGNAL_OUT(right_foot_acc, dg::Vector, m_tau_desSOUT), |
251 |
|
|
CONSTRUCT_SIGNAL_OUT(left_foot_acc, dg::Vector, m_tau_desSOUT), |
252 |
|
|
CONSTRUCT_SIGNAL_OUT(right_hand_acc, dg::Vector, m_tau_desSOUT), |
253 |
|
|
CONSTRUCT_SIGNAL_OUT(left_hand_acc, dg::Vector, m_tau_desSOUT), |
254 |
|
|
CONSTRUCT_SIGNAL_OUT(right_foot_acc_des, dg::Vector, m_tau_desSOUT), |
255 |
|
|
CONSTRUCT_SIGNAL_OUT(left_foot_acc_des, dg::Vector, m_tau_desSOUT), |
256 |
|
|
CONSTRUCT_SIGNAL_INNER(active_joints_checked, dg::Vector, |
257 |
|
|
m_active_jointsSIN), |
258 |
|
|
m_t(0.0), |
259 |
|
|
m_initSucceeded(false), |
260 |
|
|
m_enabled(false), |
261 |
|
|
m_firstTime(true), |
262 |
|
|
m_contactState(DOUBLE_SUPPORT), |
263 |
|
|
m_rightHandState(TASK_RIGHT_HAND_OFF), |
264 |
|
|
m_leftHandState(TASK_LEFT_HAND_OFF), |
265 |
|
|
m_timeLast(0), |
266 |
|
|
m_robot_util(RefVoidRobotUtil()) { |
267 |
|
|
RESETDEBUG5(); |
268 |
|
|
Entity::signalRegistration(INPUT_SIGNALS << OUTPUT_SIGNALS); |
269 |
|
|
|
270 |
|
|
m_zmp_des_RF.setZero(); |
271 |
|
|
m_zmp_des_LF.setZero(); |
272 |
|
|
m_zmp_des_RF_local.setZero(); |
273 |
|
|
m_zmp_des_LF_local.setZero(); |
274 |
|
|
m_zmp_des.setZero(); |
275 |
|
|
m_zmp_RF.setZero(); |
276 |
|
|
m_zmp_LF.setZero(); |
277 |
|
|
m_zmp.setZero(); |
278 |
|
|
m_com_offset.setZero(); |
279 |
|
|
m_v_RF_int.setZero(); |
280 |
|
|
m_v_LF_int.setZero(); |
281 |
|
|
|
282 |
|
|
/* Commands. */ |
283 |
|
|
addCommand("init", |
284 |
|
|
makeCommandVoid2(*this, &InverseDynamicsBalanceController::init, |
285 |
|
|
docCommandVoid2("Initialize the entity.", |
286 |
|
|
"Time period in seconds (double)", |
287 |
|
|
"Robot reference (string)"))); |
288 |
|
|
|
289 |
|
|
addCommand( |
290 |
|
|
"updateComOffset", |
291 |
|
|
makeCommandVoid0( |
292 |
|
|
*this, &InverseDynamicsBalanceController::updateComOffset, |
293 |
|
|
docCommandVoid0( |
294 |
|
|
"Update the offset on the CoM based on the CoP measurement."))); |
295 |
|
|
|
296 |
|
|
addCommand( |
297 |
|
|
"removeRightFootContact", |
298 |
|
|
makeCommandVoid1( |
299 |
|
|
*this, &InverseDynamicsBalanceController::removeRightFootContact, |
300 |
|
|
docCommandVoid1("Remove the contact at the right foot.", |
301 |
|
|
"Transition time in seconds (double)"))); |
302 |
|
|
|
303 |
|
|
addCommand( |
304 |
|
|
"removeLeftFootContact", |
305 |
|
|
makeCommandVoid1(*this, |
306 |
|
|
&InverseDynamicsBalanceController::removeLeftFootContact, |
307 |
|
|
docCommandVoid1("Remove the contact at the left foot.", |
308 |
|
|
"Transition time in seconds (double)"))); |
309 |
|
|
addCommand("addTaskRightHand", |
310 |
|
|
makeCommandVoid0( |
311 |
|
|
*this, &InverseDynamicsBalanceController::addTaskRightHand, |
312 |
|
|
docCommandVoid0("Adds an SE3 task for the right hand."))); |
313 |
|
|
addCommand("removeTaskRightHand", |
314 |
|
|
makeCommandVoid1( |
315 |
|
|
*this, &InverseDynamicsBalanceController::removeTaskRightHand, |
316 |
|
|
docCommandVoid1("Removes the SE3 task for the right hand.", |
317 |
|
|
"Transition time in seconds (double)"))); |
318 |
|
|
addCommand("addTaskLeftHand", |
319 |
|
|
makeCommandVoid0( |
320 |
|
|
*this, &InverseDynamicsBalanceController::addTaskLeftHand, |
321 |
|
|
docCommandVoid0("Raises the left hand."))); |
322 |
|
|
addCommand("removeTaskLeftHand", |
323 |
|
|
makeCommandVoid1( |
324 |
|
|
*this, &InverseDynamicsBalanceController::removeTaskLeftHand, |
325 |
|
|
docCommandVoid1("lowers the left hand.", |
326 |
|
|
"Transition time in seconds (double)"))); |
327 |
|
|
} |
328 |
|
|
|
329 |
|
|
void InverseDynamicsBalanceController::updateComOffset() { |
330 |
|
|
const Vector3& com = m_robot->com(m_invDyn->data()); |
331 |
|
|
m_com_offset = m_zmp - com; |
332 |
|
|
m_com_offset(2) = 0.0; |
333 |
|
|
SEND_MSG("CoM offset updated: " + toString(m_com_offset), MSG_TYPE_INFO); |
334 |
|
|
} |
335 |
|
|
|
336 |
|
|
void InverseDynamicsBalanceController::removeRightFootContact( |
337 |
|
|
const double& transitionTime) { |
338 |
|
|
if (m_contactState == DOUBLE_SUPPORT) { |
339 |
|
|
SEND_MSG("Remove right foot contact in " + toString(transitionTime) + " s", |
340 |
|
|
MSG_TYPE_INFO); |
341 |
|
|
bool res = |
342 |
|
|
m_invDyn->removeRigidContact(m_contactRF->name(), transitionTime); |
343 |
|
|
if (!res) { |
344 |
|
|
const HQPData& hqpData = |
345 |
|
|
m_invDyn->computeProblemData(m_t, m_q_urdf, m_v_urdf); |
346 |
|
|
SEND_MSG("Error while remove right foot contact." + |
347 |
|
|
tsid::solvers::HQPDataToString(hqpData, false), |
348 |
|
|
MSG_TYPE_ERROR); |
349 |
|
|
} |
350 |
|
|
const double& w_feet = m_w_feetSIN.accessCopy(); |
351 |
|
|
m_invDyn->addMotionTask(*m_taskRF, w_feet, 1); |
352 |
|
|
if (transitionTime > m_dt) { |
353 |
|
|
m_contactState = LEFT_SUPPORT_TRANSITION; |
354 |
|
|
m_contactTransitionTime = m_t + transitionTime; |
355 |
|
|
} else |
356 |
|
|
m_contactState = LEFT_SUPPORT; |
357 |
|
|
} |
358 |
|
|
} |
359 |
|
|
|
360 |
|
|
void InverseDynamicsBalanceController::removeLeftFootContact( |
361 |
|
|
const double& transitionTime) { |
362 |
|
|
if (m_contactState == DOUBLE_SUPPORT) { |
363 |
|
|
SEND_MSG("Remove left foot contact in " + toString(transitionTime) + " s", |
364 |
|
|
MSG_TYPE_INFO); |
365 |
|
|
bool res = |
366 |
|
|
m_invDyn->removeRigidContact(m_contactLF->name(), transitionTime); |
367 |
|
|
if (!res) { |
368 |
|
|
const HQPData& hqpData = |
369 |
|
|
m_invDyn->computeProblemData(m_t, m_q_urdf, m_v_urdf); |
370 |
|
|
SEND_MSG("Error while remove right foot contact." + |
371 |
|
|
tsid::solvers::HQPDataToString(hqpData, false), |
372 |
|
|
MSG_TYPE_ERROR); |
373 |
|
|
} |
374 |
|
|
const double& w_feet = m_w_feetSIN.accessCopy(); |
375 |
|
|
m_invDyn->addMotionTask(*m_taskLF, w_feet, 1); |
376 |
|
|
if (transitionTime > m_dt) { |
377 |
|
|
m_contactState = RIGHT_SUPPORT_TRANSITION; |
378 |
|
|
m_contactTransitionTime = m_t + transitionTime; |
379 |
|
|
} else |
380 |
|
|
m_contactState = RIGHT_SUPPORT; |
381 |
|
|
} |
382 |
|
|
} |
383 |
|
|
|
384 |
|
|
void InverseDynamicsBalanceController::addTaskRightHand( |
385 |
|
|
/*const double& transitionTime*/) { |
386 |
|
|
if (m_rightHandState == TASK_RIGHT_HAND_OFF) { |
387 |
|
|
SEND_MSG("Adds right hand SE3 task in " /*+toString(transitionTime)+" s"*/, |
388 |
|
|
MSG_TYPE_INFO); |
389 |
|
|
const double& w_hands = m_w_handsSIN.accessCopy(); |
390 |
|
|
m_invDyn->addMotionTask(*m_taskRH, w_hands, 1); |
391 |
|
|
} |
392 |
|
|
/*if(transitionTime>m_dt) |
393 |
|
|
{ |
394 |
|
|
m_rightHandState = TASK_RIGHT_HAND_TRANSITION; |
395 |
|
|
m_handsTransitionTime = m_t + transitionTime; |
396 |
|
|
} |
397 |
|
|
else |
398 |
|
|
m_rightHandState = TASK_RIGHT_HAND_ON;*/ |
399 |
|
|
m_rightHandState = TASK_RIGHT_HAND_ON; |
400 |
|
|
} |
401 |
|
|
|
402 |
|
|
void InverseDynamicsBalanceController::addTaskLeftHand( |
403 |
|
|
/*const double& transitionTime*/) { |
404 |
|
|
if (m_leftHandState == TASK_LEFT_HAND_OFF) { |
405 |
|
|
SEND_MSG("Raise left hand in " /*+toString(transitionTime)+" s"*/, |
406 |
|
|
MSG_TYPE_INFO); |
407 |
|
|
const double& w_hands = m_w_handsSIN.accessCopy(); |
408 |
|
|
m_invDyn->addMotionTask(*m_taskLH, w_hands, 1); |
409 |
|
|
} |
410 |
|
|
/*if(transitionTime>m_dt) |
411 |
|
|
{ |
412 |
|
|
m_leftHandState = TASK_LEFT_HAND_TRANSITION; |
413 |
|
|
m_handsTransitionTime = m_t + transitionTime; |
414 |
|
|
} |
415 |
|
|
else |
416 |
|
|
m_leftHandState = TASK_LEFT_HAND_ON;*/ |
417 |
|
|
m_leftHandState = TASK_LEFT_HAND_ON; |
418 |
|
|
} |
419 |
|
|
|
420 |
|
|
void InverseDynamicsBalanceController::addRightFootContact( |
421 |
|
|
const double& transitionTime) { |
422 |
|
|
if (m_contactState == LEFT_SUPPORT) { |
423 |
|
|
SEND_MSG("Add right foot contact in " + toString(transitionTime) + " s", |
424 |
|
|
MSG_TYPE_INFO); |
425 |
|
|
const double& w_forces = m_w_forcesSIN.accessCopy(); |
426 |
|
|
m_invDyn->addRigidContact(*m_contactRF, w_forces); |
427 |
|
|
m_invDyn->removeTask(m_taskRF->name(), transitionTime); |
428 |
|
|
m_contactState = DOUBLE_SUPPORT; |
429 |
|
|
} |
430 |
|
|
} |
431 |
|
|
|
432 |
|
|
void InverseDynamicsBalanceController::addLeftFootContact( |
433 |
|
|
const double& transitionTime) { |
434 |
|
|
if (m_contactState == RIGHT_SUPPORT) { |
435 |
|
|
SEND_MSG("Add left foot contact in " + toString(transitionTime) + " s", |
436 |
|
|
MSG_TYPE_INFO); |
437 |
|
|
const double& w_forces = m_w_forcesSIN.accessCopy(); |
438 |
|
|
m_invDyn->addRigidContact(*m_contactLF, w_forces); |
439 |
|
|
m_invDyn->removeTask(m_taskLF->name(), transitionTime); |
440 |
|
|
m_contactState = DOUBLE_SUPPORT; |
441 |
|
|
} |
442 |
|
|
} |
443 |
|
|
|
444 |
|
|
void InverseDynamicsBalanceController::removeTaskRightHand( |
445 |
|
|
const double& transitionTime) { |
446 |
|
|
if (m_rightHandState == TASK_RIGHT_HAND_ON) { |
447 |
|
|
SEND_MSG( |
448 |
|
|
"Removes right hand SE3 task in " + toString(transitionTime) + " s", |
449 |
|
|
MSG_TYPE_INFO); |
450 |
|
|
m_invDyn->removeTask(m_taskRH->name(), transitionTime); |
451 |
|
|
m_rightHandState = TASK_RIGHT_HAND_OFF; |
452 |
|
|
} |
453 |
|
|
} |
454 |
|
|
|
455 |
|
|
void InverseDynamicsBalanceController::removeTaskLeftHand( |
456 |
|
|
const double& transitionTime) { |
457 |
|
|
if (m_leftHandState == TASK_LEFT_HAND_ON) { |
458 |
|
|
SEND_MSG("Removes left hand SE3 task in " + toString(transitionTime) + " s", |
459 |
|
|
MSG_TYPE_INFO); |
460 |
|
|
m_invDyn->removeTask(m_taskLH->name(), transitionTime); |
461 |
|
|
m_leftHandState = TASK_LEFT_HAND_OFF; |
462 |
|
|
} |
463 |
|
|
} |
464 |
|
|
|
465 |
|
|
void InverseDynamicsBalanceController::init(const double& dt, |
466 |
|
|
const std::string& robotRef) { |
467 |
|
|
if (dt <= 0.0) |
468 |
|
|
return SEND_MSG("Init failed: Timestep must be positive", MSG_TYPE_ERROR); |
469 |
|
|
|
470 |
|
|
/* Retrieve m_robot_util informations */ |
471 |
|
|
std::string localName(robotRef); |
472 |
|
|
if (isNameInRobotUtil(localName)) |
473 |
|
|
m_robot_util = getRobotUtil(localName); |
474 |
|
|
else { |
475 |
|
|
SEND_MSG("You should have an entity controller manager initialized before", |
476 |
|
|
MSG_TYPE_ERROR); |
477 |
|
|
return; |
478 |
|
|
} |
479 |
|
|
|
480 |
|
|
const Eigen::Matrix<double, 3, 4>& contactPoints = m_contact_pointsSIN(0); |
481 |
|
|
const Eigen::Vector3d& contactNormal = m_contact_normalSIN(0); |
482 |
|
|
// const Eigen::VectorXd w_forceReg = m_weight_contact_forcesSIN(0); |
483 |
|
|
const dg::sot::Vector6d& kp_contact = m_kp_constraintsSIN(0); |
484 |
|
|
const dg::sot::Vector6d& kd_contact = m_kd_constraintsSIN(0); |
485 |
|
|
const Eigen::Vector3d& kp_com = m_kp_comSIN(0); |
486 |
|
|
const Eigen::Vector3d& kd_com = m_kd_comSIN(0); |
487 |
|
|
const dg::sot::Vector6d& kd_hands = m_kd_handsSIN(0); |
488 |
|
|
const dg::sot::Vector6d& kp_hands = m_kp_handsSIN(0); |
489 |
|
|
const dg::sot::Vector6d& kp_feet = m_kp_feetSIN(0); |
490 |
|
|
const dg::sot::Vector6d& kd_feet = m_kd_feetSIN(0); |
491 |
|
|
const VectorN& kp_posture = m_kp_postureSIN(0); |
492 |
|
|
const VectorN& kd_posture = m_kd_postureSIN(0); |
493 |
|
|
const VectorN& rotor_inertias_sot = m_rotor_inertiasSIN(0); |
494 |
|
|
const VectorN& gear_ratios_sot = m_gear_ratiosSIN(0); |
495 |
|
|
|
496 |
|
|
assert(kp_posture.size() == |
497 |
|
|
static_cast<Eigen::VectorXd::Index>(m_robot_util->m_nbJoints)); |
498 |
|
|
assert(kd_posture.size() == |
499 |
|
|
static_cast<Eigen::VectorXd::Index>(m_robot_util->m_nbJoints)); |
500 |
|
|
assert(rotor_inertias_sot.size() == |
501 |
|
|
static_cast<Eigen::VectorXd::Index>(m_robot_util->m_nbJoints)); |
502 |
|
|
assert(gear_ratios_sot.size() == |
503 |
|
|
static_cast<Eigen::VectorXd::Index>(m_robot_util->m_nbJoints)); |
504 |
|
|
|
505 |
|
|
m_w_hands = m_w_handsSIN(0); |
506 |
|
|
m_w_com = m_w_comSIN(0); |
507 |
|
|
m_w_posture = m_w_postureSIN(0); |
508 |
|
|
const double& w_forces = m_w_forcesSIN(0); |
509 |
|
|
// const double & w_base_orientation = m_w_base_orientationSIN(0); |
510 |
|
|
// const double & w_torques = m_w_torquesSIN(0); |
511 |
|
|
const double& mu = m_muSIN(0); |
512 |
|
|
const double& fMin = m_f_minSIN(0); |
513 |
|
|
const double& fMaxRF = m_f_max_right_footSIN(0); |
514 |
|
|
const double& fMaxLF = m_f_max_left_footSIN(0); |
515 |
|
|
|
516 |
|
|
try { |
517 |
|
|
vector<string> package_dirs; |
518 |
|
|
m_robot = |
519 |
|
|
new robots::RobotWrapper(m_robot_util->m_urdf_filename, package_dirs, |
520 |
|
|
pinocchio::JointModelFreeFlyer()); |
521 |
|
|
|
522 |
|
|
assert(m_robot->nv() >= 6); |
523 |
|
|
m_robot_util->m_nbJoints = m_robot->nv() - 6; |
524 |
|
|
|
525 |
|
|
Vector rotor_inertias_urdf(rotor_inertias_sot.size()); |
526 |
|
|
Vector gear_ratios_urdf(gear_ratios_sot.size()); |
527 |
|
|
m_robot_util->joints_sot_to_urdf(rotor_inertias_sot, rotor_inertias_urdf); |
528 |
|
|
m_robot_util->joints_sot_to_urdf(gear_ratios_sot, gear_ratios_urdf); |
529 |
|
|
m_robot->rotor_inertias(rotor_inertias_urdf); |
530 |
|
|
m_robot->gear_ratios(gear_ratios_urdf); |
531 |
|
|
|
532 |
|
|
m_dv_sot.setZero(m_robot->nv()); |
533 |
|
|
m_tau_sot.setZero(m_robot->nv() - 6); |
534 |
|
|
m_f.setZero(24); |
535 |
|
|
m_q_urdf.setZero(m_robot->nq()); |
536 |
|
|
m_v_urdf.setZero(m_robot->nv()); |
537 |
|
|
m_J_RF.setZero(6, m_robot->nv()); |
538 |
|
|
m_J_LF.setZero(6, m_robot->nv()); |
539 |
|
|
|
540 |
|
|
m_invDyn = new InverseDynamicsFormulationAccForce("invdyn", *m_robot); |
541 |
|
|
|
542 |
|
|
m_contactRF = |
543 |
|
|
new Contact6d("contact_rfoot", *m_robot, |
544 |
|
|
m_robot_util->m_foot_util.m_Right_Foot_Frame_Name, |
545 |
|
|
contactPoints, contactNormal, mu, fMin, fMaxRF); |
546 |
|
|
m_contactRF->Kp(kp_contact); |
547 |
|
|
m_contactRF->Kd(kd_contact); |
548 |
|
|
m_invDyn->addRigidContact(*m_contactRF, w_forces); |
549 |
|
|
|
550 |
|
|
m_contactLF = |
551 |
|
|
new Contact6d("contact_lfoot", *m_robot, |
552 |
|
|
m_robot_util->m_foot_util.m_Left_Foot_Frame_Name, |
553 |
|
|
contactPoints, contactNormal, mu, fMin, fMaxLF); |
554 |
|
|
m_contactLF->Kp(kp_contact); |
555 |
|
|
m_contactLF->Kd(kd_contact); |
556 |
|
|
m_invDyn->addRigidContact(*m_contactLF, w_forces); |
557 |
|
|
|
558 |
|
|
if (m_f_ref_left_footSIN.isPlugged() && m_f_ref_right_footSIN.isPlugged()) { |
559 |
|
|
m_contactLF->setRegularizationTaskWeightVector(Vector6::Ones()); |
560 |
|
|
m_contactRF->setRegularizationTaskWeightVector(Vector6::Ones()); |
561 |
|
|
} |
562 |
|
|
|
563 |
|
|
m_taskCom = new TaskComEquality("task-com", *m_robot); |
564 |
|
|
m_taskCom->Kp(kp_com); |
565 |
|
|
m_taskCom->Kd(kd_com); |
566 |
|
|
m_invDyn->addMotionTask(*m_taskCom, m_w_com, 1); |
567 |
|
|
|
568 |
|
|
m_taskRF = new TaskSE3Equality( |
569 |
|
|
"task-rf", *m_robot, m_robot_util->m_foot_util.m_Right_Foot_Frame_Name); |
570 |
|
|
m_taskRF->Kp(kp_feet); |
571 |
|
|
m_taskRF->Kd(kd_feet); |
572 |
|
|
|
573 |
|
|
m_taskLF = new TaskSE3Equality( |
574 |
|
|
"task-lf", *m_robot, m_robot_util->m_foot_util.m_Left_Foot_Frame_Name); |
575 |
|
|
m_taskLF->Kp(kp_feet); |
576 |
|
|
m_taskLF->Kd(kd_feet); |
577 |
|
|
|
578 |
|
|
m_taskPosture = new TaskJointPosture("task-posture", *m_robot); |
579 |
|
|
m_taskPosture->Kp(kp_posture); |
580 |
|
|
m_taskPosture->Kd(kd_posture); |
581 |
|
|
m_invDyn->addMotionTask(*m_taskPosture, m_w_posture, 1); |
582 |
|
|
|
583 |
|
|
m_taskRH = new TaskSE3Equality( |
584 |
|
|
"task-rh", *m_robot, m_robot_util->m_hand_util.m_Right_Hand_Frame_Name); |
585 |
|
|
m_taskRH->Kp(kp_hands); |
586 |
|
|
m_taskRH->Kd(kd_hands); |
587 |
|
|
|
588 |
|
|
m_taskLH = new TaskSE3Equality( |
589 |
|
|
"task-lh", *m_robot, m_robot_util->m_hand_util.m_Left_Hand_Frame_Name); |
590 |
|
|
m_taskLH->Kp(kp_hands); |
591 |
|
|
m_taskLH->Kd(kd_hands); |
592 |
|
|
|
593 |
|
|
m_sampleCom = TrajectorySample(3); |
594 |
|
|
m_samplePosture = TrajectorySample(m_robot->nv() - 6); |
595 |
|
|
m_sampleRH = TrajectorySample(3); |
596 |
|
|
|
597 |
|
|
m_frame_id_rf = (int)m_robot->model().getFrameId( |
598 |
|
|
m_robot_util->m_foot_util.m_Right_Foot_Frame_Name); |
599 |
|
|
m_frame_id_lf = (int)m_robot->model().getFrameId( |
600 |
|
|
m_robot_util->m_foot_util.m_Left_Foot_Frame_Name); |
601 |
|
|
|
602 |
|
|
m_frame_id_rh = (int)m_robot->model().getFrameId( |
603 |
|
|
m_robot_util->m_hand_util.m_Right_Hand_Frame_Name); |
604 |
|
|
m_frame_id_lh = (int)m_robot->model().getFrameId( |
605 |
|
|
m_robot_util->m_hand_util.m_Left_Hand_Frame_Name); |
606 |
|
|
|
607 |
|
|
m_hqpSolver = SolverHQPFactory::createNewSolver(SOLVER_HQP_EIQUADPROG_FAST, |
608 |
|
|
"eiquadprog-fast"); |
609 |
|
|
m_hqpSolver->resize(m_invDyn->nVar(), m_invDyn->nEq(), m_invDyn->nIn()); |
610 |
|
|
m_hqpSolver_60_36_34 = SolverHQPFactory::createNewSolver<60, 36, 34>( |
611 |
|
|
SOLVER_HQP_EIQUADPROG_RT, "eiquadprog_rt_60_36_34"); |
612 |
|
|
m_hqpSolver_48_30_17 = SolverHQPFactory::createNewSolver<48, 30, 17>( |
613 |
|
|
SOLVER_HQP_EIQUADPROG_RT, "eiquadprog_rt_48_30_17"); |
614 |
|
|
} catch (const std::exception& e) { |
615 |
|
|
std::cout << e.what(); |
616 |
|
|
return SEND_MSG( |
617 |
|
|
"Init failed: Could load URDF :" + m_robot_util->m_urdf_filename, |
618 |
|
|
MSG_TYPE_ERROR); |
619 |
|
|
} |
620 |
|
|
m_dt = dt; |
621 |
|
|
m_initSucceeded = true; |
622 |
|
|
} |
623 |
|
|
|
624 |
|
|
/* ------------------------------------------------------------------- */ |
625 |
|
|
/* --- SIGNALS ------------------------------------------------------- */ |
626 |
|
|
/* ------------------------------------------------------------------- */ |
627 |
|
|
/** Copy active_joints only if a valid transition occurs. (From all OFF) or (To |
628 |
|
|
* all OFF)**/ |
629 |
|
|
DEFINE_SIGNAL_INNER_FUNCTION(active_joints_checked, dynamicgraph::Vector) { |
630 |
|
|
if (s.size() != static_cast<Eigen::VectorXd::Index>(m_robot_util->m_nbJoints)) |
631 |
|
|
s.resize(m_robot_util->m_nbJoints); |
632 |
|
|
|
633 |
|
|
const Eigen::VectorXd& active_joints_sot = m_active_jointsSIN(iter); |
634 |
|
|
if (m_enabled == false) { |
635 |
|
|
if (active_joints_sot.any()) { |
636 |
|
|
/* from all OFF to some ON */ |
637 |
|
|
m_enabled = true; |
638 |
|
|
|
639 |
|
|
s = active_joints_sot; |
640 |
|
|
Eigen::VectorXd active_joints_urdf(m_robot_util->m_nbJoints); |
641 |
|
|
m_robot_util->joints_sot_to_urdf(active_joints_sot, active_joints_urdf); |
642 |
|
|
// joints_sot_to_urdf(active_joints_sot, active_joints_urdf); |
643 |
|
|
|
644 |
|
|
m_taskBlockedJoints = new TaskJointPosture("task-posture", *m_robot); |
645 |
|
|
Eigen::VectorXd blocked_joints(m_robot_util->m_nbJoints); |
646 |
|
|
for (unsigned int i = 0; i < m_robot_util->m_nbJoints; i++) |
647 |
|
|
if (active_joints_urdf(i) == 0.0) |
648 |
|
|
blocked_joints(i) = 1.0; |
649 |
|
|
else |
650 |
|
|
blocked_joints(i) = 0.0; |
651 |
|
|
SEND_MSG("Blocked joints: " + toString(blocked_joints.transpose()), |
652 |
|
|
MSG_TYPE_INFO); |
653 |
|
|
m_taskBlockedJoints->mask(blocked_joints); |
654 |
|
|
TrajectorySample ref_zero( |
655 |
|
|
static_cast<unsigned int>(m_robot_util->m_nbJoints)); |
656 |
|
|
m_taskBlockedJoints->setReference(ref_zero); |
657 |
|
|
m_invDyn->addMotionTask(*m_taskBlockedJoints, 1.0, 0); |
658 |
|
|
} |
659 |
|
|
} else if (!active_joints_sot.any()) { |
660 |
|
|
/* from some ON to all OFF */ |
661 |
|
|
m_enabled = false; |
662 |
|
|
} |
663 |
|
|
if (m_enabled == false) |
664 |
|
|
for (unsigned int i = 0; i < m_robot_util->m_nbJoints; i++) s(i) = false; |
665 |
|
|
return s; |
666 |
|
|
} |
667 |
|
|
|
668 |
|
|
DEFINE_SIGNAL_OUT_FUNCTION(tau_des, dynamicgraph::Vector) { |
669 |
|
|
if (!m_initSucceeded) { |
670 |
|
|
SEND_WARNING_STREAM_MSG( |
671 |
|
|
"Cannot compute signal tau_des before initialization!"); |
672 |
|
|
return s; |
673 |
|
|
} |
674 |
|
|
if (s.size() != static_cast<Eigen::VectorXd::Index>(m_robot_util->m_nbJoints)) |
675 |
|
|
s.resize(m_robot_util->m_nbJoints); |
676 |
|
|
|
677 |
|
|
getProfiler().start(PROFILE_TAU_DES_COMPUTATION); |
678 |
|
|
|
679 |
|
|
// use reference contact wrenches (if plugged) to determine contact phase |
680 |
|
|
if (m_f_ref_left_footSIN.isPlugged() && m_f_ref_right_footSIN.isPlugged()) { |
681 |
|
|
const Vector6& f_ref_left_foot = m_f_ref_left_footSIN(iter); |
682 |
|
|
const Vector6& f_ref_right_foot = m_f_ref_right_footSIN(iter); |
683 |
|
|
m_contactLF->setForceReference(f_ref_left_foot); |
684 |
|
|
m_contactRF->setForceReference(f_ref_right_foot); |
685 |
|
|
|
686 |
|
|
if (m_contactState == DOUBLE_SUPPORT) { |
687 |
|
|
if (f_ref_left_foot.norm() < ZERO_FORCE_THRESHOLD) { |
688 |
|
|
removeLeftFootContact(0.0); |
689 |
|
|
} else if (f_ref_right_foot.norm() < ZERO_FORCE_THRESHOLD) { |
690 |
|
|
removeRightFootContact(0.0); |
691 |
|
|
} |
692 |
|
|
} else if (m_contactState == LEFT_SUPPORT && |
693 |
|
|
f_ref_right_foot.norm() > ZERO_FORCE_THRESHOLD) { |
694 |
|
|
addRightFootContact(0.0); |
695 |
|
|
} else if (m_contactState == RIGHT_SUPPORT && |
696 |
|
|
f_ref_left_foot.norm() > ZERO_FORCE_THRESHOLD) { |
697 |
|
|
addLeftFootContact(0.0); |
698 |
|
|
} |
699 |
|
|
} |
700 |
|
|
|
701 |
|
|
if (m_contactState == RIGHT_SUPPORT_TRANSITION && |
702 |
|
|
m_t >= m_contactTransitionTime) { |
703 |
|
|
m_contactState = RIGHT_SUPPORT; |
704 |
|
|
} else if (m_contactState == LEFT_SUPPORT_TRANSITION && |
705 |
|
|
m_t >= m_contactTransitionTime) { |
706 |
|
|
m_contactState = LEFT_SUPPORT; |
707 |
|
|
} |
708 |
|
|
/*if(m_rightHandState == TASK_RIGHT_HAND_TRANSITION && m_t >= |
709 |
|
|
m_handsTransitionTime) |
710 |
|
|
{ |
711 |
|
|
m_rightHandState = TASK_RIGHT_HAND_ON; |
712 |
|
|
} |
713 |
|
|
if(m_leftHandState == TASK_LEFT_HAND_TRANSITION && m_t >= |
714 |
|
|
m_handsTransitionTime) |
715 |
|
|
{ |
716 |
|
|
m_leftHandState = TASK_LEFT_HAND_ON; |
717 |
|
|
}*/ |
718 |
|
|
|
719 |
|
|
getProfiler().start(PROFILE_READ_INPUT_SIGNALS); |
720 |
|
|
m_w_feetSIN(iter); |
721 |
|
|
m_active_joints_checkedSINNER(iter); |
722 |
|
|
const VectorN6& q_sot = m_qSIN(iter); |
723 |
|
|
assert(q_sot.size() == |
724 |
|
|
static_cast<Eigen::VectorXd::Index>(m_robot_util->m_nbJoints + 6)); |
725 |
|
|
const VectorN6& v_sot = m_vSIN(iter); |
726 |
|
|
assert(v_sot.size() == |
727 |
|
|
static_cast<Eigen::VectorXd::Index>(m_robot_util->m_nbJoints + 6)); |
728 |
|
|
const Vector3& x_com_ref = m_com_ref_posSIN(iter); |
729 |
|
|
const Vector3& dx_com_ref = m_com_ref_velSIN(iter); |
730 |
|
|
const Vector3& ddx_com_ref = m_com_ref_accSIN(iter); |
731 |
|
|
const VectorN& q_ref = m_posture_ref_posSIN(iter); |
732 |
|
|
assert(q_ref.size() == |
733 |
|
|
static_cast<Eigen::VectorXd::Index>(m_robot_util->m_nbJoints)); |
734 |
|
|
const VectorN& dq_ref = m_posture_ref_velSIN(iter); |
735 |
|
|
assert(dq_ref.size() == |
736 |
|
|
static_cast<Eigen::VectorXd::Index>(m_robot_util->m_nbJoints)); |
737 |
|
|
const VectorN& ddq_ref = m_posture_ref_accSIN(iter); |
738 |
|
|
assert(ddq_ref.size() == |
739 |
|
|
static_cast<Eigen::VectorXd::Index>(m_robot_util->m_nbJoints)); |
740 |
|
|
|
741 |
|
|
const Vector6& kp_contact = m_kp_constraintsSIN(iter); |
742 |
|
|
const Vector6& kd_contact = m_kd_constraintsSIN(iter); |
743 |
|
|
const Vector3& kp_com = m_kp_comSIN(iter); |
744 |
|
|
const Vector3& kd_com = m_kd_comSIN(iter); |
745 |
|
|
|
746 |
|
|
const VectorN& kp_posture = m_kp_postureSIN(iter); |
747 |
|
|
assert(kp_posture.size() == |
748 |
|
|
static_cast<Eigen::VectorXd::Index>(m_robot_util->m_nbJoints)); |
749 |
|
|
const VectorN& kd_posture = m_kd_postureSIN(iter); |
750 |
|
|
assert(kd_posture.size() == |
751 |
|
|
static_cast<Eigen::VectorXd::Index>(m_robot_util->m_nbJoints)); |
752 |
|
|
const VectorN& kp_pos = m_kp_posSIN(iter); |
753 |
|
|
assert(kp_pos.size() == |
754 |
|
|
static_cast<Eigen::VectorXd::Index>(m_robot_util->m_nbJoints)); |
755 |
|
|
const VectorN& kd_pos = m_kd_posSIN(iter); |
756 |
|
|
assert(kd_pos.size() == |
757 |
|
|
static_cast<Eigen::VectorXd::Index>(m_robot_util->m_nbJoints)); |
758 |
|
|
|
759 |
|
|
/*const double & w_hands = m_w_handsSIN(iter);*/ |
760 |
|
|
const double& w_com = m_w_comSIN(iter); |
761 |
|
|
const double& w_posture = m_w_postureSIN(iter); |
762 |
|
|
const double& w_forces = m_w_forcesSIN(iter); |
763 |
|
|
|
764 |
|
|
if (m_contactState == LEFT_SUPPORT || |
765 |
|
|
m_contactState == LEFT_SUPPORT_TRANSITION) { |
766 |
|
|
const Eigen::Matrix<double, 12, 1>& x_rf_ref = m_rf_ref_posSIN(iter); |
767 |
|
|
const Vector6& dx_rf_ref = m_rf_ref_velSIN(iter); |
768 |
|
|
const Vector6& ddx_rf_ref = m_rf_ref_accSIN(iter); |
769 |
|
|
const Vector6& kp_feet = m_kp_feetSIN(iter); |
770 |
|
|
const Vector6& kd_feet = m_kd_feetSIN(iter); |
771 |
|
|
m_sampleRF.pos = x_rf_ref; |
772 |
|
|
m_sampleRF.vel = dx_rf_ref; |
773 |
|
|
m_sampleRF.acc = ddx_rf_ref; |
774 |
|
|
m_taskRF->setReference(m_sampleRF); |
775 |
|
|
m_taskRF->Kp(kp_feet); |
776 |
|
|
m_taskRF->Kd(kd_feet); |
777 |
|
|
} else if (m_contactState == RIGHT_SUPPORT || |
778 |
|
|
m_contactState == RIGHT_SUPPORT_TRANSITION) { |
779 |
|
|
const Eigen::Matrix<double, 12, 1>& x_lf_ref = m_lf_ref_posSIN(iter); |
780 |
|
|
const Vector6& dx_lf_ref = m_lf_ref_velSIN(iter); |
781 |
|
|
const Vector6& ddx_lf_ref = m_lf_ref_accSIN(iter); |
782 |
|
|
const Vector6& kp_feet = m_kp_feetSIN(iter); |
783 |
|
|
const Vector6& kd_feet = m_kd_feetSIN(iter); |
784 |
|
|
m_sampleLF.pos = x_lf_ref; |
785 |
|
|
m_sampleLF.vel = dx_lf_ref; |
786 |
|
|
m_sampleLF.acc = ddx_lf_ref; |
787 |
|
|
m_taskLF->setReference(m_sampleLF); |
788 |
|
|
m_taskLF->Kp(kp_feet); |
789 |
|
|
m_taskLF->Kd(kd_feet); |
790 |
|
|
} |
791 |
|
|
if (m_rightHandState == TASK_RIGHT_HAND_ON /*|| m_rightHandState == TASK_RIGHT_HAND_TRANSITION*/) { |
792 |
|
|
const Eigen::Matrix<double, 12, 1>& x_rh_ref = m_rh_ref_posSIN(iter); |
793 |
|
|
const Vector6& dx_rh_ref = m_rh_ref_velSIN(iter); |
794 |
|
|
const Vector6& ddx_rh_ref = m_rh_ref_accSIN(iter); |
795 |
|
|
const Vector6& kp_hands = m_kp_handsSIN(iter); |
796 |
|
|
const Vector6& kd_hands = m_kd_handsSIN(iter); |
797 |
|
|
m_sampleRH.pos = x_rh_ref; |
798 |
|
|
m_sampleRH.vel = dx_rh_ref; |
799 |
|
|
m_sampleRH.acc = ddx_rh_ref; |
800 |
|
|
m_taskRH->setReference(m_sampleRH); |
801 |
|
|
m_taskRH->Kp(kp_hands); |
802 |
|
|
m_taskRH->Kd(kd_hands); |
803 |
|
|
} |
804 |
|
|
if (m_leftHandState == |
805 |
|
|
TASK_LEFT_HAND_ON /*|| m_leftHandState == TASK_LEFT_HAND_TRANSITION*/) { |
806 |
|
|
const Eigen::Matrix<double, 12, 1>& x_lh_ref = m_lh_ref_posSIN(iter); |
807 |
|
|
const Vector6& dx_lh_ref = m_lh_ref_velSIN(iter); |
808 |
|
|
const Vector6& ddx_lh_ref = m_lh_ref_accSIN(iter); |
809 |
|
|
const Vector6& kp_hands = m_kp_handsSIN(iter); |
810 |
|
|
const Vector6& kd_hands = m_kd_handsSIN(iter); |
811 |
|
|
m_sampleLH.pos = x_lh_ref; |
812 |
|
|
m_sampleLH.vel = dx_lh_ref; |
813 |
|
|
m_sampleLH.acc = ddx_lh_ref; |
814 |
|
|
m_taskLH->setReference(m_sampleLH); |
815 |
|
|
m_taskLH->Kp(kp_hands); |
816 |
|
|
m_taskLH->Kd(kd_hands); |
817 |
|
|
} |
818 |
|
|
|
819 |
|
|
getProfiler().stop(PROFILE_READ_INPUT_SIGNALS); |
820 |
|
|
|
821 |
|
|
getProfiler().start(PROFILE_PREPARE_INV_DYN); |
822 |
|
|
m_robot_util->config_sot_to_urdf(q_sot, m_q_urdf); |
823 |
|
|
m_robot_util->velocity_sot_to_urdf(m_q_urdf, v_sot, m_v_urdf); |
824 |
|
|
|
825 |
|
|
m_sampleCom.pos = x_com_ref - m_com_offset; |
826 |
|
|
m_sampleCom.vel = dx_com_ref; |
827 |
|
|
m_sampleCom.acc = ddx_com_ref; |
828 |
|
|
m_taskCom->setReference(m_sampleCom); |
829 |
|
|
m_taskCom->Kp(kp_com); |
830 |
|
|
m_taskCom->Kd(kd_com); |
831 |
|
|
if (m_w_com != w_com) { |
832 |
|
|
// SEND_MSG("Change w_com from "+toString(m_w_com)+" to |
833 |
|
|
// "+toString(w_com), MSG_TYPE_INFO); |
834 |
|
|
m_w_com = w_com; |
835 |
|
|
m_invDyn->updateTaskWeight(m_taskCom->name(), w_com); |
836 |
|
|
} |
837 |
|
|
|
838 |
|
|
m_robot_util->joints_sot_to_urdf(q_ref, m_samplePosture.pos); |
839 |
|
|
m_robot_util->joints_sot_to_urdf(dq_ref, m_samplePosture.vel); |
840 |
|
|
m_robot_util->joints_sot_to_urdf(ddq_ref, m_samplePosture.acc); |
841 |
|
|
m_taskPosture->setReference(m_samplePosture); |
842 |
|
|
m_taskPosture->Kp(kp_posture); |
843 |
|
|
m_taskPosture->Kd(kd_posture); |
844 |
|
|
if (m_w_posture != w_posture) { |
845 |
|
|
// SEND_MSG("Change posture from "+toString(m_w_posture)+" to |
846 |
|
|
// "+toString(w_posture), MSG_TYPE_INFO); |
847 |
|
|
m_w_posture = w_posture; |
848 |
|
|
m_invDyn->updateTaskWeight(m_taskPosture->name(), w_posture); |
849 |
|
|
} |
850 |
|
|
|
851 |
|
|
/*m_sampleRH.pos = x_rh_ref; |
852 |
|
|
m_sampleRH.vel = dx_rh_ref; |
853 |
|
|
m_sampleRH.acc = ddx_rh_ref; |
854 |
|
|
m_taskRH->setReference(m_sampleRH); |
855 |
|
|
m_taskRH->Kp(kp_hands); |
856 |
|
|
m_taskRH->Kd(kd_hands);*/ |
857 |
|
|
|
858 |
|
|
const double& fMin = m_f_minSIN(0); |
859 |
|
|
const double& fMaxRF = m_f_max_right_footSIN(iter); |
860 |
|
|
const double& fMaxLF = m_f_max_left_footSIN(iter); |
861 |
|
|
m_contactLF->setMinNormalForce(fMin); |
862 |
|
|
m_contactRF->setMinNormalForce(fMin); |
863 |
|
|
m_contactLF->setMaxNormalForce(fMaxLF); |
864 |
|
|
m_contactRF->setMaxNormalForce(fMaxRF); |
865 |
|
|
m_contactLF->Kp(kp_contact); |
866 |
|
|
m_contactLF->Kd(kd_contact); |
867 |
|
|
m_contactRF->Kp(kp_contact); |
868 |
|
|
m_contactRF->Kd(kd_contact); |
869 |
|
|
m_invDyn->updateRigidContactWeights(m_contactLF->name(), w_forces); |
870 |
|
|
m_invDyn->updateRigidContactWeights(m_contactRF->name(), w_forces); |
871 |
|
|
|
872 |
|
|
if (m_firstTime) { |
873 |
|
|
m_firstTime = false; |
874 |
|
|
m_invDyn->computeProblemData(m_t, m_q_urdf, m_v_urdf); |
875 |
|
|
// m_robot->computeAllTerms(m_invDyn->data(), q, v); |
876 |
|
|
pinocchio::SE3 H_lf = m_robot->position( |
877 |
|
|
m_invDyn->data(), |
878 |
|
|
m_robot->model().getJointId( |
879 |
|
|
m_robot_util->m_foot_util.m_Left_Foot_Frame_Name)); |
880 |
|
|
m_contactLF->setReference(H_lf); |
881 |
|
|
SEND_MSG("Setting left foot reference to " + toString(H_lf), |
882 |
|
|
MSG_TYPE_DEBUG); |
883 |
|
|
|
884 |
|
|
pinocchio::SE3 H_rf = m_robot->position( |
885 |
|
|
m_invDyn->data(), |
886 |
|
|
m_robot->model().getJointId( |
887 |
|
|
m_robot_util->m_foot_util.m_Right_Foot_Frame_Name)); |
888 |
|
|
m_contactRF->setReference(H_rf); |
889 |
|
|
SEND_MSG("Setting right foot reference to " + toString(H_rf), |
890 |
|
|
MSG_TYPE_DEBUG); |
891 |
|
|
} else if (m_timeLast != static_cast<unsigned int>(iter - 1)) { |
892 |
|
|
SEND_MSG("Last time " + toString(m_timeLast) + |
893 |
|
|
" is not current time-1: " + toString(iter), |
894 |
|
|
MSG_TYPE_ERROR); |
895 |
|
|
if (m_timeLast == static_cast<unsigned int>(iter)) { |
896 |
|
|
s = m_tau_sot; |
897 |
|
|
return s; |
898 |
|
|
} |
899 |
|
|
} |
900 |
|
|
m_timeLast = static_cast<unsigned int>(iter); |
901 |
|
|
|
902 |
|
|
const HQPData& hqpData = |
903 |
|
|
m_invDyn->computeProblemData(m_t, m_q_urdf, m_v_urdf); |
904 |
|
|
|
905 |
|
|
getProfiler().stop(PROFILE_PREPARE_INV_DYN); |
906 |
|
|
|
907 |
|
|
getProfiler().start(PROFILE_HQP_SOLUTION); |
908 |
|
|
SolverHQPBase* solver = m_hqpSolver; |
909 |
|
|
if (m_invDyn->nVar() == 60 && m_invDyn->nEq() == 36 && |
910 |
|
|
m_invDyn->nIn() == 34) { |
911 |
|
|
solver = m_hqpSolver_60_36_34; |
912 |
|
|
getStatistics().store("solver fixed size 60_36_34", 1.0); |
913 |
|
|
} else if (m_invDyn->nVar() == 48 && m_invDyn->nEq() == 30 && |
914 |
|
|
m_invDyn->nIn() == 17) { |
915 |
|
|
solver = m_hqpSolver_48_30_17; |
916 |
|
|
getStatistics().store("solver fixed size 48_30_17", 1.0); |
917 |
|
|
} else |
918 |
|
|
getStatistics().store("solver dynamic size", 1.0); |
919 |
|
|
|
920 |
|
|
const HQPOutput& sol = solver->solve(hqpData); |
921 |
|
|
getProfiler().stop(PROFILE_HQP_SOLUTION); |
922 |
|
|
|
923 |
|
|
if (sol.status != HQP_STATUS_OPTIMAL) { |
924 |
|
|
SEND_ERROR_STREAM_MSG("HQP solver failed to find a solution: " + |
925 |
|
|
toString(sol.status)); |
926 |
|
|
SEND_DEBUG_STREAM_MSG(tsid::solvers::HQPDataToString(hqpData, false)); |
927 |
|
|
SEND_DEBUG_STREAM_MSG("q=" + toString(q_sot.transpose(), 1, 5)); |
928 |
|
|
SEND_DEBUG_STREAM_MSG("v=" + toString(v_sot.transpose(), 1, 5)); |
929 |
|
|
s.setZero(); |
930 |
|
|
return s; |
931 |
|
|
} |
932 |
|
|
|
933 |
|
|
getStatistics().store("active inequalities", |
934 |
|
|
static_cast<double>(sol.activeSet.size())); |
935 |
|
|
getStatistics().store("solver iterations", sol.iterations); |
936 |
|
|
if (ddx_com_ref.norm() > 1e-3) |
937 |
|
|
getStatistics().store( |
938 |
|
|
"com ff ratio", |
939 |
|
|
ddx_com_ref.norm() / m_taskCom->getConstraint().vector().norm()); |
940 |
|
|
|
941 |
|
|
m_dv_urdf = m_invDyn->getAccelerations(sol); |
942 |
|
|
m_robot_util->velocity_urdf_to_sot(m_q_urdf, m_dv_urdf, m_dv_sot); |
943 |
|
|
Eigen::Matrix<double, 12, 1> tmp; |
944 |
|
|
if (m_invDyn->getContactForces(m_contactRF->name(), sol, tmp)) |
945 |
|
|
m_f_RF = m_contactRF->getForceGeneratorMatrix() * tmp; |
946 |
|
|
if (m_invDyn->getContactForces(m_contactLF->name(), sol, tmp)) |
947 |
|
|
m_f_LF = m_contactLF->getForceGeneratorMatrix() * tmp; |
948 |
|
|
m_robot_util->joints_urdf_to_sot(m_invDyn->getActuatorForces(sol), m_tau_sot); |
949 |
|
|
|
950 |
|
|
m_tau_sot += |
951 |
|
|
kp_pos.cwiseProduct(q_ref - q_sot.tail(m_robot_util->m_nbJoints)) + |
952 |
|
|
kd_pos.cwiseProduct(dq_ref - v_sot.tail(m_robot_util->m_nbJoints)); |
953 |
|
|
|
954 |
|
|
getProfiler().stop(PROFILE_TAU_DES_COMPUTATION); |
955 |
|
|
m_t += m_dt; |
956 |
|
|
|
957 |
|
|
s = m_tau_sot; |
958 |
|
|
|
959 |
|
|
return s; |
960 |
|
|
} |
961 |
|
|
|
962 |
|
|
DEFINE_SIGNAL_OUT_FUNCTION(M, dynamicgraph::Matrix) { |
963 |
|
|
if (!m_initSucceeded) { |
964 |
|
|
SEND_WARNING_STREAM_MSG("Cannot compute signal M before initialization!"); |
965 |
|
|
return s; |
966 |
|
|
} |
967 |
|
|
if (s.cols() != m_robot->nv() || s.rows() != m_robot->nv()) |
968 |
|
|
s.resize(m_robot->nv(), m_robot->nv()); |
969 |
|
|
m_tau_desSOUT(iter); |
970 |
|
|
s = m_robot->mass(m_invDyn->data()); |
971 |
|
|
return s; |
972 |
|
|
} |
973 |
|
|
|
974 |
|
|
DEFINE_SIGNAL_OUT_FUNCTION(dv_des, dynamicgraph::Vector) { |
975 |
|
|
if (!m_initSucceeded) { |
976 |
|
|
SEND_WARNING_STREAM_MSG( |
977 |
|
|
"Cannot compute signal dv_des before initialization!"); |
978 |
|
|
return s; |
979 |
|
|
} |
980 |
|
|
if (s.size() != m_robot->nv()) s.resize(m_robot->nv()); |
981 |
|
|
m_tau_desSOUT(iter); |
982 |
|
|
s = m_dv_sot; |
983 |
|
|
return s; |
984 |
|
|
} |
985 |
|
|
|
986 |
|
|
DEFINE_SIGNAL_OUT_FUNCTION(f_des_right_foot, dynamicgraph::Vector) { |
987 |
|
|
if (!m_initSucceeded) { |
988 |
|
|
SEND_WARNING_STREAM_MSG( |
989 |
|
|
"Cannot compute signal f_des_right_foot before initialization!"); |
990 |
|
|
return s; |
991 |
|
|
} |
992 |
|
|
if (s.size() != 6) s.resize(6); |
993 |
|
|
m_tau_desSOUT(iter); |
994 |
|
|
if (m_contactState == LEFT_SUPPORT) { |
995 |
|
|
s.setZero(); |
996 |
|
|
return s; |
997 |
|
|
} |
998 |
|
|
s = m_f_RF; |
999 |
|
|
return s; |
1000 |
|
|
} |
1001 |
|
|
|
1002 |
|
|
DEFINE_SIGNAL_OUT_FUNCTION(f_des_left_foot, dynamicgraph::Vector) { |
1003 |
|
|
if (!m_initSucceeded) { |
1004 |
|
|
SEND_WARNING_STREAM_MSG( |
1005 |
|
|
"Cannot compute signal f_des_left_foot before initialization!"); |
1006 |
|
|
return s; |
1007 |
|
|
} |
1008 |
|
|
if (s.size() != 6) s.resize(6); |
1009 |
|
|
m_tau_desSOUT(iter); |
1010 |
|
|
if (m_contactState == RIGHT_SUPPORT) { |
1011 |
|
|
s.setZero(); |
1012 |
|
|
return s; |
1013 |
|
|
} |
1014 |
|
|
s = m_f_LF; |
1015 |
|
|
return s; |
1016 |
|
|
} |
1017 |
|
|
|
1018 |
|
|
DEFINE_SIGNAL_OUT_FUNCTION(com_acc_des, dynamicgraph::Vector) { |
1019 |
|
|
if (!m_initSucceeded) { |
1020 |
|
|
SEND_WARNING_STREAM_MSG( |
1021 |
|
|
"Cannot compute signal com_acc_des before initialization!"); |
1022 |
|
|
return s; |
1023 |
|
|
} |
1024 |
|
|
if (s.size() != 3) s.resize(3); |
1025 |
|
|
m_tau_desSOUT(iter); |
1026 |
|
|
s = m_taskCom->getDesiredAcceleration(); |
1027 |
|
|
return s; |
1028 |
|
|
} |
1029 |
|
|
|
1030 |
|
|
DEFINE_SIGNAL_OUT_FUNCTION(com_acc, dynamicgraph::Vector) { |
1031 |
|
|
if (!m_initSucceeded) { |
1032 |
|
|
SEND_WARNING_STREAM_MSG( |
1033 |
|
|
"Cannot compute signal com_acc before initialization!"); |
1034 |
|
|
return s; |
1035 |
|
|
} |
1036 |
|
|
if (s.size() != 3) s.resize(3); |
1037 |
|
|
m_tau_desSOUT(iter); |
1038 |
|
|
s = m_taskCom->getAcceleration(m_dv_urdf); |
1039 |
|
|
return s; |
1040 |
|
|
} |
1041 |
|
|
|
1042 |
|
|
DEFINE_SIGNAL_OUT_FUNCTION(zmp_des_right_foot_local, dynamicgraph::Vector) { |
1043 |
|
|
if (!m_initSucceeded) { |
1044 |
|
|
SEND_WARNING_STREAM_MSG( |
1045 |
|
|
"Cannot compute signal zmp_des_right_foot_local before " |
1046 |
|
|
"initialization!"); |
1047 |
|
|
return s; |
1048 |
|
|
} |
1049 |
|
|
if (s.size() != 2) s.resize(2); |
1050 |
|
|
|
1051 |
|
|
m_f_des_right_footSOUT(iter); |
1052 |
|
|
if (fabs(m_f_RF(2) > 1.0)) { |
1053 |
|
|
m_zmp_des_RF_local(0) = -m_f_RF(4) / m_f_RF(2); |
1054 |
|
|
m_zmp_des_RF_local(1) = m_f_RF(3) / m_f_RF(2); |
1055 |
|
|
m_zmp_des_RF_local(2) = 0.0; |
1056 |
|
|
} else |
1057 |
|
|
m_zmp_des_RF_local.setZero(); |
1058 |
|
|
|
1059 |
|
|
s = m_zmp_des_RF_local.head<2>(); |
1060 |
|
|
return s; |
1061 |
|
|
} |
1062 |
|
|
|
1063 |
|
|
DEFINE_SIGNAL_OUT_FUNCTION(zmp_des_left_foot_local, dynamicgraph::Vector) { |
1064 |
|
|
if (!m_initSucceeded) { |
1065 |
|
|
SEND_WARNING_STREAM_MSG( |
1066 |
|
|
"Cannot compute signal zmp_des_left_foot_local before initialization!"); |
1067 |
|
|
return s; |
1068 |
|
|
} |
1069 |
|
|
if (s.size() != 2) s.resize(2); |
1070 |
|
|
m_f_des_left_footSOUT(iter); |
1071 |
|
|
if (fabs(m_f_LF(2) > 1.0)) { |
1072 |
|
|
m_zmp_des_LF_local(0) = -m_f_LF(4) / m_f_LF(2); |
1073 |
|
|
m_zmp_des_LF_local(1) = m_f_LF(3) / m_f_LF(2); |
1074 |
|
|
m_zmp_des_LF_local(2) = 0.0; |
1075 |
|
|
} else |
1076 |
|
|
m_zmp_des_LF_local.setZero(); |
1077 |
|
|
|
1078 |
|
|
s = m_zmp_des_LF_local.head<2>(); |
1079 |
|
|
return s; |
1080 |
|
|
} |
1081 |
|
|
|
1082 |
|
|
DEFINE_SIGNAL_OUT_FUNCTION(zmp_des_right_foot, dynamicgraph::Vector) { |
1083 |
|
|
if (!m_initSucceeded) { |
1084 |
|
|
SEND_WARNING_STREAM_MSG( |
1085 |
|
|
"Cannot compute signal zmp_des_right_foot before initialization!"); |
1086 |
|
|
return s; |
1087 |
|
|
} |
1088 |
|
|
if (s.size() != 2) s.resize(2); |
1089 |
|
|
m_f_des_right_footSOUT(iter); |
1090 |
|
|
pinocchio::SE3 H_rf = m_robot->position( |
1091 |
|
|
m_invDyn->data(), m_robot->model().getJointId( |
1092 |
|
|
m_robot_util->m_foot_util.m_Right_Foot_Frame_Name)); |
1093 |
|
|
if (fabs(m_f_RF(2) > 1.0)) { |
1094 |
|
|
m_zmp_des_RF(0) = -m_f_RF(4) / m_f_RF(2); |
1095 |
|
|
m_zmp_des_RF(1) = m_f_RF(3) / m_f_RF(2); |
1096 |
|
|
m_zmp_des_RF(2) = -H_rf.translation()(2); |
1097 |
|
|
} else |
1098 |
|
|
m_zmp_des_RF.setZero(); |
1099 |
|
|
|
1100 |
|
|
m_zmp_des_RF = H_rf.act(m_zmp_des_RF); |
1101 |
|
|
s = m_zmp_des_RF.head<2>(); |
1102 |
|
|
return s; |
1103 |
|
|
} |
1104 |
|
|
|
1105 |
|
|
DEFINE_SIGNAL_OUT_FUNCTION(zmp_des_left_foot, dynamicgraph::Vector) { |
1106 |
|
|
if (!m_initSucceeded) { |
1107 |
|
|
SEND_WARNING_STREAM_MSG( |
1108 |
|
|
"Cannot compute signal zmp_des_left_foot before initialization!"); |
1109 |
|
|
return s; |
1110 |
|
|
} |
1111 |
|
|
if (s.size() != 2) s.resize(2); |
1112 |
|
|
m_f_des_left_footSOUT(iter); |
1113 |
|
|
pinocchio::SE3 H_lf = m_robot->position( |
1114 |
|
|
m_invDyn->data(), m_robot->model().getJointId( |
1115 |
|
|
m_robot_util->m_foot_util.m_Left_Foot_Frame_Name)); |
1116 |
|
|
if (fabs(m_f_LF(2) > 1.0)) { |
1117 |
|
|
m_zmp_des_LF(0) = -m_f_LF(4) / m_f_LF(2); |
1118 |
|
|
m_zmp_des_LF(1) = m_f_LF(3) / m_f_LF(2); |
1119 |
|
|
m_zmp_des_LF(2) = -H_lf.translation()(2); |
1120 |
|
|
} else |
1121 |
|
|
m_zmp_des_LF.setZero(); |
1122 |
|
|
|
1123 |
|
|
m_zmp_des_LF = H_lf.act(m_zmp_des_LF); |
1124 |
|
|
s = m_zmp_des_LF.head<2>(); |
1125 |
|
|
return s; |
1126 |
|
|
} |
1127 |
|
|
|
1128 |
|
|
DEFINE_SIGNAL_OUT_FUNCTION(zmp_des, dynamicgraph::Vector) { |
1129 |
|
|
if (!m_initSucceeded) { |
1130 |
|
|
SEND_WARNING_STREAM_MSG( |
1131 |
|
|
"Cannot compute signal zmp_des before initialization!"); |
1132 |
|
|
return s; |
1133 |
|
|
} |
1134 |
|
|
if (s.size() != 2) s.resize(2); |
1135 |
|
|
m_zmp_des_left_footSOUT(iter); |
1136 |
|
|
m_zmp_des_right_footSOUT(iter); |
1137 |
|
|
|
1138 |
|
|
m_zmp_des = (m_f_RF(2) * m_zmp_des_RF + m_f_LF(2) * m_zmp_des_LF) / |
1139 |
|
|
(m_f_LF(2) + m_f_RF(2)); |
1140 |
|
|
s = m_zmp_des.head<2>(); |
1141 |
|
|
return s; |
1142 |
|
|
} |
1143 |
|
|
|
1144 |
|
|
DEFINE_SIGNAL_OUT_FUNCTION(zmp_ref, dynamicgraph::Vector) { |
1145 |
|
|
if (!m_initSucceeded) { |
1146 |
|
|
SEND_WARNING_STREAM_MSG( |
1147 |
|
|
"Cannot compute signal zmp_ref before initialization!"); |
1148 |
|
|
return s; |
1149 |
|
|
} |
1150 |
|
|
if (s.size() != 2) s.resize(2); |
1151 |
|
|
const Vector6& f_LF = m_f_ref_left_footSIN(iter); |
1152 |
|
|
const Vector6& f_RF = m_f_ref_right_footSIN(iter); |
1153 |
|
|
|
1154 |
|
|
pinocchio::SE3 H_lf = m_robot->position( |
1155 |
|
|
m_invDyn->data(), m_robot->model().getJointId( |
1156 |
|
|
m_robot_util->m_foot_util.m_Left_Foot_Frame_Name)); |
1157 |
|
|
Vector3 zmp_LF, zmp_RF; |
1158 |
|
|
if (fabs(f_LF(2) > 1.0)) { |
1159 |
|
|
zmp_LF(0) = -f_LF(4) / f_LF(2); |
1160 |
|
|
zmp_LF(1) = f_LF(3) / f_LF(2); |
1161 |
|
|
zmp_LF(2) = -H_lf.translation()(2); |
1162 |
|
|
} else |
1163 |
|
|
zmp_LF.setZero(); |
1164 |
|
|
zmp_LF = H_lf.act(zmp_LF); |
1165 |
|
|
|
1166 |
|
|
pinocchio::SE3 H_rf = m_robot->position( |
1167 |
|
|
m_invDyn->data(), m_robot->model().getJointId( |
1168 |
|
|
m_robot_util->m_foot_util.m_Right_Foot_Frame_Name)); |
1169 |
|
|
if (fabs(f_RF(2) > 1.0)) { |
1170 |
|
|
zmp_RF(0) = -f_RF(4) / f_RF(2); |
1171 |
|
|
zmp_RF(1) = f_RF(3) / f_RF(2); |
1172 |
|
|
zmp_RF(2) = -H_rf.translation()(2); |
1173 |
|
|
} else |
1174 |
|
|
zmp_RF.setZero(); |
1175 |
|
|
zmp_RF = H_rf.act(zmp_RF); |
1176 |
|
|
|
1177 |
|
|
if (f_LF(2) + f_RF(2) != 0.0) |
1178 |
|
|
s = (f_RF(2) * zmp_RF.head<2>() + f_LF(2) * zmp_LF.head<2>()) / |
1179 |
|
|
(f_LF(2) + f_RF(2)); |
1180 |
|
|
|
1181 |
|
|
return s; |
1182 |
|
|
} |
1183 |
|
|
|
1184 |
|
|
DEFINE_SIGNAL_OUT_FUNCTION(zmp_right_foot, dynamicgraph::Vector) { |
1185 |
|
|
if (!m_initSucceeded) { |
1186 |
|
|
SEND_WARNING_STREAM_MSG( |
1187 |
|
|
"Cannot compute signal zmp_right_foot before initialization!"); |
1188 |
|
|
return s; |
1189 |
|
|
} |
1190 |
|
|
if (s.size() != 2) s.resize(2); |
1191 |
|
|
const Vector6& f = m_wrench_right_footSIN(iter); |
1192 |
|
|
assert(f.size() == 6); |
1193 |
|
|
pinocchio::SE3 H_rf = m_robot->position( |
1194 |
|
|
m_invDyn->data(), m_robot->model().getJointId( |
1195 |
|
|
m_robot_util->m_foot_util.m_Right_Foot_Frame_Name)); |
1196 |
|
|
if (fabs(f(2) > 1.0)) { |
1197 |
|
|
m_zmp_RF(0) = -f(4) / f(2); |
1198 |
|
|
m_zmp_RF(1) = f(3) / f(2); |
1199 |
|
|
m_zmp_RF(2) = -H_rf.translation()(2); |
1200 |
|
|
} else |
1201 |
|
|
m_zmp_RF.setZero(); |
1202 |
|
|
|
1203 |
|
|
m_zmp_RF = H_rf.act(m_zmp_RF); |
1204 |
|
|
s = m_zmp_RF.head<2>(); |
1205 |
|
|
return s; |
1206 |
|
|
} |
1207 |
|
|
|
1208 |
|
|
DEFINE_SIGNAL_OUT_FUNCTION(zmp_left_foot, dynamicgraph::Vector) { |
1209 |
|
|
if (!m_initSucceeded) { |
1210 |
|
|
SEND_WARNING_STREAM_MSG( |
1211 |
|
|
"Cannot compute signal zmp_left_foot before initialization!"); |
1212 |
|
|
return s; |
1213 |
|
|
} |
1214 |
|
|
if (s.size() != 2) s.resize(2); |
1215 |
|
|
const Vector6& f = m_wrench_left_footSIN(iter); |
1216 |
|
|
pinocchio::SE3 H_lf = m_robot->position( |
1217 |
|
|
m_invDyn->data(), m_robot->model().getJointId( |
1218 |
|
|
m_robot_util->m_foot_util.m_Left_Foot_Frame_Name)); |
1219 |
|
|
if (fabs(f(2) > 1.0)) { |
1220 |
|
|
m_zmp_LF(0) = -f(4) / f(2); |
1221 |
|
|
m_zmp_LF(1) = f(3) / f(2); |
1222 |
|
|
m_zmp_LF(2) = -H_lf.translation()(2); |
1223 |
|
|
} else |
1224 |
|
|
m_zmp_LF.setZero(); |
1225 |
|
|
|
1226 |
|
|
m_zmp_LF = H_lf.act(m_zmp_LF); |
1227 |
|
|
s = m_zmp_LF.head<2>(); |
1228 |
|
|
return s; |
1229 |
|
|
} |
1230 |
|
|
|
1231 |
|
|
DEFINE_SIGNAL_OUT_FUNCTION(zmp, dynamicgraph::Vector) { |
1232 |
|
|
if (!m_initSucceeded) { |
1233 |
|
|
std::ostringstream oss("Cannot compute signal zmp before initialization!"); |
1234 |
|
|
oss << iter; |
1235 |
|
|
SEND_WARNING_STREAM_MSG(oss.str()); |
1236 |
|
|
return s; |
1237 |
|
|
} |
1238 |
|
|
if (s.size() != 2) s.resize(2); |
1239 |
|
|
const Vector6& f_LF = m_wrench_left_footSIN(iter); |
1240 |
|
|
const Vector6& f_RF = m_wrench_right_footSIN(iter); |
1241 |
|
|
m_zmp_left_footSOUT(iter); |
1242 |
|
|
m_zmp_right_footSOUT(iter); |
1243 |
|
|
|
1244 |
|
|
if (f_LF(2) + f_RF(2) > 1.0) |
1245 |
|
|
m_zmp = (f_RF(2) * m_zmp_RF + f_LF(2) * m_zmp_LF) / (f_LF(2) + f_RF(2)); |
1246 |
|
|
s = m_zmp.head<2>(); |
1247 |
|
|
return s; |
1248 |
|
|
} |
1249 |
|
|
|
1250 |
|
|
DEFINE_SIGNAL_OUT_FUNCTION(com, dynamicgraph::Vector) { |
1251 |
|
|
if (!m_initSucceeded) { |
1252 |
|
|
std::ostringstream oss( |
1253 |
|
|
"Cannot compute signal com before initialization! iter:"); |
1254 |
|
|
oss << iter; |
1255 |
|
|
SEND_WARNING_STREAM_MSG(oss.str()); |
1256 |
|
|
return s; |
1257 |
|
|
} |
1258 |
|
|
if (s.size() != 3) s.resize(3); |
1259 |
|
|
const Vector3& com = m_robot->com(m_invDyn->data()); |
1260 |
|
|
s = com + m_com_offset; |
1261 |
|
|
return s; |
1262 |
|
|
} |
1263 |
|
|
|
1264 |
|
|
DEFINE_SIGNAL_OUT_FUNCTION(com_vel, dynamicgraph::Vector) { |
1265 |
|
|
if (!m_initSucceeded) { |
1266 |
|
|
std::ostringstream oss( |
1267 |
|
|
"Cannot compute signal com_vel before initialization! iter:"); |
1268 |
|
|
oss << iter; |
1269 |
|
|
SEND_WARNING_STREAM_MSG(oss.str()); |
1270 |
|
|
return s; |
1271 |
|
|
} |
1272 |
|
|
if (s.size() != 3) s.resize(3); |
1273 |
|
|
const Vector3& com_vel = m_robot->com_vel(m_invDyn->data()); |
1274 |
|
|
s = com_vel; |
1275 |
|
|
return s; |
1276 |
|
|
} |
1277 |
|
|
|
1278 |
|
|
DEFINE_SIGNAL_OUT_FUNCTION(base_orientation, dynamicgraph::Vector) { |
1279 |
|
|
if (!m_initSucceeded) { |
1280 |
|
|
std::ostringstream oss( |
1281 |
|
|
"Cannot compute signal com_vel before initialization! iter:"); |
1282 |
|
|
oss << iter; |
1283 |
|
|
SEND_WARNING_STREAM_MSG(oss.str()); |
1284 |
|
|
return s; |
1285 |
|
|
} |
1286 |
|
|
/* |
1287 |
|
|
* Code |
1288 |
|
|
*/ |
1289 |
|
|
return s; |
1290 |
|
|
} |
1291 |
|
|
|
1292 |
|
|
DEFINE_SIGNAL_OUT_FUNCTION(left_foot_pos, dynamicgraph::Vector) { |
1293 |
|
|
if (!m_initSucceeded) { |
1294 |
|
|
std::ostringstream oss( |
1295 |
|
|
"Cannot compute signal left_foot_pos before initialization! iter:"); |
1296 |
|
|
oss << iter; |
1297 |
|
|
SEND_WARNING_STREAM_MSG(oss.str()); |
1298 |
|
|
return s; |
1299 |
|
|
} |
1300 |
|
|
m_tau_desSOUT(iter); |
1301 |
|
|
pinocchio::SE3 oMi; |
1302 |
|
|
s.resize(12); |
1303 |
|
|
m_robot->framePosition(m_invDyn->data(), m_frame_id_lf, oMi); |
1304 |
|
|
tsid::math::SE3ToVector(oMi, s); |
1305 |
|
|
return s; |
1306 |
|
|
} |
1307 |
|
|
|
1308 |
|
|
DEFINE_SIGNAL_OUT_FUNCTION(left_hand_pos, dynamicgraph::Vector) { |
1309 |
|
|
if (!m_initSucceeded) { |
1310 |
|
|
SEND_WARNING_STREAM_MSG( |
1311 |
|
|
"Cannot compute signal left hand_pos before initialization!"); |
1312 |
|
|
return s; |
1313 |
|
|
} |
1314 |
|
|
m_tau_desSOUT(iter); |
1315 |
|
|
pinocchio::SE3 oMi; |
1316 |
|
|
s.resize(12); |
1317 |
|
|
m_robot->framePosition(m_invDyn->data(), m_frame_id_lh, oMi); |
1318 |
|
|
tsid::math::SE3ToVector(oMi, s); |
1319 |
|
|
return s; |
1320 |
|
|
} |
1321 |
|
|
|
1322 |
|
|
DEFINE_SIGNAL_OUT_FUNCTION(right_foot_pos, dynamicgraph::Vector) { |
1323 |
|
|
if (!m_initSucceeded) { |
1324 |
|
|
std::ostringstream oss( |
1325 |
|
|
"Cannot compute signal rigt_foot_pos before initialization! iter:"); |
1326 |
|
|
oss << iter; |
1327 |
|
|
SEND_WARNING_STREAM_MSG(oss.str()); |
1328 |
|
|
return s; |
1329 |
|
|
} |
1330 |
|
|
m_tau_desSOUT(iter); |
1331 |
|
|
pinocchio::SE3 oMi; |
1332 |
|
|
s.resize(12); |
1333 |
|
|
m_robot->framePosition(m_invDyn->data(), m_frame_id_rf, oMi); |
1334 |
|
|
tsid::math::SE3ToVector(oMi, s); |
1335 |
|
|
return s; |
1336 |
|
|
} |
1337 |
|
|
|
1338 |
|
|
DEFINE_SIGNAL_OUT_FUNCTION(right_hand_pos, dynamicgraph::Vector) { |
1339 |
|
|
if (!m_initSucceeded) { |
1340 |
|
|
SEND_WARNING_STREAM_MSG( |
1341 |
|
|
"Cannot compute signal right_hand_pos before initialization!"); |
1342 |
|
|
return s; |
1343 |
|
|
} |
1344 |
|
|
m_tau_desSOUT(iter); |
1345 |
|
|
pinocchio::SE3 oMi; |
1346 |
|
|
s.resize(12); |
1347 |
|
|
m_robot->framePosition(m_invDyn->data(), m_frame_id_rh, oMi); |
1348 |
|
|
tsid::math::SE3ToVector(oMi, s); |
1349 |
|
|
return s; |
1350 |
|
|
} |
1351 |
|
|
|
1352 |
|
|
DEFINE_SIGNAL_OUT_FUNCTION(left_foot_vel, dynamicgraph::Vector) { |
1353 |
|
|
if (!m_initSucceeded) { |
1354 |
|
|
std::ostringstream oss( |
1355 |
|
|
"Cannot compute signal left_foot_vel before initialization!"); |
1356 |
|
|
oss << iter; |
1357 |
|
|
SEND_WARNING_STREAM_MSG(oss.str()); |
1358 |
|
|
return s; |
1359 |
|
|
} |
1360 |
|
|
pinocchio::Motion v; |
1361 |
|
|
m_robot->frameVelocity(m_invDyn->data(), m_frame_id_lf, v); |
1362 |
|
|
s = v.toVector(); |
1363 |
|
|
return s; |
1364 |
|
|
} |
1365 |
|
|
|
1366 |
|
|
DEFINE_SIGNAL_OUT_FUNCTION(left_hand_vel, dynamicgraph::Vector) { |
1367 |
|
|
if (!m_initSucceeded) { |
1368 |
|
|
std::ostringstream oss( |
1369 |
|
|
"Cannot compute signal left_hand_vel before initialization!:"); |
1370 |
|
|
oss << iter; |
1371 |
|
|
SEND_WARNING_STREAM_MSG(oss.str()); |
1372 |
|
|
return s; |
1373 |
|
|
} |
1374 |
|
|
pinocchio::Motion v; |
1375 |
|
|
m_robot->frameVelocity(m_invDyn->data(), m_frame_id_lh, v); |
1376 |
|
|
s = v.toVector(); |
1377 |
|
|
return s; |
1378 |
|
|
} |
1379 |
|
|
|
1380 |
|
|
DEFINE_SIGNAL_OUT_FUNCTION(right_foot_vel, dynamicgraph::Vector) { |
1381 |
|
|
if (!m_initSucceeded) { |
1382 |
|
|
SEND_WARNING_STREAM_MSG( |
1383 |
|
|
"Cannot compute signal right_foot_vel before initialization! iter:" + |
1384 |
|
|
toString(iter)); |
1385 |
|
|
return s; |
1386 |
|
|
} |
1387 |
|
|
pinocchio::Motion v; |
1388 |
|
|
m_robot->frameVelocity(m_invDyn->data(), m_frame_id_rf, v); |
1389 |
|
|
s = v.toVector(); |
1390 |
|
|
return s; |
1391 |
|
|
} |
1392 |
|
|
|
1393 |
|
|
DEFINE_SIGNAL_OUT_FUNCTION(right_hand_vel, dynamicgraph::Vector) { |
1394 |
|
|
if (!m_initSucceeded) { |
1395 |
|
|
SEND_WARNING_STREAM_MSG( |
1396 |
|
|
"Cannot compute signal right_hand_vel before initialization! " + |
1397 |
|
|
toString(iter)); |
1398 |
|
|
return s; |
1399 |
|
|
} |
1400 |
|
|
pinocchio::Motion v; |
1401 |
|
|
m_robot->frameVelocity(m_invDyn->data(), m_frame_id_rh, v); |
1402 |
|
|
s = v.toVector(); |
1403 |
|
|
return s; |
1404 |
|
|
} |
1405 |
|
|
|
1406 |
|
|
DEFINE_SIGNAL_OUT_FUNCTION(left_foot_acc, dynamicgraph::Vector) { |
1407 |
|
|
if (!m_initSucceeded) { |
1408 |
|
|
SEND_WARNING_STREAM_MSG( |
1409 |
|
|
"Cannot compute signal left_foot_acc before initialization! " + |
1410 |
|
|
toString(iter)); |
1411 |
|
|
return s; |
1412 |
|
|
} |
1413 |
|
|
m_tau_desSOUT(iter); |
1414 |
|
|
if (m_contactState == RIGHT_SUPPORT) |
1415 |
|
|
s = m_taskLF->getAcceleration(m_dv_urdf); |
1416 |
|
|
else |
1417 |
|
|
s = m_contactLF->getMotionTask().getAcceleration(m_dv_urdf); |
1418 |
|
|
return s; |
1419 |
|
|
} |
1420 |
|
|
|
1421 |
|
|
DEFINE_SIGNAL_OUT_FUNCTION(left_hand_acc, dynamicgraph::Vector) { |
1422 |
|
|
if (!m_initSucceeded) { |
1423 |
|
|
SEND_WARNING_STREAM_MSG( |
1424 |
|
|
"Cannot compute signal left_hand_acc before initialization!"); |
1425 |
|
|
return s; |
1426 |
|
|
} |
1427 |
|
|
m_tau_desSOUT(iter); |
1428 |
|
|
s = m_contactLH->getMotionTask().getAcceleration(m_dv_urdf); |
1429 |
|
|
return s; |
1430 |
|
|
} |
1431 |
|
|
|
1432 |
|
|
DEFINE_SIGNAL_OUT_FUNCTION(right_foot_acc, dynamicgraph::Vector) { |
1433 |
|
|
if (!m_initSucceeded) { |
1434 |
|
|
SEND_WARNING_STREAM_MSG( |
1435 |
|
|
"Cannot compute signal right_foot_acc before initialization!"); |
1436 |
|
|
return s; |
1437 |
|
|
} |
1438 |
|
|
m_tau_desSOUT(iter); |
1439 |
|
|
if (m_contactState == LEFT_SUPPORT) |
1440 |
|
|
s = m_taskRF->getAcceleration(m_dv_urdf); |
1441 |
|
|
else |
1442 |
|
|
s = m_contactRF->getMotionTask().getAcceleration(m_dv_urdf); |
1443 |
|
|
return s; |
1444 |
|
|
} |
1445 |
|
|
|
1446 |
|
|
DEFINE_SIGNAL_OUT_FUNCTION(right_hand_acc, dynamicgraph::Vector) { |
1447 |
|
|
if (!m_initSucceeded) { |
1448 |
|
|
SEND_WARNING_STREAM_MSG( |
1449 |
|
|
"Cannot compute signal right_hand_acc before initialization!"); |
1450 |
|
|
return s; |
1451 |
|
|
} |
1452 |
|
|
m_tau_desSOUT(iter); |
1453 |
|
|
s = m_contactRH->getMotionTask().getAcceleration(m_dv_urdf); |
1454 |
|
|
return s; |
1455 |
|
|
} |
1456 |
|
|
|
1457 |
|
|
DEFINE_SIGNAL_OUT_FUNCTION(left_foot_acc_des, dynamicgraph::Vector) { |
1458 |
|
|
if (!m_initSucceeded) { |
1459 |
|
|
SEND_WARNING_STREAM_MSG( |
1460 |
|
|
"Cannot compute signal left_foot_acc_des before initialization!"); |
1461 |
|
|
return s; |
1462 |
|
|
} |
1463 |
|
|
m_tau_desSOUT(iter); |
1464 |
|
|
if (m_contactState == RIGHT_SUPPORT) |
1465 |
|
|
s = m_taskLF->getDesiredAcceleration(); |
1466 |
|
|
else |
1467 |
|
|
s = m_contactLF->getMotionTask().getDesiredAcceleration(); |
1468 |
|
|
return s; |
1469 |
|
|
} |
1470 |
|
|
|
1471 |
|
|
DEFINE_SIGNAL_OUT_FUNCTION(right_foot_acc_des, dynamicgraph::Vector) { |
1472 |
|
|
if (!m_initSucceeded) { |
1473 |
|
|
SEND_WARNING_STREAM_MSG( |
1474 |
|
|
"Cannot compute signal right_foot_acc_des before initialization!"); |
1475 |
|
|
return s; |
1476 |
|
|
} |
1477 |
|
|
m_tau_desSOUT(iter); |
1478 |
|
|
if (m_contactState == LEFT_SUPPORT) |
1479 |
|
|
s = m_taskRF->getDesiredAcceleration(); |
1480 |
|
|
else |
1481 |
|
|
s = m_contactRF->getMotionTask().getDesiredAcceleration(); |
1482 |
|
|
return s; |
1483 |
|
|
} |
1484 |
|
|
|
1485 |
|
|
/* --- COMMANDS ---------------------------------------------------------- */ |
1486 |
|
|
|
1487 |
|
|
/* ------------------------------------------------------------------- */ |
1488 |
|
|
/* --- ENTITY -------------------------------------------------------- */ |
1489 |
|
|
/* ------------------------------------------------------------------- */ |
1490 |
|
|
|
1491 |
|
|
void InverseDynamicsBalanceController::display(std::ostream& os) const { |
1492 |
|
|
os << "InverseDynamicsBalanceController " << getName(); |
1493 |
|
|
try { |
1494 |
|
|
getProfiler().report_all(3, os); |
1495 |
|
|
getStatistics().report_all(1, os); |
1496 |
|
|
os << "QP size: nVar " << m_invDyn->nVar() << " nEq " << m_invDyn->nEq() |
1497 |
|
|
<< " nIn " << m_invDyn->nIn() << "\n"; |
1498 |
|
|
} catch (ExceptionSignal e) { |
1499 |
|
|
} |
1500 |
|
|
} |
1501 |
|
|
} // namespace torque_control |
1502 |
|
|
} // namespace sot |
1503 |
|
|
} // namespace dynamicgraph |