From b4a4909d227b2cddeeb1ac164217b809b34e81b4 Mon Sep 17 00:00:00 2001
From: odri <odri@furano.laas.fr>
Date: Fri, 29 Oct 2021 14:01:17 +0200
Subject: [PATCH] Increase size of arrays that store non zero coefficients
---
include/qrw/MPC.hpp | 6 +++---
scripts/main_solo12_control.py | 11 ++++++++++-
2 files changed, 13 insertions(+), 4 deletions(-)
diff --git a/include/qrw/MPC.hpp b/include/qrw/MPC.hpp
index ba91198c..5bab3bc8 100644
--- a/include/qrw/MPC.hpp
+++ b/include/qrw/MPC.hpp
@@ -249,17 +249,17 @@ class MPC {
// TODO FOR S ????
// Matrix NK
- const static int size_nz_NK = 5000;
+ const static int size_nz_NK = 15000;
double v_NK_up[size_nz_NK] = {}; // maxtrix NK (upper bound)
double v_NK_low[size_nz_NK] = {}; // maxtrix NK (lower bound)
double v_warmxf[size_nz_NK] = {}; // maxtrix NK (lower bound)
// Matrix P
- const static int size_nz_P = 5000;
+ const static int size_nz_P = 15000;
csc *P; // Compressed Sparse Column matrix
// Matrix Q
- const static int size_nz_Q = 5000;
+ const static int size_nz_Q = 15000;
double Q[size_nz_Q] = {}; // Q is full of zeros
// OSQP solver variables
diff --git a/scripts/main_solo12_control.py b/scripts/main_solo12_control.py
index c86c606c..e7a118c0 100644
--- a/scripts/main_solo12_control.py
+++ b/scripts/main_solo12_control.py
@@ -51,6 +51,15 @@ def put_on_the_floor(device, q_init):
device.parse_sensor_data()
device.send_command_and_wait_end_of_cycle(params.dt_wbc)
+ # Slow increase till 1/4th of mass is supported by each foot
+ duration_increase = 2.0; # in seconds
+ steps = int(duration_increase / params.dt_wbc)
+ tau_ff = np.array([0.0, 0.022, 0.5, 0.0, 0.022, 0.5, 0.0, 0.025, 0.575, 0.0, 0.025, 0.575])
+ for i in range(steps):
+ device.joints.set_torques(tau_ff * i / steps)
+ device.parse_sensor_data()
+ device.send_command_and_wait_end_of_cycle(params.dt_wbc)
+
print("Start the motion.")
@@ -375,7 +384,7 @@ def main():
help='Name of the clone interface that will reproduce the movement of the first one \
(use ifconfig in a terminal), for instance "enp1s0"')
- # os.nice(-20) # Â Set the process to highest priority (from -20 highest to +20 lowest)
+ os.nice(-20) # Â Set the process to highest priority (from -20 highest to +20 lowest)
f, v = control_loop(parser.parse_args().clone) # , np.array([1.5, 0.0, 0.0, 0.0, 0.0, 0.0]))
print(f, v)
quit()
--
GitLab