Tetrapod Project
hierarchical_optimization_controller.cpp
Go to the documentation of this file.
1 /*******************************************************************/
2 /* AUTHOR: Paal Arthur S. Thorseth */
3 /* ORGN: Dept of Eng Cybernetics, NTNU Trondheim */
4 /* FILE: hierarchical_optimization_controller.cpp */
5 /* DATE: May 15, 2021 */
6 /* */
7 /* Copyright (C) 2021 Paal Arthur S. Thorseth, */
8 /* Adrian B. Ghansah */
9 /* */
10 /* This program is free software: you can redistribute it */
11 /* and/or modify it under the terms of the GNU General */
12 /* Public License as published by the Free Software Foundation, */
13 /* either version 3 of the License, or (at your option) any */
14 /* later version. */
15 /* */
16 /* This program is distributed in the hope that it will be useful, */
17 /* but WITHOUT ANY WARRANTY; without even the implied warranty */
18 /* of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. */
19 /* See the GNU General Public License for more details. */
20 /* */
21 /* You should have received a copy of the GNU General Public */
22 /* License along with this program. If not, see */
23 /* <https://www.gnu.org/licenses/>. */
24 /* */
25 /*******************************************************************/
26 
27 #include <hierarchical_optimization_controller/hierarchical_optimization_controller.h>
28 
29 // Constructor
30 HierarchicalOptimizationControl::HierarchicalOptimizationControl()
31 {
32  this->InitRos();
33  this->InitRosQueueThreads();
34 
35  this->genCoord.setZero();
36  this->genVel.setZero();
37  this->fPos.setZero();
38 
39  this->contactState[0] = 1;
40  this->contactState[1] = 1;
41  this->contactState[2] = 1;
42  this->contactState[3] = 1;
43 }
44 
45 // Destructor
46 HierarchicalOptimizationControl::~HierarchicalOptimizationControl()
47 {
48  this->rosNode->shutdown();
49 
50  this->rosProcessQueue.clear();
51  this->rosProcessQueue.disable();
52  this->rosProcessQueueThread.join();
53 
54  this->rosPublishQueue.clear();
55  this->rosPublishQueue.disable();
56  this->rosPublishQueueThread.join();
57 }
58 
59 // TODO remove this
60 void HierarchicalOptimizationControl::StaticTorqueTest()
61 {
62  // Declare desired states
63  Eigen::Vector3d desired_base_pos;
64  Eigen::Vector3d desired_base_vel;
65  Eigen::Vector3d desired_base_acc;
66  Eigen::Vector3d desired_base_ori;
67  Eigen::Matrix<Eigen::Vector3d, 4, 1> desired_f_pos;
68  Eigen::Matrix<Eigen::Vector3d, 4, 1> desired_f_vel;
69  Eigen::Matrix<Eigen::Vector3d, 4, 1> desired_f_acc;
70 
71  // Declare torque solution
72  Eigen::Matrix<double, 12, 1> desired_tau;
73 
74  // TODO Message to send with base pos command to be removed after plotting..
75  std_msgs::Float64MultiArray base_pose_cmd_msg;
76  std_msgs::Float64MultiArray base_twist_cmd_msg;
77 
78  base_pose_cmd_msg.data.resize(6);
79  base_twist_cmd_msg.data.resize(6);
80 
81  // Loop rate
82  ros::Rate loop_rate(200);
83 
84  // Loop
85  while(this->rosNode->ok())
86  {
87  // Set desired values
88  desired_base_pos << 0,
89  0,
90  0.25;
91  desired_base_vel.setZero();
92  desired_base_acc.setZero();
93  desired_base_ori.setZero();
94 
95  desired_f_pos = this->fPos;
96 
97  for (int i = 0; i < 4; i++)
98  {
99  desired_f_vel(i).setZero();
100  desired_f_acc(i).setZero();
101  }
102 
103  auto start = std::chrono::steady_clock::now();
104 
105  desired_tau = this->HierarchicalOptimization(desired_base_pos,
106  desired_base_vel,
107  desired_base_acc,
108  desired_base_ori,
109  desired_f_pos,
110  desired_f_vel,
111  desired_f_acc,
112  this->fPos,
113  this->fVel,
114  this->genCoord,
115  this->genVel,
116  0);
117 
118  auto end = std::chrono::steady_clock::now();
119 
120  std::chrono::duration<double, std::micro> diff = end - start;
121 
122  ROS_INFO_STREAM("HO run-time: " << diff.count() << " microseconds. \n");
123 
124  //debug_utils::printBaseState(this->genCoord.topRows(6));
125  //debug_utils::printFootstepPositions(this->fPos);
126  //debug_utils::printJointTorques(desired_tau.bottomRows(12));
127  //debug_utils::printJointTorques(desired_tau);
128 
129  // TODO find better solution to plot data and remove this..
130  base_pose_cmd_msg.data[0] = desired_base_pos(0);
131  base_pose_cmd_msg.data[1] = desired_base_pos(1);
132  base_pose_cmd_msg.data[2] = desired_base_pos(2);
133  base_pose_cmd_msg.data[3] = desired_base_ori(0);
134  base_pose_cmd_msg.data[4] = desired_base_ori(1);
135  base_pose_cmd_msg.data[5] = desired_base_ori(2);
136  this->basePoseCmdPub.publish(base_pose_cmd_msg);
137 
138  base_twist_cmd_msg.data[0] = desired_base_vel(0);
139  base_twist_cmd_msg.data[1] = desired_base_vel(1);
140  base_twist_cmd_msg.data[2] = desired_base_vel(2);
141  base_twist_cmd_msg.data[3] = 0;
142  base_twist_cmd_msg.data[4] = 0;
143  base_twist_cmd_msg.data[5] = 0;
144  this->baseTwistCmdPub.publish(base_twist_cmd_msg);
145 
146  //this->PublishTorqueMsg(desired_tau.bottomRows(12));
147  this->PublishTorqueMsg(desired_tau);
148 
149  loop_rate.sleep();
150  }
151 }
152 
153 // TODO remove this
154 void HierarchicalOptimizationControl::HeightRollYawTest()
155 {
156  // Declare desired states
157  Eigen::Vector3d desired_base_pos;
158  Eigen::Vector3d desired_base_vel;
159  Eigen::Vector3d desired_base_acc;
160  Eigen::Vector3d desired_base_ori;
161  Eigen::Matrix<Eigen::Vector3d, 4, 1> desired_f_pos;
162  Eigen::Matrix<Eigen::Vector3d, 4, 1> desired_f_vel;
163  Eigen::Matrix<Eigen::Vector3d, 4, 1> desired_f_acc;
164 
165  // Declare torque solution
166  Eigen::Matrix<double, 12, 1> desired_tau;
167 
168  // TODO Message to send with base pos command to be removed after plotting..
169  std_msgs::Float64MultiArray base_pose_cmd_msg;
170  std_msgs::Float64MultiArray base_twist_cmd_msg;
171 
172  base_pose_cmd_msg.data.resize(6);
173  base_twist_cmd_msg.data.resize(6);
174 
175  // Loop rate
176  ros::Rate loop_rate(200);
177 
178  // Time factor
179  int timefactor = 3;
180 
181  // Loop
182  while(this->rosNode->ok())
183  {
184  // Set desired values
185  desired_base_pos << 0,
186  0,
187  // 0.25;
188  0.05 * std::sin(timefactor*ros::Time::now().toSec()) + 0.2;
189  desired_base_vel.setZero();
190  desired_base_vel << 0,
191  0,
192  timefactor * 0.05 * std::cos(timefactor*ros::Time::now().toSec());
193  desired_base_acc.setZero();
194  desired_base_acc << 0,
195  0,
196  - timefactor * timefactor * 0.05 * std::sin(timefactor*ros::Time::now().toSec());
197  desired_base_ori.setZero();
198 
199  desired_base_ori(0) = 0.3 * std::cos(timefactor*ros::Time::now().toSec());
200  desired_base_ori(2) = 0.15 * std::sin(timefactor*ros::Time::now().toSec());
201 
202  desired_f_pos = this->fPos;
203 
204  for (int i = 0; i < 4; i++)
205  {
206  desired_f_vel(i).setZero();
207  desired_f_acc(i).setZero();
208  }
209 
210 
211 
212 
213  auto start = std::chrono::steady_clock::now();
214 
215  desired_tau = this->HierarchicalOptimization(desired_base_pos,
216  desired_base_vel,
217  desired_base_acc,
218  desired_base_ori,
219  desired_f_pos,
220  desired_f_vel,
221  desired_f_acc,
222  this->fPos,
223  this->fVel,
224  this->genCoord,
225  this->genVel,
226  0);
227 
228  auto end = std::chrono::steady_clock::now();
229 
230  std::chrono::duration<double, std::micro> diff = end - start;
231 
232  ROS_INFO_STREAM("HO run-time: " << diff.count() << " microseconds. \n");
233 
234  //debug_utils::printBaseState(this->genCoord.topRows(6));
235  //debug_utils::printFootstepPositions(this->fPos);
236  //debug_utils::printJointTorques(desired_tau.bottomRows(12));
237  //debug_utils::printJointTorques(desired_tau);
238 
239  // TODO find better solution to plot data and remove this..
240  base_pose_cmd_msg.data[0] = desired_base_pos(0);
241  base_pose_cmd_msg.data[1] = desired_base_pos(1);
242  base_pose_cmd_msg.data[2] = desired_base_pos(2);
243  base_pose_cmd_msg.data[3] = desired_base_ori(0);
244  base_pose_cmd_msg.data[4] = desired_base_ori(1);
245  base_pose_cmd_msg.data[5] = desired_base_ori(2);
246  this->basePoseCmdPub.publish(base_pose_cmd_msg);
247 
248  base_twist_cmd_msg.data[0] = desired_base_vel(0);
249  base_twist_cmd_msg.data[1] = desired_base_vel(1);
250  base_twist_cmd_msg.data[2] = desired_base_vel(2);
251  base_twist_cmd_msg.data[3] = 0;
252  base_twist_cmd_msg.data[4] = 0;
253  base_twist_cmd_msg.data[5] = 0;
254  this->baseTwistCmdPub.publish(base_twist_cmd_msg);
255 
256  //this->PublishTorqueMsg(desired_tau.bottomRows(12));
257  this->PublishTorqueMsg(desired_tau);
258 
259  //break;
260 
261  loop_rate.sleep();
262  }
263 }
264 
265 // Hierarchical Optimization
266 Eigen::Matrix<double, 12, 1> HierarchicalOptimizationControl::HierarchicalOptimization(const Eigen::Matrix<double, 3, 1> &_desired_base_pos,
267  const Eigen::Matrix<double, 3, 1> &_desired_base_vel,
268  const Eigen::Matrix<double, 3, 1> &_desired_base_acc,
269  const Eigen::Matrix<double, 3, 1> &_desired_base_ori,
270  const Eigen::Matrix<Eigen::Vector3d, 4, 1> &_desired_f_pos,
271  const Eigen::Matrix<Eigen::Vector3d, 4, 1> &_desired_f_vel,
272  const Eigen::Matrix<Eigen::Vector3d, 4, 1> &_desired_f_acc,
273  const Eigen::Matrix<Eigen::Vector3d, 4, 1> &_f_pos,
274  const Eigen::Matrix<Eigen::Vector3d, 4, 1> &_f_vel,
275  const Eigen::Matrix<double, 18, 1> &_q,
276  const Eigen::Matrix<double, 18, 1> &_u,
277  const int &_v)
278 {
279  //*************************************************************************************
280  // Declarations
281  //*************************************************************************************
282 
283  Eigen::Matrix<double, 12, 1> desired_tau; // 12x1 Reference joint torques
284 
285  Eigen::VectorXd x_opt; // Optimal solution x_opt = [dot_u_opt, F_c_opt]^T
286 
287  SolverType solver = SolverType::OSQP; // Solver type
288 
289  // Tasks used to formulate the hierarchical optimization
290  // problem
291  Task t_eom; // Task for the equations of motion
292  Task t_cmc; // Task for the contact motion constraint
293  Task t_cftl; // Task for the contact force and torque limits
294  Task t_mt; // Task for the motion tracking of the floating base and swing legs
295  Task t_pt; // Task for posture tracking, for a nominal posture
296  Task t_cfm; // Task for the contact force minimization
297 
298  std::vector<Task> tasks; // Set of tasks
299 
300  // Kinematic and dynamic matrices and terms used to
301  // enforce constraints.
302  Eigen::Matrix<double, 18, 18> M; // 18x18 Mass matrix
303  Eigen::Matrix<double, 18, 1> b; // 18x1 Coriolis and centrifugal terms
304  Eigen::Matrix<double, 18, 1> g; // 18x1 Gravitational terms
305  Eigen::Matrix<double, Eigen::Dynamic, 18> J_c; // (3*n_c)x18 Contact Jacobian
306  Eigen::Matrix<double, Eigen::Dynamic, 18> dot_J_c; // (3*n_c)x18 Time derivative of the Contact Jacobian
307 
308  // Matrices and parameters used to enfore motion tracking
309  Eigen::Matrix<double, 3, 18> J_P_fb; // 3x18 Floating base Translation Jacobian
310  Eigen::Matrix<double, 3, 18> J_R_fb; // 3x18 Floating base Rotation Jacobian
311  Eigen::Matrix<double, 3, 18> J_P_fl; // 3x18 Front left foot Translation Jacobian
312  Eigen::Matrix<double, 3, 18> J_P_fr; // 3x18 Front right foot Translation Jacobian
313  Eigen::Matrix<double, 3, 18> J_P_rl; // 3x18 Rear left foot Translation Jacobian
314  Eigen::Matrix<double, 3, 18> J_P_rr; // 3x18 Rear right foot Translation Jacobian
315 
316  Eigen::Matrix<double, 3, 18> dot_J_P_fb; // 3x18 Time derivative of the Floating base Translation Jacobian
317  Eigen::Matrix<double, 3, 18> dot_J_R_fb; // 3x18 Time derivative of the Floating base Rotation Jacobian
318  Eigen::Matrix<double, 3, 18> dot_J_P_fl; // 3x18 Time derivative of the Front left foot Translation Jacobian
319  Eigen::Matrix<double, 3, 18> dot_J_P_fr; // 3x18 Time derivative of the Front right foot Translation Jacobian
320  Eigen::Matrix<double, 3, 18> dot_J_P_rl; // 3x18 Time derivative of the Rear left foot Translation Jacobian
321  Eigen::Matrix<double, 3, 18> dot_J_P_rr; // 3x18 Time derivative of the Rear right foot Translation Jacobian
322 
323  // Terms used to enforce posture tracking
324  Eigen::Matrix<double, 12, 1> q_r_nom; // 12x1 Nominal posture configuration
325 
326  // Terms used to enforce contact force and torque limits
327  Eigen::Vector3d h; // 3x1 Heading direction of the control frame expressed in the inertial frame
328  Eigen::Vector3d l; // 3x1 Lateral direction of the control frame expressed in the inertial frame
329  Eigen::Vector3d n; // 3x1 Normal direction of the control frame expressed in the inertial frame
330  Eigen::Matrix<double, 3, 3> rotationWToC; // 3x3 Rotation matrix from world to control frame (transform from control to world).
331  Eigen::Matrix<double, 12, 1> tau_min; // 18x1 Minimum actuator torques
332  Eigen::Matrix<double, 12, 1> tau_max; // 18x1 Maximum actuator torques
333 
334  std::vector<Kinematics::LegType> contact_legs; // Vector of contact points (legs)
335  std::vector<Kinematics::LegType> swing_legs; // Vector of swing legs
336 
337  //*************************************************************************************
338  // Tuning parameters
339  //*************************************************************************************
340 
341  // Friction coefficient
342  double mu = 0.6;
343 
344  // Motion tracking gains
345  Eigen::Matrix3d k_p_fb_pos = 15*Eigen::Matrix3d::Identity(); // Floating base position proportional gain
346  Eigen::Matrix3d k_d_fb_pos = 2*Eigen::Matrix3d::Identity(); // Floating base position derivative gain
347  Eigen::Matrix3d k_p_fb_rot = 30*Eigen::Matrix3d::Identity(); // Floating base rotation proportional gain
348  Eigen::Matrix3d k_d_fb_rot = 2*Eigen::Matrix3d::Identity(); // Floating base rotation proportional gain
349  Eigen::Matrix3d k_p_fl = 2*Eigen::Matrix3d::Identity(); // Front left foot proportional gain
350  Eigen::Matrix3d k_d_fl = 2*Eigen::Matrix3d::Identity(); // Front left foot derivative gain
351  Eigen::Matrix3d k_p_fr = 2*Eigen::Matrix3d::Identity(); // Front right foot proportional gain
352  Eigen::Matrix3d k_d_fr = 2*Eigen::Matrix3d::Identity(); // Front right foot derivative gain
353  Eigen::Matrix3d k_p_rl = 2*Eigen::Matrix3d::Identity(); // Rear left foot proportional gain
354  Eigen::Matrix3d k_d_rl = 2*Eigen::Matrix3d::Identity(); // Rear left foot derivative gain
355  Eigen::Matrix3d k_p_rr = 2*Eigen::Matrix3d::Identity(); // Rear right foot proportional gain
356  Eigen::Matrix3d k_d_rr = 2*Eigen::Matrix3d::Identity(); // Rear right foot derivative gain
357 
358  // Posture tracking gains
359  Eigen::Matrix<double, 12, 12> k_p_pt = 15*Eigen::Matrix<double, 12, 12>::Identity(); // Posture tracking proportional gain
360  Eigen::Matrix<double, 12, 12> k_d_pt = 4*Eigen::Matrix<double, 12, 12>::Identity(); // Posture tracking derivative gain
361 
362  //*************************************************************************************
363  // Updates
364  //*************************************************************************************
365 
366  // Fill contact and swing legs
367  for (size_t i = 0; i < 4; i++)
368  {
369  // Contact State is assumed sorted by fl, fr, rl, rr
370  if (contactState[i])
371  {
372  // fl = 1, fr = 2, rl = 3, rr = 4
373  contact_legs.push_back(Kinematics::LegType(i + 1));
374  }
375  else
376  {
377  // fl = 1, fr = 2, rl = 3, rr = 4
378  swing_legs.push_back(Kinematics::LegType(i + 1));
379  }
380  }
381 
382  // Number of contact and swing legs, respectively
383  const unsigned int n_c = contact_legs.size();
384  const unsigned int n_s = swing_legs.size();
385 
386  // Update state dimension x = [dot_u, F_c]^T
387  const unsigned int state_dim = 18 + 3*n_c;
388 
389  // Resize task dimensions
390  t_eom.A_eq.resize(6, state_dim);
391  t_eom.b_eq.resize(6, 1);
392 
393  t_cmc.A_eq.resize(3*n_c, state_dim);
394  t_cmc.b_eq.resize(3*n_c, 1);
395 
396  t_cftl.A_ineq.resize(4*n_c + 24, state_dim);
397  t_cftl.b_ineq.resize(4*n_c + 24, 1);
398 
399  t_mt.A_eq.resize(18, state_dim);
400  t_mt.b_eq.resize(18, 1);
401 
402  t_pt.A_eq.resize(12, state_dim);
403  t_pt.b_eq.resize(12, 1);
404 
405  t_cfm.A_eq.resize(3*n_c, state_dim);
406  t_cfm.b_eq.resize(3*n_c, 1);
407 
408  // Resize dynamic matrices and terms
409  x_opt.resize(state_dim, 1);
410 
411  J_c.resize(3*n_c, 18);
412  dot_J_c.resize(3*n_c, 18);
413 
414 
415  // Get base orientation
416  kindr::EulerAnglesXyz<double> base_ori(_q.segment(3,3));
417 
418  kindr::EulerAnglesXyz<double> desired_base_ori_kindr(_desired_base_ori);
419 
420  Eigen::Vector3d orientation_error = desired_base_ori_kindr.boxMinus(base_ori);
421 
422  // Update matrices and terms
423  M = kinematics.GetMassMatrix(_q);
424 
425  b = kinematics.GetCoriolisAndCentrifugalTerms(_q, _u);
426 
427  g = kinematics.GetGravitationalTerms(_q);
428 
429  J_c = kinematics.GetContactJacobianInW(contact_legs, _q);
430 
431  dot_J_c = kinematics.GetContactJacobianInWDiff(contact_legs, _q, _u);
432 
433  // Update matrices used by motion tracking constraints
434  J_P_fb = kinematics.GetTranslationJacobianInC(Kinematics::LegType::NONE,
435  Kinematics::BodyType::base,
436  _q);
437  J_R_fb = kinematics.GetRotationJacobianInC(Kinematics::LegType::NONE,
438  Kinematics::BodyType::base,
439  _q);
440  J_P_fl = kinematics.GetTranslationJacobianInC(Kinematics::LegType::frontLeft,
441  Kinematics::BodyType::foot,
442  _q);
443  J_P_fr = kinematics.GetTranslationJacobianInC(Kinematics::LegType::frontRight,
444  Kinematics::BodyType::foot,
445  _q);
446  J_P_rl = kinematics.GetTranslationJacobianInC(Kinematics::LegType::rearLeft,
447  Kinematics::BodyType::foot,
448  _q);
449  J_P_rr = kinematics.GetTranslationJacobianInC(Kinematics::LegType::rearRight,
450  Kinematics::BodyType::foot,
451  _q);
452 
453  dot_J_P_fb = kinematics.GetTranslationJacobianInCDiff(Kinematics::LegType::NONE,
454  Kinematics::BodyType::base,
455  _q,
456  _u);
457  dot_J_R_fb = kinematics.GetRotationJacobianInCDiff(Kinematics::LegType::NONE,
458  Kinematics::BodyType::base,
459  _q,
460  _u);
461  dot_J_P_fl = kinematics.GetTranslationJacobianInCDiff(Kinematics::LegType::frontLeft,
462  Kinematics::BodyType::foot,
463  _q,
464  _u);
465  dot_J_P_fr = kinematics.GetTranslationJacobianInCDiff(Kinematics::LegType::frontRight,
466  Kinematics::BodyType::foot,
467  _q,
468  _u);
469  dot_J_P_rl = kinematics.GetTranslationJacobianInCDiff(Kinematics::LegType::rearLeft,
470  Kinematics::BodyType::foot,
471  _q,
472  _u);
473  dot_J_P_rr = kinematics.GetTranslationJacobianInCDiff(Kinematics::LegType::rearRight,
474  Kinematics::BodyType::foot,
475  _q,
476  _u);
477  // Update terms used by posture tracking constraints
478  q_r_nom << math_utils::degToRad(45),
479  math_utils::degToRad(-20),
480  math_utils::degToRad(90),
481  math_utils::degToRad(-45),
482  math_utils::degToRad(20),
483  math_utils::degToRad(-90),
484  math_utils::degToRad(135),
485  math_utils::degToRad(20),
486  math_utils::degToRad(-90),
487  math_utils::degToRad(-135),
488  math_utils::degToRad(-20),
489  math_utils::degToRad(90);
490  //q_r_nom << math_utils::degToRad(45),
491  // math_utils::degToRad(40),
492  // math_utils::degToRad(35),
493  // math_utils::degToRad(-45),
494  // math_utils::degToRad(-40),
495  // math_utils::degToRad(-35),
496  // math_utils::degToRad(135),
497  // math_utils::degToRad(-40),
498  // math_utils::degToRad(-35),
499  // math_utils::degToRad(-135),
500  // math_utils::degToRad(40),
501  // math_utils::degToRad(35);
502 
503  // Update terms used by the contact force and torque limits
504  rotationWToC = kinematics.GetRotationMatrixWToC(0, 0, _q(5));
505 
506  h = rotationWToC * Eigen::Vector3d(1, 0, 0);
507  l = rotationWToC * Eigen::Vector3d(0, 1, 0);
508  n = Eigen::Vector3d(0, 0, 1);
509 
510  tau_max.setConstant(40);
511  tau_min.setConstant(-40);
512 
513  // Update equations of motion task
514  t_eom.A_eq.leftCols(18) = M.topRows(6);
515  t_eom.A_eq.rightCols(3*n_c) = - J_c.transpose().topRows(6);
516  t_eom.b_eq = - (b.topRows(6) + g.topRows(6));
517 
518  // Update contact force and torque limits task
519  // Friction Cone
520  for (size_t i = 0; i < n_c; i++)
521  {
522  t_cftl.A_ineq.block(4*i, 0, 1, state_dim).setZero();
523  t_cftl.A_ineq.block(4*i, 18 + 3*i, 1, 3) = (h.transpose() - n.transpose() * mu);
524 
525  t_cftl.A_ineq.block(4*i + 1, 0, 1, state_dim).setZero();
526  t_cftl.A_ineq.block(4*i + 1, 18 + 3*i, 1, 3) = - (h.transpose() + n.transpose() * mu);
527 
528  t_cftl.A_ineq.block(4*i + 2, 0, 1, state_dim).setZero();
529  t_cftl.A_ineq.block(4*i + 2, 18 + 3*i, 1, 3) = (l.transpose() - n.transpose() * mu);
530 
531  t_cftl.A_ineq.block(4*i + 3, 0, 1, state_dim).setZero();
532  t_cftl.A_ineq.block(4*i + 3, 18 + 3*i, 1, 3) = - (l.transpose() + n.transpose() * mu);
533  }
534 
535  t_cftl.b_ineq.topRows(4*n_c).setZero();
536  // Max torque limit
537  //t_cftl.A_ineq.block(4*n_c, 0, 12, state_dim).leftCols(18) = M.bottomRows(12);
538  //t_cftl.A_ineq.block(4*n_c, 0, 12, state_dim).rightCols(3*n_c) = - J_c.transpose().bottomRows(12);
539  //t_cftl.b_ineq.block(4*n_c, 0, 12, state_dim) = tau_max - (b.bottomRows(12) + g.bottomRows(12));
540  t_cftl.A_ineq.block(4*n_c, 0, 12, 18) = M.bottomRows(12);
541  t_cftl.A_ineq.block(4*n_c, 18, 12, 3*n_c) = - J_c.transpose().bottomRows(12);
542  t_cftl.b_ineq.block(4*n_c, 0, 12, 1) = tau_max - (b.bottomRows(12) + g.bottomRows(12));
543 
544  // // Min torque limit
545  //t_cftl.A_ineq.block(4*n_c + 12, 0, 12, state_dim).leftCols(18) = - M.bottomRows(12);
546  //t_cftl.A_ineq.block(4*n_c + 12, 0, 12, state_dim).rightCols(3*n_c) = J_c.transpose().bottomRows(12);
547  //t_cftl.b_ineq.block(4*n_c + 12, 0, 12, state_dim) = - tau_min + (b.bottomRows(12) + g.bottomRows(12));
548  t_cftl.A_ineq.block(4*n_c + 12, 0, 12, 18) = - M.bottomRows(12);
549  t_cftl.A_ineq.block(4*n_c + 12, 18, 12, 3*n_c) = J_c.transpose().bottomRows(12);
550  t_cftl.b_ineq.block(4*n_c + 12, 0, 12, 1) = - tau_min + (b.bottomRows(12) + g.bottomRows(12));
551 
552  // Update contact motion constraint task
553  t_cmc.A_eq.leftCols(18) = J_c;
554  t_cmc.A_eq.rightCols(3*n_c).setZero();
555  t_cmc.b_eq = - dot_J_c * _u;
556 
557  // Update motion tracking task
558  // Floating base position
559  t_mt.A_eq.setZero();
560  t_mt.b_eq.setZero();
561 
562  t_mt.A_eq.block(0, 0, 3, state_dim).leftCols(18) = J_P_fb;
563  t_mt.A_eq.block(0, 0, 3, state_dim).rightCols(3*n_c).setZero();
564  t_mt.b_eq.block(0, 0, 3, 1) = _desired_base_acc + k_p_fb_pos * (_desired_base_pos - _q.topRows(3))
565  + k_d_fb_pos * (_desired_base_vel - _u.topRows(3)) - dot_J_P_fb * _u;
566  //t_mt.A_eq.block(0, 0, 3, state_dim).setZero();
567  //t_mt.b_eq.block(0, 0, 3, 1).setZero();
568  // Floating base orientation
569  t_mt.A_eq.block(3, 0, 3, state_dim).leftCols(18) = J_R_fb;
570  t_mt.A_eq.block(3, 0, 3, state_dim).rightCols(3*n_c).setZero();
571 
572  //t_mt.b_eq.block(3, 0, 3, 1) = k_p_fb_rot * (_desired_base_ori - _q.segment(3,3))
573  // + k_d_fb_rot * (- _u.segment(3,3)) - dot_J_R_fb * _u;
574 
575  t_mt.b_eq.block(3, 0, 3, 1) = k_p_fb_rot * (orientation_error)
576  + k_d_fb_rot * (- _u.segment(3,3)) - dot_J_R_fb * _u;
577  //t_mt.A_eq.block(3, 0, 3, state_dim).setZero();
578  //t_mt.b_eq.block(3, 0, 3, 1).setZero();
579 
580  // Front-left foot
581  t_mt.A_eq.block(6, 0, 3, state_dim).leftCols(18) = J_P_fl;
582  t_mt.A_eq.block(6, 0, 3, state_dim).rightCols(3*n_c).setZero();
583  t_mt.b_eq.block(6, 0, 3, 1) = _desired_f_acc(0) + k_p_fl * (_desired_f_pos(0) - _f_pos(0))
584  + k_d_fl * (_desired_f_vel(0) - _f_vel(0)) - dot_J_P_fl * _u;
585 
586  // Front-right foot
587  t_mt.A_eq.block(9, 0, 3, state_dim).leftCols(18) = J_P_fr;
588  t_mt.A_eq.block(9, 0, 3, state_dim).rightCols(3*n_c).setZero();
589  t_mt.b_eq.block(9, 0, 3, 1) = _desired_f_acc(1) + k_p_fr * (_desired_f_pos(1) - _f_pos(1))
590  + k_d_fr * (_desired_f_vel(1) - _f_vel(1)) - dot_J_P_fr * _u;
591 
592  // Rear-left foot
593  t_mt.A_eq.block(12, 0, 3, state_dim).leftCols(18) = J_P_rl;
594  t_mt.A_eq.block(12, 0, 3, state_dim).rightCols(3*n_c).setZero();
595  t_mt.b_eq.block(12, 0, 3, 1) = _desired_f_acc(2) + k_p_rl * (_desired_f_pos(2) - _f_pos(2))
596  + k_d_rl * (_desired_f_vel(2) - _f_vel(2)) - dot_J_P_rl * _u;
597 
598  // Rear-right foot
599  t_mt.A_eq.block(15, 0, 3, state_dim).leftCols(18) = J_P_rr;
600  t_mt.A_eq.block(15, 0, 3, state_dim).rightCols(3*n_c).setZero();
601  t_mt.b_eq.block(15, 0, 3, 1) = _desired_f_acc(3) + k_p_rr * (_desired_f_pos(3) - _f_pos(3))
602  + k_d_rr * (_desired_f_vel(3) - _f_vel(3)) - dot_J_P_rr * _u;
603 
604  // Update posture tracking task
605  t_pt.A_eq.leftCols(6).setZero();
606  t_pt.A_eq.block(0, 6, 12, 12).setIdentity();
607  t_pt.A_eq.rightCols(3*n_c).setZero();
608  t_pt.b_eq = k_p_pt * (q_r_nom - _q.bottomRows(12)) + k_d_pt * (-_u.bottomRows(12));
609 
610  // Update contact force minimization task
611  t_cfm.A_eq.leftCols(18).setZero();
612  t_cfm.A_eq.rightCols(3*n_c).setIdentity();
613  t_cfm.b_eq.setZero();
614 
615  // Update task constraint setting
616  t_eom.has_eq_constraint = true;
617  t_cftl.has_ineq_constraint = true;
618  t_cmc.has_eq_constraint = true;
619  t_mt.has_eq_constraint = true;
620  t_pt.has_eq_constraint = true;
621  t_cfm.has_eq_constraint = true;
622 
623  //*************************************************************************************
624  // Hierarchical QP Optimization
625  //*************************************************************************************
626 
627  // Add tasks in prioritized order
628  tasks.push_back(t_eom);
629  tasks.push_back(t_cftl);
630  tasks.push_back(t_cmc);
631  tasks.push_back(t_mt);
632  //tasks.push_back(t_pt);
633  tasks.push_back(t_cfm);
634 
635  Eigen::Matrix<Eigen::MatrixXd, 5, 1> A_ls;
636  Eigen::Matrix<Eigen::VectorXd, 5, 1> b_ls;
637 
638  A_ls(0) = t_eom.A_eq;
639  b_ls(0) = t_eom.b_eq;
640  A_ls(1) = t_cmc.A_eq;
641  b_ls(1) = t_cmc.b_eq;
642  A_ls(2) = t_mt.A_eq;
643  b_ls(2) = t_mt.b_eq;
644  A_ls(3) = t_pt.A_eq;
645  b_ls(3) = t_pt.b_eq;
646  A_ls(4) = t_cfm.A_eq;
647  b_ls(4) = t_cfm.b_eq;
648 
649  // Solve the hierarchical optimization problem
650  x_opt = HierarchicalQPOptimization(state_dim,
651  tasks,
652  solver,
653  _v);
654 
655  //x_opt = HierarchicalLeastSquareOptimization(A_ls,
656  // b_ls,
657  // _v);
658 
659  //*************************************************************************************
660  // Compute reference joint torques
661  //*************************************************************************************
662 
663  desired_tau = M.bottomRows(12) * x_opt.topRows(18) - J_c.transpose().bottomRows(12) * x_opt.bottomRows(3*n_c) + b.bottomRows(12) + g.bottomRows(12);
664 
665  // Printing TODO Fix
666  //if (_v > 0)
667  if (true)
668  {
669  debug_utils::printGeneralizedAccelerations(x_opt.topRows(18));
670  debug_utils::printFootstepForces(this->contactState, x_opt.bottomRows(3*n_c));
671  debug_utils::printJointTorques(desired_tau);
672  }
673 
674 
675  return desired_tau;
676 }
677 
678 // Hierarchical QP Optimization
679 Eigen::Matrix<double, Eigen::Dynamic, 1> HierarchicalOptimizationControl::HierarchicalQPOptimization(const int &_state_dim,
680  const std::vector<Task> &_tasks,
681  const SolverType &_solver,
682  const int &_v)
683 {
684  // Declarations
685  Eigen::VectorXd x_opt(_state_dim); // Optimal solution
686  Eigen::MatrixXd N(_state_dim, _state_dim); // Null-space projector
687  Eigen::VectorXd z(_state_dim); // Null-space vector
688  Eigen::MatrixXd stacked_A_eq; // Stacked equality matrices
689  Eigen::MatrixXd stacked_A_ineq; // Stacked inequality matrices
690  Eigen::VectorXd stacked_b_ineq; // Stacked inequality vectors
691 
692  Eigen::MatrixXd Q; // Quadratic program Q cost matrix
693  Eigen::VectorXd c; // Quadratic program c cost vector
694  Eigen::MatrixXd E_ineq; // Quadratic program E_ineq inequality constraint matrix
695  Eigen::VectorXd f_ineq; // Quadratic program f_ineq inequality constraint vector
696  Eigen::VectorXd xi; // Quadratic program optimization variable, xi = [z, v]^T
697  Eigen::VectorXd v; // Quadratic program inequality constraint slack variable
698  Eigen::VectorXd stacked_v; // Stacked slack variables
699 
700  int rowsA_eq; // Number of rows of current tasks A_eq
701  int rowsA_ineq; // Number of rows of current tasks A_ineq
702  int colsN; // Number of cols of current null-space projector N
703  int count = 0; // Task counter
704 
705  // Initializations
706  x_opt.setZero();
707  N.setIdentity();
708 
709  // Iterate over the set of tasks
710  for (Task t : _tasks)
711  {
712  // Update count
713  count += 1;
714 
715  // Update null-space dimension
716  colsN = N.cols();
717 
718  // If task has a equality constraint
719  if (t.has_eq_constraint)
720  {
721  // Update row count
722  rowsA_eq = t.A_eq.rows();
723 
724  // Update stacked equality matrices
725  Eigen::MatrixXd stacked_A_eq_tmp = stacked_A_eq;
726 
727  stacked_A_eq.resize(t.A_eq.rows() + stacked_A_eq_tmp.rows(), _state_dim);
728  stacked_A_eq << t.A_eq,
729  stacked_A_eq_tmp;
730  }
731  else
732  {
733  // Update row count
734  rowsA_eq = 0;
735  }
736 
737  // If task has a inequality constraint
738  if (t.has_ineq_constraint)
739  {
740  // Update row count
741  rowsA_ineq = t.A_ineq.rows();
742 
743  // Update stacked inequality matrices
744  Eigen::MatrixXd stacked_A_ineq_tmp = stacked_A_ineq;
745 
746  stacked_A_ineq.resize(t.A_ineq.rows() + stacked_A_ineq_tmp.rows(), _state_dim);
747  stacked_A_ineq << t.A_ineq,
748  stacked_A_ineq_tmp;
749 
750  // Update stacked inequality vectors
751  Eigen::VectorXd stacked_b_ineq_tmp = stacked_b_ineq;
752 
753  stacked_b_ineq.resize(t.b_ineq.rows() + stacked_b_ineq_tmp.rows(), 1);
754  stacked_b_ineq << t.b_ineq,
755  stacked_b_ineq_tmp;
756  }
757  else
758  {
759  // Update row count
760  rowsA_ineq = 0;
761  }
762 
763  // Update dimensions
764  z.resize(colsN, 1);
765  v.resize(rowsA_ineq, 1);
766  xi.resize(colsN + rowsA_ineq, 1);
767 
768  // Update QP costs
769  Q.resize(colsN + rowsA_ineq, colsN + rowsA_ineq);
770  c.resize(colsN + rowsA_ineq, 1);
771 
772  if (t.has_eq_constraint)
773  {
774  Q.topLeftCorner(colsN, colsN) = N.transpose() * t.A_eq.transpose() * t.A_eq * N;
775  Q.topRightCorner(colsN, rowsA_ineq).setZero();
776  Q.bottomLeftCorner(rowsA_ineq, colsN).setZero();
777  Q.bottomRightCorner(rowsA_ineq, rowsA_ineq).setIdentity();
778 
779  c.topRows(colsN) = N.transpose() * t.A_eq.transpose() * (t.A_eq * x_opt - t.b_eq);
780  c.bottomRows(rowsA_ineq).setZero();
781  }
782  else
783  {
784  Q.setZero();
785  Q.bottomRightCorner(rowsA_ineq, rowsA_ineq).setIdentity();
786 
787  c.setZero();
788  }
789 
790  // Update inequality constraints
791  E_ineq.resize(stacked_A_ineq.rows() + rowsA_ineq, colsN + rowsA_ineq);
792  f_ineq.resize(stacked_A_ineq.rows() + rowsA_ineq, 1);
793 
794  E_ineq.setZero();
795 
796  E_ineq.topLeftCorner(stacked_A_ineq.rows(), colsN) = stacked_A_ineq * N;
797  E_ineq.topRightCorner(rowsA_ineq, rowsA_ineq) = - Eigen::MatrixXd::Identity(rowsA_ineq, rowsA_ineq);
798 
799  E_ineq.bottomRightCorner(rowsA_ineq, rowsA_ineq) = - Eigen::MatrixXd::Identity(rowsA_ineq, rowsA_ineq);
800 
801  f_ineq.topRows(stacked_b_ineq.rows()) = stacked_b_ineq - stacked_A_ineq * x_opt;
802  f_ineq.segment(rowsA_ineq, stacked_b_ineq.rows() - rowsA_ineq) += stacked_v;
803  f_ineq.bottomRows(rowsA_ineq).setZero();
804 
805  // Print w.r.t verbosity level
806  if (_v > 1)
807  {
808  ROS_INFO_STREAM("----------------------" << "Task: " << count << "---------------------- \n" );
809  ROS_INFO_STREAM("Q: \n" << Q << "\n");
810  ROS_INFO_STREAM("eigenvalues(Q): \n" << Q.eigenvalues() << "\n");
811  ROS_INFO_STREAM("c: \n" << c << "\n");
812  ROS_INFO_STREAM("E_ineq: \n" << E_ineq << "\n");
813  ROS_INFO_STREAM("f_ineq: \n" << f_ineq << "\n");
814  }
815  if (_v > 2)
816  {
817  ROS_INFO_STREAM("N: \n" << N << "\n");
818  ROS_INFO_STREAM("A_eq: \n" << t.A_eq << "\n");
819  ROS_INFO_STREAM("b_eq: \n" << t.b_eq << "\n");
820  ROS_INFO_STREAM("stacked_A_eq: \n" << stacked_A_eq << "\n");
821  ROS_INFO_STREAM("stacked_A_ineq: \n" << stacked_A_ineq << "\n");
822  ROS_INFO_STREAM("stacked_b_ineq: \n" << stacked_b_ineq << "\n");
823  }
824 
825  // Solve QP
826  if (!SolveQP(Q, c, E_ineq, f_ineq, xi, _solver,_v))
827  {
828  ROS_ERROR_STREAM("[HierarchicalOptimizationControl::HierarchicalQPOptimization] Failed to solve QP at task " << count << "!");
829 
830  break;
831  }
832 
833  // Get solutions
834  z = xi.topRows(colsN);
835  v = xi.bottomRows(rowsA_ineq);
836 
837  // Update optimal state for the hierarchical optimization
838  x_opt = x_opt + N * z;
839 
840  // Update the null-space projector if a equality constraint
841  // is present.
842  if (t.has_eq_constraint)
843  {
844  N = math_utils::SVDNullSpaceProjector(stacked_A_eq);
845  }
846 
847  // Update stacked slack variables if a inequality constraint
848  // is present.
849  if (t.has_ineq_constraint)
850  {
851  Eigen::VectorXd stacked_v_tmp = stacked_v;
852 
853  stacked_v.resize(v.rows() + stacked_v_tmp.rows(), 1);
854  stacked_v << v,
855  stacked_v_tmp;
856  }
857 
858  // Print w.r.t verbosity level
859  if (_v > 0 )
860  {
861  ROS_INFO_STREAM("Solution at task " << count << " is: \n" << x_opt);
862  }
863 
864  // Terminate if null-space is zero (i.e. only the trivial solution exist)
865  if (N.isZero())
866  {
867  if (_v > 0)
868  {
869  ROS_INFO_STREAM("Terminating at task " << count << " due to the null-space being zero. \n");
870  }
871  break;
872  }
873 
874  }
875 
876  return x_opt;
877 }
878 
879 // Hierarchical Least-Square Optimization
880 Eigen::Matrix<double, Eigen::Dynamic, 1> HierarchicalOptimizationControl::HierarchicalLeastSquareOptimization(const Eigen::Matrix<Eigen::MatrixXd, Eigen::Dynamic, 1> &_A,
881  const Eigen::Matrix<Eigen::Matrix<double, Eigen::Dynamic, 1>, Eigen::Dynamic, 1> &_b,
882  const int &_v)
883 {
884  const auto rowsA = _A.rows();
885  const auto rowsb = _b.rows();
886 
887  // Validate number of equations
888  if( rowsA != rowsb )
889  {
890  ROS_ERROR("[HierarchicalOptimizationControl::HierarchicalLeastSquareOptimization] _A and _b does not contain equal amount of elements!");
891 
892  std::abort();
893  }
894 
895  const auto state_dim = _A(0).cols();
896 
897  // Validate state and equation dimensions
898  for (size_t i = 0; i < rowsA; i++)
899  {
900  if (_A(i).cols() != state_dim || _A(i).rows() != _b(i).rows() )
901  {
902  ROS_ERROR_STREAM("[HierarchicalOptimizationControl::HierarchicalLeastSquareOptimization] State or equation dimensions failed at index: i = !" << i);
903 
904  std::abort();
905  }
906  }
907 
908  // Declarations
909  Eigen::Matrix<double, Eigen::Dynamic, 1> x_opt; // Optimal solution
910  Eigen::Matrix<double, Eigen::Dynamic, 1> x_i; // Optimal solution for the at hand projected QP
911  Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> N; // Null-space projector
912  Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> AN; // Matrix multiplication of A and N (A*N)
913  Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> pinvAN; // Pseudo-inverse of A*N
914  Eigen::MatrixXd A_stacked; // Stacked matrix of A_i matrices
915 
916  // Setup
917  x_opt.resize(state_dim, 1);
918  N.resize(state_dim, state_dim);
919 
920  // Initialize
921  x_opt.setZero();
922  N.setIdentity();
923 
924  // Loop
925  for (size_t i = 0; i < rowsA; i++)
926  {
927  // Find Pseudoinverse
928  AN = _A(i) * N;
929  kindr::pseudoInverse(AN, pinvAN);
930 
931  // Find least squares optimal state for the at hand QP
932  x_i = pinvAN * (_b(i) - _A(i) * x_opt);
933 
934  // Update optimal state for the hierarchical optimization
935  x_opt = x_opt + N * x_i;
936 
937  // Find new null-space projector
938  Eigen::MatrixXd A_stacked_tmp = A_stacked;
939 
940  A_stacked.resize(A_stacked_tmp.rows() + _A(i).rows(), state_dim);
941  A_stacked << A_stacked_tmp,
942  _A(i);
943 
944  N = math_utils::SVDNullSpaceProjector(A_stacked);
945 
946  // Print w.r.t verbosity level
947  if (_v == 1)
948  {
949  ROS_INFO_STREAM("Solution at iteration " << i << " is: \n" << x_opt);
950  }
951 
952  }
953 
954  return x_opt;
955 }
956 
957 // Solve a Quadratic Program
958 bool HierarchicalOptimizationControl::SolveQP(const Eigen::MatrixXd &_Q,
959  const Eigen::VectorXd &_c,
960  const Eigen::MatrixXd &_E_ineq,
961  const Eigen::VectorXd &_f_ineq,
962  Eigen::VectorXd &_sol,
963  const SolverType &_solver,
964  const int &_v)
965 {
966  // Validate dimensions
967  if (_E_ineq.rows() != _f_ineq.rows())
968  {
969  ROS_INFO_STREAM("[HierarchicalOptimizationControl::SolveQP] Equality constraint dimensions does not match!");
970 
971  std::abort();
972  }
973 
974  // Create a negative infinite lower bound
975  Eigen::VectorXd lb;
976 
977  lb.resize(_f_ineq.rows(), 1);
978 
979  lb.setConstant(-math_utils::INF);
980 
981  return this->SolveQP(_Q, _c, _E_ineq, lb, _f_ineq, _sol, _solver, _v);
982 }
983 
984 // Solve a Quadratic Program
985 bool HierarchicalOptimizationControl::SolveQP(const Eigen::MatrixXd &_Q,
986  const Eigen::VectorXd &_c,
987  const Eigen::MatrixXd &_A,
988  const Eigen::VectorXd &_lb,
989  const Eigen::VectorXd &_ub,
990  Eigen::VectorXd &_sol,
991  const SolverType &_solver,
992  const int &_v)
993 {
994  // Create an empty Mathematical Program
995  drake::solvers::MathematicalProgram prog;
996 
997  // Create an empty Mathematical Program Result
998  drake::solvers::MathematicalProgramResult result;
999 
1000  // Add decision variables
1001  auto x = prog.NewContinuousVariables(_Q.cols(), "x");
1002 
1003  // Add quadratic cost
1004  prog.AddQuadraticCost(_Q, _c, x, true);
1005 
1006  // Add inequality constraints
1007  if (!_A.isZero())
1008  {
1009  if (_v > 2)
1010  {
1011  ROS_INFO_STREAM("Linear ineq, A: \n" << _A << "\n");
1012  ROS_INFO_STREAM("Linear ineq, lb: \n" << _lb << "\n");
1013  ROS_INFO_STREAM("Linear ineq, ub: \n" << _ub << "\n");
1014  }
1015 
1016  prog.AddLinearConstraint(_A, _lb, _ub, x);
1017  }
1018 
1019  // Solve the program
1020  switch (_solver)
1021  {
1022  case OSQP:
1023  {
1024  auto solver = drake::solvers::OsqpSolver();
1025 
1026  auto start = std::chrono::steady_clock::now();
1027 
1028  result = solver.Solve(prog);
1029 
1030  if (_v > 2)
1031  {
1032  auto end = std::chrono::steady_clock::now();
1033 
1034  std::chrono::duration<double, std::micro> diff = end - start;
1035 
1036  ROS_INFO_STREAM("OSQP run-time: " << diff.count() << " microseconds. \n");
1037 
1038  auto details = result.get_solver_details<drake::solvers::OsqpSolver>();
1039  ROS_INFO_STREAM("Solver details: run_time: " << details.run_time << "\n");
1040  }
1041 
1042  break;
1043  }
1044  case ECQP:
1045  {
1046  auto solver = drake::solvers::EqualityConstrainedQPSolver();
1047  result = solver.Solve(prog);
1048 
1049  break;
1050  }
1051  case CLP:
1052  {
1053  auto solver = drake::solvers::ClpSolver();
1054  result = solver.Solve(prog);
1055 
1056  break;
1057  }
1058  case SCS:
1059  {
1060  auto solver = drake::solvers::ScsSolver();
1061  result = solver.Solve(prog);
1062 
1063  break;
1064  }
1065  case SNOPT:
1066  {
1067  auto solver = drake::solvers::SnoptSolver();
1068  result = solver.Solve(prog);
1069 
1070  break;
1071  }
1072  case UNSPECIFIED:
1073  {
1074  result = Solve(prog);
1075 
1076  break;
1077  }
1078  default:
1079  {
1080  ROS_WARN("[HierarchicalOptimizationControl::SolveQP] Could not determine solver type!");
1081 
1082  std::abort();
1083  }
1084  }
1085 
1086  if (!result.is_success())
1087  {
1088  ROS_WARN("[HierarchicalOptimizationControl::SolveQP] Failed to solve QP!");
1089 
1090  if (_v > 0)
1091  {
1092  //auto details = result.get_solver_details<drake::solvers::SnoptSolver>();
1093 
1094  ROS_INFO_STREAM("Solver id: " << result.get_solver_id() << "\n");
1095  ROS_INFO_STREAM("Solver result: " << result.get_solution_result() << "\n");
1096  ROS_INFO_STREAM("Solution result: " << result.GetSolution(x) << "\n");
1097  ROS_INFO_STREAM("Solution result: " << result.GetSolution(x) << "\n");
1098  //ROS_INFO_STREAM("Solver detals: status_val: " << details.status_val << "\n");
1099  //ROS_INFO_STREAM("Solver details: run_time: " << details.run_time);
1100  //ROS_INFO_STREAM("Solver details: info: " << details.info << "\n");
1101  }
1102 
1103  _sol.setZero();
1104 
1105  return false;
1106  }
1107 
1108  // Set solution
1109  _sol = result.GetSolution(x);
1110 
1111 
1112  // TODO add verbosity prints
1113 
1114 
1115  return true;
1116 }
1117 
1118 
1119 // Solve a Quadratic Program
1120 bool HierarchicalOptimizationControl::SolveQP(const Eigen::MatrixXd &_Q,
1121  const Eigen::VectorXd &_c,
1122  const Eigen::MatrixXd &_E_eq,
1123  const Eigen::VectorXd &_f_eq,
1124  const Eigen::MatrixXd &_E_ineq,
1125  const Eigen::VectorXd &_f_ineq,
1126  Eigen::VectorXd &_sol,
1127  const SolverType &_solver,
1128  const int &_v)
1129 {
1130  // Create an empty Mathematical Program
1131  drake::solvers::MathematicalProgram prog;
1132 
1133  // Create an empty Mathematical Program Result
1134  drake::solvers::MathematicalProgramResult result;
1135 
1136  // Add decision variables
1137  auto x = prog.NewContinuousVariables(_Q.cols(), "x");
1138 
1139  // Add quadratic cost
1140  prog.AddQuadraticCost(_Q, _c, x, true);
1141 
1142  // Add equality constraints
1143  if (!_E_eq.isZero())
1144  {
1145  prog.AddLinearEqualityConstraint(_E_eq, _f_eq, x);
1146  }
1147 
1148  // Add inequality constraints
1149  if (!_E_ineq.isZero())
1150  {
1151  prog.AddLinearConstraint((_E_ineq * x).array() <= _f_ineq.array());
1152  }
1153 
1154  // Solve the program
1155  switch (_solver)
1156  {
1157  case OSQP:
1158  {
1159  auto solver = drake::solvers::OsqpSolver();
1160  result = solver.Solve(prog);
1161  break;
1162  }
1163  case ECQP:
1164  {
1165  auto solver = drake::solvers::EqualityConstrainedQPSolver();
1166  result = solver.Solve(prog);
1167  break;
1168  }
1169  case CLP:
1170  {
1171  auto solver = drake::solvers::ClpSolver();
1172  result = solver.Solve(prog);
1173  break;
1174  }
1175  case SCS:
1176  {
1177  auto solver = drake::solvers::ScsSolver();
1178  result = solver.Solve(prog);
1179  break;
1180  }
1181  case SNOPT:
1182  {
1183  auto solver = drake::solvers::SnoptSolver();
1184  result = solver.Solve(prog);
1185  break;
1186  }
1187  case UNSPECIFIED:
1188  {
1189  result = Solve(prog);
1190  break;
1191  }
1192  default:
1193  {
1194  ROS_WARN("[HierarchicalOptimizationControl::SolveQP] Could not determine solver type!");
1195 
1196  std::abort();
1197  }
1198  }
1199 
1200  if (!result.is_success())
1201  {
1202  ROS_WARN("[HierarchicalOptimizationControl::SolveQP] Failed to solve QP!");
1203 
1204  _sol.setZero();
1205 
1206  return false;
1207  }
1208 
1209  // Set solution
1210  _sol = result.GetSolution(x);
1211 
1212 
1213  // TODO add verbosity prints
1214 
1215 
1216  return true;
1217 }
1218 
1219 // Publish function for ROS Joint State Torque messages
1220 void HierarchicalOptimizationControl::PublishTorqueMsg(const Eigen::Matrix<double, 12, 1> &_desired_tau)
1221 {
1222  // Declare msg
1223  sensor_msgs::JointState joint_state_msg;
1224 
1225  // Set dimension
1226  joint_state_msg.effort.resize(12);
1227 
1228  // Front-left
1229  joint_state_msg.effort[0] = _desired_tau(0);
1230  joint_state_msg.effort[1] = _desired_tau(1);
1231  joint_state_msg.effort[2] = _desired_tau(2);
1232 
1233  // Front right
1234  joint_state_msg.effort[3] = _desired_tau(3);
1235  joint_state_msg.effort[4] = _desired_tau(4);
1236  joint_state_msg.effort[5] = _desired_tau(5);
1237 
1238  // Rear left
1239  joint_state_msg.effort[6] = _desired_tau(6);
1240  joint_state_msg.effort[7] = _desired_tau(7);
1241  joint_state_msg.effort[8] = _desired_tau(8);
1242 
1243  // Rear right
1244  joint_state_msg.effort[9] = _desired_tau(9);
1245  joint_state_msg.effort[10] = _desired_tau(10);
1246  joint_state_msg.effort[11] = _desired_tau(11);
1247 
1248  // Set timestamp
1249  joint_state_msg.header.stamp = ros::Time::now();
1250 
1251  // Publish
1252  this->jointStatePub.publish(joint_state_msg);
1253 }
1254 
1255 // Callback for ROS Generalized Coordinates messages
1256 void HierarchicalOptimizationControl::OnGenCoordMsg(const std_msgs::Float64MultiArrayConstPtr &_msg)
1257 {
1258  std::vector<double> data = _msg->data;
1259 
1260  this->genCoord = Eigen::Map<Eigen::MatrixXd>(data.data(), 18, 1);
1261 
1262  //debug_utils::printGeneralizedCoordinates(this->genCoord);
1263 
1264  if (!this->kinematics.SolveForwardKinematics(genCoord, fPos))
1265  {
1266  ROS_ERROR("[HierarchicalOptimizationControl::OnGenCoordMsg] Could not solve forward kinematics!");
1267  }
1268 
1269  //debug_utils::printFootstepPositions(this->fPos);
1270 }
1271 
1272 // Callback for ROS Generalized Velocities messages
1273 void HierarchicalOptimizationControl::OnGenVelMsg(const std_msgs::Float64MultiArrayConstPtr &_msg)
1274 {
1275  std::vector<double> data = _msg->data;
1276 
1277  this->genVel = Eigen::Map<Eigen::MatrixXd>(data.data(), 18, 1);
1278 
1279  for (int i = 0; i < 4; i++)
1280  {
1281  this->fVel(i) = kinematics.GetTranslationJacobianInW(Kinematics::LegType(i + 1), Kinematics::BodyType::foot, this->genCoord)
1282  * this->genVel;
1283  }
1284 
1285 }
1286 
1287 // Callback for ROS Contact State messages
1288 void HierarchicalOptimizationControl::OnContactStateMsg(const std_msgs::Int8MultiArrayConstPtr &_msg)
1289 {
1290  if (!_msg->data.size() == 4)
1291  {
1292  ROS_ERROR("[HierarchicalOptimizationControl::OnContactStateMsg] Received contact message with wrong size!");
1293  }
1294  else
1295  {
1296  // TODO Fix this
1297  //this->contactState[0] = _msg->data[0];
1298  //this->contactState[1] = _msg->data[1];
1299  //this->contactState[2] = _msg->data[2];
1300  //this->contactState[3] = _msg->data[3];
1301  }
1302 }
1303 
1304 // Setup thread to process messages
1305 void HierarchicalOptimizationControl::ProcessQueueThread()
1306 {
1307  static const double timeout = 0.01;
1308  while (this->rosNode->ok())
1309  {
1310  this->rosProcessQueue.callAvailable(ros::WallDuration(timeout));
1311  }
1312 }
1313 
1314 // Setup thread to publish messages
1315 void HierarchicalOptimizationControl::PublishQueueThread()
1316 {
1317  static const double timeout = 0.01;
1318  while (this->rosNode->ok())
1319  {
1320  this->rosPublishQueue.callAvailable(ros::WallDuration(timeout));
1321  }
1322 }
1323 
1324 // Initialize ROS
1325 void HierarchicalOptimizationControl::InitRos()
1326 {
1327  if (!ros::isInitialized())
1328  {
1329  int argc = 0;
1330  char **argv = NULL;
1331  ros::init(
1332  argc,
1333  argv,
1334  "hierarchical_optimization_control_node",
1335  ros::init_options::NoSigintHandler
1336  );
1337  }
1338 
1339  this->rosNode.reset(new ros::NodeHandle("hierarchical_optimization_control_node"));
1340 
1341  ros::SubscribeOptions gen_coord_so =
1342  ros::SubscribeOptions::create<std_msgs::Float64MultiArray>(
1343  "/my_robot/gen_coord",
1344  1,
1345  boost::bind(&HierarchicalOptimizationControl::OnGenCoordMsg, this, _1),
1346  ros::VoidPtr(),
1347  &this->rosProcessQueue
1348  );
1349 
1350  ros::SubscribeOptions gen_vel_so =
1351  ros::SubscribeOptions::create<std_msgs::Float64MultiArray>(
1352  "/my_robot/gen_vel",
1353  1,
1354  boost::bind(&HierarchicalOptimizationControl::OnGenVelMsg, this, _1),
1355  ros::VoidPtr(),
1356  &this->rosProcessQueue
1357  );
1358 
1359  ros::SubscribeOptions contact_state_so =
1360  ros::SubscribeOptions::create<std_msgs::Int8MultiArray>(
1361  "/my_robot/contact_state",
1362  1,
1363  boost::bind(&HierarchicalOptimizationControl::OnContactStateMsg, this, _1),
1364  ros::VoidPtr(),
1365  &this->rosProcessQueue
1366  );
1367 
1368  ros::AdvertiseOptions joint_state_ao =
1369  ros::AdvertiseOptions::create<sensor_msgs::JointState>(
1370  "/my_robot/joint_state_cmd",
1371  1,
1372  ros::SubscriberStatusCallback(),
1373  ros::SubscriberStatusCallback(),
1374  ros::VoidPtr(),
1375  &this->rosPublishQueue
1376  );
1377 
1378  ros::AdvertiseOptions base_pose_cmd_ao =
1379  ros::AdvertiseOptions::create<std_msgs::Float64MultiArray>(
1380  "/my_robot/base_pose_cmd",
1381  1,
1382  ros::SubscriberStatusCallback(),
1383  ros::SubscriberStatusCallback(),
1384  ros::VoidPtr(),
1385  &this->rosPublishQueue
1386  );
1387 
1388  ros::AdvertiseOptions base_twist_cmd_ao =
1389  ros::AdvertiseOptions::create<std_msgs::Float64MultiArray>(
1390  "/my_robot/base_twist_cmd",
1391  1,
1392  ros::SubscriberStatusCallback(),
1393  ros::SubscriberStatusCallback(),
1394  ros::VoidPtr(),
1395  &this->rosPublishQueue
1396  );
1397 
1398  this->genCoordSub = this->rosNode->subscribe(gen_coord_so);
1399 
1400  this->genVelSub = this->rosNode->subscribe(gen_vel_so);
1401 
1402  this->contactStateSub = this->rosNode->subscribe(contact_state_so);
1403 
1404  this->jointStatePub = this->rosNode->advertise(joint_state_ao);
1405 
1406  this->basePoseCmdPub = this->rosNode->advertise(base_pose_cmd_ao);
1407 
1408  this->baseTwistCmdPub = this->rosNode->advertise(base_twist_cmd_ao);
1409 }
1410 
1411 // Initialize ROS Publish and Process Queue Threads
1412 void HierarchicalOptimizationControl::InitRosQueueThreads()
1413 {
1414  this->rosPublishQueueThread = std::thread(
1415  std::bind(&HierarchicalOptimizationControl::PublishQueueThread, this)
1416  );
1417 
1418  this->rosProcessQueueThread = std::thread(
1419  std::bind(&HierarchicalOptimizationControl::ProcessQueueThread, this)
1420  );
1421 }
HierarchicalOptimizationControl::genVelSub
ros::Subscriber genVelSub
ROS Generalized Coordinates Subscriber.
Definition: hierarchical_optimization_controller.h:323
HierarchicalOptimizationControl::rosProcessQueueThread
std::thread rosProcessQueueThread
Thread running the rosProcessQueue.
Definition: hierarchical_optimization_controller.h:346
HierarchicalOptimizationControl::rosNode
std::unique_ptr< ros::NodeHandle > rosNode
Node used for ROS transport.
Definition: hierarchical_optimization_controller.h:317
HierarchicalOptimizationControl::rosPublishQueueThread
std::thread rosPublishQueueThread
Thread running the rosPublishQueue.
Definition: hierarchical_optimization_controller.h:349
HierarchicalOptimizationControl::genCoord
Eigen::Matrix< double, 18, 1 > genCoord
Generalized Coordinates.
Definition: hierarchical_optimization_controller.h:302
HierarchicalOptimizationControl::SolveQP
bool SolveQP(const Eigen::MatrixXd &_Q, const Eigen::VectorXd &_c, const Eigen::MatrixXd &_E_ineq, const Eigen::VectorXd &_f_ineq, Eigen::VectorXd &_sol, const SolverType &_solver=SolverType::UNSPECIFIED, const int &_v=0)
The SolveQP function solves a (convex) quadratic program (QP) on the form:
Definition: hierarchical_optimization_controller.cpp:958
HierarchicalOptimizationControl::PublishTorqueMsg
void PublishTorqueMsg(const Eigen::Matrix< double, 12, 1 > &_desired_tau)
The PublishTorqueMsg function publishes a desired torque message to the ROS topic set by the joint st...
Definition: hierarchical_optimization_controller.cpp:1220
HierarchicalOptimizationControl::HierarchicalQPOptimization
Eigen::Matrix< double, Eigen::Dynamic, 1 > HierarchicalQPOptimization(const int &_state_dim, const std::vector< Task > &_tasks, const SolverType &_solver=SolverType::OSQP, const int &_v=0)
The HierarchicalQPOptimization function finds the optimal solution for a set of tasks in a strictly p...
Definition: hierarchical_optimization_controller.cpp:679
HierarchicalOptimizationControl::rosProcessQueue
ros::CallbackQueue rosProcessQueue
ROS callbackque that helps process messages.
Definition: hierarchical_optimization_controller.h:340
HierarchicalOptimizationControl::genVel
Eigen::Matrix< double, 18, 1 > genVel
Generalized Velocities.
Definition: hierarchical_optimization_controller.h:305
HierarchicalOptimizationControl::ProcessQueueThread
void ProcessQueueThread()
The ProcessQueueThread function is a ROS helper function that processes messages.
Definition: hierarchical_optimization_controller.cpp:1305
HierarchicalOptimizationControl::OnGenCoordMsg
void OnGenCoordMsg(const std_msgs::Float64MultiArrayConstPtr &_msg)
The OnGenCoordMsg function handles an incoming generalized coordinates message from ROS.
Definition: hierarchical_optimization_controller.cpp:1256
HierarchicalOptimizationControl::fVel
Eigen::Matrix< Eigen::Vector3d, 4, 1 > fVel
Foot-point velocities.
Definition: hierarchical_optimization_controller.h:311
HierarchicalOptimizationControl::InitRosQueueThreads
void InitRosQueueThreads()
The InitRosQueueThreads function is called to initialize the ROS Publish and Process Queue Threads.
Definition: hierarchical_optimization_controller.cpp:1412
HierarchicalOptimizationControl::HierarchicalOptimization
Eigen::Matrix< double, 12, 1 > HierarchicalOptimization(const Eigen::Matrix< double, 3, 1 > &_desired_base_pos, const Eigen::Matrix< double, 3, 1 > &_desired_base_vel, const Eigen::Matrix< double, 3, 1 > &_desired_base_acc, const Eigen::Matrix< double, 3, 1 > &_desired_base_ori, const Eigen::Matrix< Eigen::Vector3d, 4, 1 > &_desired_f_pos, const Eigen::Matrix< Eigen::Vector3d, 4, 1 > &_desired_f_vel, const Eigen::Matrix< Eigen::Vector3d, 4, 1 > &_desired_f_acc, const Eigen::Matrix< Eigen::Vector3d, 4, 1 > &_f_pos, const Eigen::Matrix< Eigen::Vector3d, 4, 1 > &_f_vel, const Eigen::Matrix< double, 18, 1 > &_q, const Eigen::Matrix< double, 18, 1 > &_u, const int &_v=0)
The HierarchicalOptimization function finds the optimal joint torques for a desired motion plan....
Definition: hierarchical_optimization_controller.cpp:266
HierarchicalOptimizationControl::OnContactStateMsg
void OnContactStateMsg(const std_msgs::Int8MultiArrayConstPtr &_msg)
The OnContactStateMsg function handles an incoming contact state message from ROS.
Definition: hierarchical_optimization_controller.cpp:1288
HierarchicalOptimizationControl::fPos
Eigen::Matrix< Eigen::Vector3d, 4, 1 > fPos
Foot-point positions.
Definition: hierarchical_optimization_controller.h:308
HierarchicalOptimizationControl::PublishQueueThread
void PublishQueueThread()
The PublishQueueThread function is a ROS helper function that publish state messages.
Definition: hierarchical_optimization_controller.cpp:1315
HierarchicalOptimizationControl::jointStatePub
ros::Publisher jointStatePub
ROS Joint State Publisher.
Definition: hierarchical_optimization_controller.h:329
HierarchicalOptimizationControl::HierarchicalLeastSquareOptimization
Eigen::Matrix< double, Eigen::Dynamic, 1 > HierarchicalLeastSquareOptimization(const Eigen::Matrix< Eigen::MatrixXd, Eigen::Dynamic, 1 > &_A, const Eigen::Matrix< Eigen::Matrix< double, Eigen::Dynamic, 1 >, Eigen::Dynamic, 1 > &_b, const int &_v=0)
The HierarchicalLeastSquareOptimization function finds the optimal solution for a set of linear equal...
Definition: hierarchical_optimization_controller.cpp:880
HierarchicalOptimizationControl::InitRos
void InitRos()
The InitRos function is called to initialize ROS.
Definition: hierarchical_optimization_controller.cpp:1325
HierarchicalOptimizationControl::OnGenVelMsg
void OnGenVelMsg(const std_msgs::Float64MultiArrayConstPtr &_msg)
The OnGenVelMsg function handles an incoming generalized velocities message from ROS.
Definition: hierarchical_optimization_controller.cpp:1273
HierarchicalOptimizationControl::genCoordSub
ros::Subscriber genCoordSub
ROS Generalized Coordinates Subscriber.
Definition: hierarchical_optimization_controller.h:320
HierarchicalOptimizationControl::rosPublishQueue
ros::CallbackQueue rosPublishQueue
ROS callbackque that helps publish messages.
Definition: hierarchical_optimization_controller.h:343
HierarchicalOptimizationControl::basePoseCmdPub
ros::Publisher basePoseCmdPub
ROS Base Position Command Publisher.
Definition: hierarchical_optimization_controller.h:333
HierarchicalOptimizationControl::contactStateSub
ros::Subscriber contactStateSub
ROS Contact State Subscriber.
Definition: hierarchical_optimization_controller.h:326
HierarchicalOptimizationControl::baseTwistCmdPub
ros::Publisher baseTwistCmdPub
ROS Base Position Command Publisher.
Definition: hierarchical_optimization_controller.h:337
Kinematics::GetTranslationJacobianInW
Eigen::Matrix< double, 3, 18 > GetTranslationJacobianInW(const LegType &_leg, const BodyType &_body, const Eigen::Matrix< double, 18, 1 > &_q)
The GetTranslationJacobianInW function returns the 3x18 spatial translation Jacobian mapping generali...
Definition: kinematics.cpp:911
Kinematics::GetContactJacobianInW
Eigen::Matrix< double, Eigen::Dynamic, 18 > GetContactJacobianInW(std::vector< LegType > &_legs, const Eigen::Matrix< double, 18, 1 > &_q)
The GetContactJacobianInW function returns the (3*n_c)x18 support Jacobian mapping reaction forces at...
Definition: kinematics.cpp:2104
Kinematics::GetMassMatrix
Eigen::Matrix< double, 18, 18 > GetMassMatrix(const Eigen::Matrix< double, 18, 1 > &_q)
The GetMassMatrix function returns the mass matrix for the floating base system.
Definition: kinematics.cpp:2330
Kinematics::GetTranslationJacobianInCDiff
Eigen::Matrix< double, 3, 18 > GetTranslationJacobianInCDiff(const LegType &_leg, const BodyType &_body, const Eigen::Matrix< double, 18, 1 > &_q, const Eigen::Matrix< double, 18, 1 > &_u)
The GetTranslationJacobianInCDiff function returns the time derivative of the 3x18 spatial translatio...
Definition: kinematics.cpp:1337
Kinematics::GetRotationMatrixWToC
Eigen::Matrix< double, 3, 3 > GetRotationMatrixWToC(const double &_roll, const double &_pitch, const double &_yaw)
The GetRotationMatrixWToC function returns the Euler Angles ZYX rotation matrix from World to Control...
Definition: kinematics.cpp:574
Kinematics::SolveForwardKinematics
bool SolveForwardKinematics(const GeneralizedCoordinates &_q, FootstepPositions &_f_pos)
The SolveForwardKinematics function calculates the Forward Kinematics, i.e. maps joint angles in Join...
Definition: kinematics.cpp:165
Kinematics::GetGravitationalTerms
Eigen::Matrix< double, 18, 1 > GetGravitationalTerms(const Eigen::Matrix< double, 18, 1 > &_q)
The GetGravitationalTerms function returns the gravitational terms for the floating base system.
Definition: kinematics.cpp:2543
Kinematics::GetContactJacobianInWDiff
Eigen::Matrix< double, Eigen::Dynamic, 18 > GetContactJacobianInWDiff(std::vector< LegType > &_legs, const Eigen::Matrix< double, 18, 1 > &_q, const Eigen::Matrix< double, 18, 1 > &_u)
The GetContactJacobianInWDiff function returns the time derivative of the (3*n_c)x18 support Jacobian...
Definition: kinematics.cpp:2156
Kinematics::GetTranslationJacobianInC
Eigen::Matrix< double, 3, 18 > GetTranslationJacobianInC(const LegType &_leg, const BodyType &_body, const Eigen::Matrix< double, 18, 1 > &_q)
The GetTranslationJacobianInC function returns the 3x18 spatial translation Jacobian mapping generali...
Definition: kinematics.cpp:969
Kinematics::GetRotationJacobianInC
Eigen::Matrix< double, 3, 18 > GetRotationJacobianInC(const LegType &_leg, const BodyType &_body, const Eigen::Matrix< double, 18, 1 > &_q)
The GetRotationJacobianInC function returns the 3x18 spatial rotation Jacobian mapping generalized ve...
Definition: kinematics.cpp:1661
Kinematics::GetRotationJacobianInCDiff
Eigen::Matrix< double, 3, 18 > GetRotationJacobianInCDiff(const LegType &_leg, const BodyType &_body, const Eigen::Matrix< double, 18, 1 > &_q, const Eigen::Matrix< double, 18, 1 > &_u)
The GetRotationJacobianInCDiff function returns the time derivative of the 3x18 control frame Rotatio...
Definition: kinematics.cpp:1991
Kinematics::LegType
LegType
Leg type enumerator.
Definition: kinematics.h:56
Kinematics::GetCoriolisAndCentrifugalTerms
Eigen::Matrix< double, 18, 1 > GetCoriolisAndCentrifugalTerms(const Eigen::Matrix< double, 18, 1 > &_q, const Eigen::Matrix< double, 18, 1 > &_u)
The GetCoriolisAndCentrifugalTerms function returns the coriolis and centrifugal terms for the floati...
Definition: kinematics.cpp:2419
hierarchical_optimization_controller.h
Vector3d
Eigen::Vector3d Vector3d
Definition: kinematics.h:49
calculate_polynomials.t
t
Definition: calculate_polynomials.py:41
calculate_polynomials.b
tuple b
Definition: calculate_polynomials.py:27
calculate_polynomials.c
int c
Definition: calculate_polynomials.py:28
calculate_polynomials.x
x
Definition: calculate_polynomials.py:42
debug_utils::printGeneralizedAccelerations
void printGeneralizedAccelerations(const Eigen::Matrix< double, 18, 1 > &_gen_accel)
The printGeneralizedAccelerations function prints a given set of generalized accelerations to ROS usi...
Definition: debug_utils.cpp:162
debug_utils::printJointTorques
void printJointTorques(const Eigen::Matrix< double, 12, 1 > &_joint_trq)
The printJointTorques function prints given joint- torques to ROS using print level INFO.
Definition: debug_utils.cpp:129
debug_utils::printFootstepForces
void printFootstepForces(const int *_contact_state, const Eigen::VectorXd &_F_c)
The printFootstepPositions function prints a given set of footstep forces to ROS using print level IN...
Definition: debug_utils.cpp:196
math_utils::degToRad
double degToRad(const double &_deg)
The degToRad function converts degrees to radians.
Definition: angle_utils.cpp:33
math_utils::SVDNullSpaceProjector
Eigen::Matrix< typename Matrix_TypeA::Scalar, Eigen::Dynamic, Eigen::Dynamic > SVDNullSpaceProjector(const Matrix_TypeA &_A)
The SVDNullSpaceProjector function calculates the null-space projector of a given matrix....
Definition: linalg_utils.h:144
math_utils::INF
static constexpr double INF
Define infinity.
Definition: const_utils.h:33
HierarchicalOptimizationControl::Task::has_eq_constraint
bool has_eq_constraint
Bool indicating whether the task has a linear equality constraint.
Definition: hierarchical_optimization_controller.h:68
HierarchicalOptimizationControl::Task::A_ineq
Eigen::MatrixXd A_ineq
Linear inequality constraint matrix A.
Definition: hierarchical_optimization_controller.h:72
HierarchicalOptimizationControl::Task::b_ineq
Eigen::VectorXd b_ineq
Linear inequality constraint vector b.
Definition: hierarchical_optimization_controller.h:73
HierarchicalOptimizationControl::Task::b_eq
Eigen::VectorXd b_eq
Linear equality constraint vector b.
Definition: hierarchical_optimization_controller.h:71
HierarchicalOptimizationControl::Task::has_ineq_constraint
bool has_ineq_constraint
Bool indicating whether the task has a linear inequality constraint.
Definition: hierarchical_optimization_controller.h:69
HierarchicalOptimizationControl::Task::A_eq
Eigen::MatrixXd A_eq
Linear equality constraint matrix A.
Definition: hierarchical_optimization_controller.h:70