From 01cffa0ce1ea6b57dd1161391256d44b7fbe4bb2 Mon Sep 17 00:00:00 2001
From: Ale <alessandroassirell98@gmail.com>
Date: Mon, 22 Aug 2022 11:50:47 +0200
Subject: [PATCH] fix results export issue
---
config/walk_parameters.yaml | 2 +-
.../quadruped_reactive_walking/WB_MPC/CasadiOCP.py | 2 +-
.../WB_MPC/ProblemData.py | 6 +++---
python/quadruped_reactive_walking/WB_MPC_Wrapper.py | 13 ++++++-------
4 files changed, 11 insertions(+), 12 deletions(-)
diff --git a/config/walk_parameters.yaml b/config/walk_parameters.yaml
index a921b352..ccb0c3f4 100644
--- a/config/walk_parameters.yaml
+++ b/config/walk_parameters.yaml
@@ -38,7 +38,7 @@ robot:
# Parameters of Gait
N_periods: 1
gait: [20, 1, 0, 0, 1,
- 20, 0, 1, 1, 0] # Initial gait matrix
+ 20, 1, 1, 1, 1] # Initial gait matrix
# Parameters of Joystick
gp_alpha_vel: 0.003 #Â Coefficient of the low pass filter applied to gamepad velocity
diff --git a/python/quadruped_reactive_walking/WB_MPC/CasadiOCP.py b/python/quadruped_reactive_walking/WB_MPC/CasadiOCP.py
index 21c5e30d..885cc955 100644
--- a/python/quadruped_reactive_walking/WB_MPC/CasadiOCP.py
+++ b/python/quadruped_reactive_walking/WB_MPC/CasadiOCP.py
@@ -211,7 +211,7 @@ class OCP:
self.results.a = acs_sol
self.results.u = us_sol
self.results.fs = fsol_to_ws
- return self.results
+ return self.results.x, self.results.a, self.results.u, self.results.fs, 0
def get_feet_position(self, xs_sol):
""" Get the feet positions computed durnig one ocp
diff --git a/python/quadruped_reactive_walking/WB_MPC/ProblemData.py b/python/quadruped_reactive_walking/WB_MPC/ProblemData.py
index 77127abf..c5504935 100644
--- a/python/quadruped_reactive_walking/WB_MPC/ProblemData.py
+++ b/python/quadruped_reactive_walking/WB_MPC/ProblemData.py
@@ -92,14 +92,14 @@ class ProblemData(problemDataAbstract):
# Cost function weights
# Cost function weights
self.mu = 0.7
- self.base_velocity_tracking_w = 1e2
- self.ground_collision_w = 1e3
+ self.base_velocity_tracking_w = 1e2*0
+ self.ground_collision_w = 1e3*0
self.friction_cone_w = 1e4
self.impact_altitude_w= 1e3
self.impact_velocity_w = 1e3
- self.control_bound_w = 1e3 * 0
+ self.control_bound_w = 1e3*0
self.control_reg_w = 1e0
self.state_reg_w = np.array([0] * 3
+ [1e1] * 3
diff --git a/python/quadruped_reactive_walking/WB_MPC_Wrapper.py b/python/quadruped_reactive_walking/WB_MPC_Wrapper.py
index 5e2b5ab0..2f93c1a1 100644
--- a/python/quadruped_reactive_walking/WB_MPC_Wrapper.py
+++ b/python/quadruped_reactive_walking/WB_MPC_Wrapper.py
@@ -16,7 +16,7 @@ class Result:
self.xs = list(np.zeros((pd.T + 1, pd.nx)))
self.us = list(np.zeros((pd.T, pd.nu)))
self.acs = list(np.zeros((pd.T, pd.nv)))
- self.fs = list(np.zeros((pd.T, pd.nu)))
+ self.fs = list(np.zeros((pd.T, 12)))
self.K = list(np.zeros([pd.T, pd.nu, pd.ndx]))
self.solving_duration = 0.0
self.new_result = False
@@ -98,12 +98,11 @@ class MPC_Wrapper:
Run the MPC (synchronous version)
"""
self.ocp.solve(x0, tasks, gait, guess)
- (
- self.last_available_result.xs,
- self.last_available_result.us,
- self.last_available_result.K,
- self.last_available_result.solving_duration,
- ) = self.ocp.get_results()
+ self.last_available_result.xs,\
+ self.last_available_result.acs,\
+ self.last_available_result.us,\
+ self.last_available_result.fs,\
+ self.last_available_result.solving_duration = self.ocp.get_results()
self.new_result.value = True
def run_MPC_asynchronous(self, k, x0, tasks, gait, xs, us):
--
GitLab