diff --git a/include/qrw/MPC.hpp b/include/qrw/MPC.hpp index ba91198ca2c3ad93d282c8597df8d68ddad086d0..5bab3bc8cb0e8a73592a5f2ccdaa2f4a3d056830 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 c86c606cf57fb2ded47e19f8aff1a9354a51b6b3..e7a118c09ccd2c69b4e3a8135ad888d05d4434fb 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()