From 4b8b2b0fdde2a0c343b0125e7918785b34b44657 Mon Sep 17 00:00:00 2001
From: odri <odri@furano.laas.fr>
Date: Thu, 28 Jul 2022 15:32:56 +0200
Subject: [PATCH] Third experiment : CL + FF + dt = 1.4ms
---
config/walk_parameters.yaml | 2 +-
.../quadruped_reactive_walking/Controller.py | 18 +++++++++---------
.../main_solo12_control.py | 2 +-
3 files changed, 11 insertions(+), 11 deletions(-)
diff --git a/config/walk_parameters.yaml b/config/walk_parameters.yaml
index c06099e0..3fb5c80b 100644
--- a/config/walk_parameters.yaml
+++ b/config/walk_parameters.yaml
@@ -6,7 +6,7 @@ robot:
LOGGING: true # Enable/disable logging during the experiment
PLOTTING: true # Enable/disable automatic plotting at the end of the experiment
DEMONSTRATION: false # Enable/disable demonstration functionalities
- SIMULATION: true # Enable/disable PyBullet simulation or running on real robot
+ SIMULATION: false # Enable/disable PyBullet simulation or running on real robot
enable_pyb_GUI: false # Enable/disable PyBullet GUI
envID: 0 # Identifier of the environment to choose in which one the simulation will happen
use_flat_plane: true # If True the ground is flat, otherwise it has bumps
diff --git a/python/quadruped_reactive_walking/Controller.py b/python/quadruped_reactive_walking/Controller.py
index 27b4ba04..12f9b271 100644
--- a/python/quadruped_reactive_walking/Controller.py
+++ b/python/quadruped_reactive_walking/Controller.py
@@ -62,13 +62,15 @@ class Controller:
self.pd = pd
self.target = target
self.point_target = []
+ self.params = params
+ self.q_init = pd.q0
self.k = 0
self.error = False
self.initialized = False
self.result = Result(params)
- self.params = params
- self.q_init = pd.q0
+ self.q = self.pd.q0[7:].copy()
+ self.v = self.pd.v0[6:].copy()
device = DummyDevice()
device.joints.positions = q_init
@@ -128,18 +130,16 @@ class Controller:
# print("Initial guess saved")
# Keep only the actuated joints and set the other to default values
- self.q = self.pd.q0[7:].copy()
- self.v = self.pd.v0[6:].copy()
+
self.q[3:6] = np.array(self.mpc_result.xs)[1, :self.pd.nq]
self.v[3:6] = np.array(self.mpc_result.xs)[1, self.pd.nq:]
- self.result.P = np.array(self.params.Kp_main.tolist() * 4)
- self.result.D = np.array(self.params.Kd_main.tolist() * 4)
- self.result.FF = self.params.Kff_main * np.zeros(12)
+ # self.result.P = np.array(self.params.Kp_main.tolist() * 4)
+ # self.result.D = np.array(self.params.Kd_main.tolist() * 4)
+ # self.result.FF = self.params.Kff_main * np.ones(12)
self.result.q_des = self.q
self.result.v_des = self.v
- self.result.tau_ff = np.array(
- [0] * 3 + list(self.mpc_result.us[0]) + [0] * 6)
+ self.result.tau_ff = np.array([0] * 3 + list(self.mpc_result.us[0]) + [0] * 6)
self.xs_init = self.mpc_result.xs[1:] + [self.mpc_result.xs[-1]]
self.us_init = self.mpc_result.us[1:] + [self.mpc_result.us[-1]]
diff --git a/python/quadruped_reactive_walking/main_solo12_control.py b/python/quadruped_reactive_walking/main_solo12_control.py
index 7fe98179..14dc17e2 100644
--- a/python/quadruped_reactive_walking/main_solo12_control.py
+++ b/python/quadruped_reactive_walking/main_solo12_control.py
@@ -111,7 +111,7 @@ def damp_control(device, nb_motors):
device.joints.set_velocity_gains(0.1 * np.ones(nb_motors))
device.joints.set_desired_positions(np.zeros(nb_motors))
device.joints.set_desired_velocities(np.zeros(nb_motors))
- # device.joints.set_torques(np.zeros(nb_motors))
+ device.joints.set_torques(np.zeros(nb_motors))
# Send command to the robot
device.send_command_and_wait_end_of_cycle(params.dt_wbc)
--
GitLab