From 7048fb4b57540d7b033334ddc07affbc5052d139 Mon Sep 17 00:00:00 2001
From: Ale <alessandroassirell98@gmail.com>
Date: Fri, 29 Jul 2022 14:57:07 +0200
Subject: [PATCH] reach point working well
---
config/walk_parameters.yaml | 6 +++---
python/quadruped_reactive_walking/Controller.py | 8 +++++---
python/quadruped_reactive_walking/WB_MPC/ProblemData.py | 2 +-
python/quadruped_reactive_walking/main_solo12_control.py | 2 +-
4 files changed, 10 insertions(+), 8 deletions(-)
diff --git a/config/walk_parameters.yaml b/config/walk_parameters.yaml
index ed301589..a500cddb 100644
--- a/config/walk_parameters.yaml
+++ b/config/walk_parameters.yaml
@@ -11,7 +11,7 @@ robot:
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
predefined_vel: true # If we are using a predefined reference velocity (True) or a joystick (False)
- N_SIMULATION: 10000 # Number of simulated wbc time steps
+ N_SIMULATION: 5000 # Number of simulated wbc time steps
enable_corba_viewer: false # Enable/disable Corba Viewer
enable_multiprocessing: false # Enable/disable running the MPC in another process in parallel of the main loop
perfect_estimator: true # Enable/disable perfect estimator by using data directly from PyBullet
@@ -21,13 +21,13 @@ robot:
# q_init: [0.0, 0.865, -1.583, 0.0, 0.865, -1.583, 0.0, 0.865, -1.583, 0.0, 0.865, -1.583] # h_com = 0.2
# q_init: [0.0, 0.764, -1.407, 0.0, 0.76407, -1.4, 0.0, 0.76407, -1.407, 0.0, 0.764, -1.407] #Â h_com = 0.218
q_init: [0.0, 0.7, -1.4, 0.0, 0.7, -1.4, 0.0, -0.7, 1.4, 0.0, -0.7, 1.4] # Initial articular positions
- dt_wbc: 0.0014 # Time step of the whole body control
+ dt_wbc: 0.001 # Time step of the whole body control
dt_mpc: 0.015 # Time step of the model predictive control
type_MPC: 3 # Which MPC solver you want to use: 0 for OSQP MPC, 1, 2, 3 for Crocoddyl MPCs
# Kp_main: [0.0, 0.0, 0.0] # Proportional gains for the PD+
Kp_main: [3, 3, 3] # Proportional gains for the PD+
# Kd_main: [0., 0., 0.] # Derivative gains for the PD+
- Kd_main: [0.3, 0.3, 0.3] # Derivative gains for the PD+
+ Kd_main: [0.1, 0.1, 0.1] # Derivative gains for the PD+
# Kff_main: 0.0 # Feedforward torques multiplier for the PD+
Kff_main: 1.0 # Feedforward torques multiplier for the PD+
diff --git a/python/quadruped_reactive_walking/Controller.py b/python/quadruped_reactive_walking/Controller.py
index c722aa12..13ba4aab 100644
--- a/python/quadruped_reactive_walking/Controller.py
+++ b/python/quadruped_reactive_walking/Controller.py
@@ -154,7 +154,9 @@ class Controller:
self.result.q_des = self.q
self.result.v_des = self.v
self.result.tau_ff = self.mpc_result.us[0] + np.dot(self.mpc_result.K[0],
- self.mpc.ocp.state.diff(m["x_m"], self.mpc_result.xs[0]))
+ np.concatenate([pin.difference(self.pd.model, m["x_m"][: self.pd.nq],
+ self.mpc_result.xs[0][: self.pd.nq]),
+ m["x_m"][self.pd.nq] - self.mpc_result.xs[0][self.pd.nq:] ]) )
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]]
@@ -162,8 +164,8 @@ class Controller:
t_send = time.time()
self.t_send = t_send - t_mpc
- #self.clamp_result(device)
- #self.security_check(m)
+ self.clamp_result(device)
+ self.security_check(m)
if self.error:
self.set_null_control()
diff --git a/python/quadruped_reactive_walking/WB_MPC/ProblemData.py b/python/quadruped_reactive_walking/WB_MPC/ProblemData.py
index 56420816..a4e769a4 100644
--- a/python/quadruped_reactive_walking/WB_MPC/ProblemData.py
+++ b/python/quadruped_reactive_walking/WB_MPC/ProblemData.py
@@ -118,7 +118,7 @@ class ProblemDataFull(problemDataAbstract):
+ [1e-3] * 3\
+ [1e2] * 6
+ [1e1] * 3 \
- + [3* 1e-1] * 3\
+ + [1e0] * 3\
+ [1e1] * 6
)
self.terminal_velocity_w = np.array([0] * 12 + [1e3] * 12 )
diff --git a/python/quadruped_reactive_walking/main_solo12_control.py b/python/quadruped_reactive_walking/main_solo12_control.py
index 56eb9b99..e5535345 100644
--- a/python/quadruped_reactive_walking/main_solo12_control.py
+++ b/python/quadruped_reactive_walking/main_solo12_control.py
@@ -85,7 +85,7 @@ def check_position_error(device, controller):
device (robot wrapper): a wrapper to communicate with the robot
controller (array): the controller storing the desired position
"""
- if np.max(np.abs(controller.result.q_des - device.joints.positions)) > 15:
+ if np.max(np.abs(controller.result.q_des - device.joints.positions)) > 0.15:
print("DIFFERENCE: ", controller.result.q_des - device.joints.positions)
print("q_des: ", controller.result.q_des)
print("q_mes: ", device.joints.positions)
--
GitLab