From 3e7bf634d5a530a5906a2f3f111603866ef66def Mon Sep 17 00:00:00 2001
From: Ale <alessandroassirell98@gmail.com>
Date: Wed, 27 Jul 2022 12:27:03 +0200
Subject: [PATCH] 1KHz mpc (reduced model) Closed loop EndEff trajectory follow
Impedance + Feedforward control
---
python/quadruped_reactive_walking/Controller.py | 4 ++--
python/quadruped_reactive_walking/WB_MPC/ProblemData.py | 2 +-
python/quadruped_reactive_walking/WB_MPC/Target.py | 6 +++---
3 files changed, 6 insertions(+), 6 deletions(-)
diff --git a/python/quadruped_reactive_walking/Controller.py b/python/quadruped_reactive_walking/Controller.py
index 961e6731..1d1d16a1 100644
--- a/python/quadruped_reactive_walking/Controller.py
+++ b/python/quadruped_reactive_walking/Controller.py
@@ -122,7 +122,7 @@ class Controller:
# self.result.P = np.array(self.params.Kp_main.tolist() * 4)
self.result.P = np.array([5] * 3 + [1] * 3 + [5] * 6)
# self.result.D = np.array(self.params.Kd_main.tolist() * 4)
- self.result.D = np.array([0.3] * 3 + [0.5] * 3 + [0.3] * 6)
+ self.result.D = np.array([0.3] * 3 + [0.01] * 3 + [0.3] * 6)
tauFF = self.mpc_result.u[0]
self.result.FF = self.params.Kff_main * np.array(
[0] * 3 + list(tauFF) + [0] * 6
@@ -185,7 +185,7 @@ class Controller:
elif (np.abs(m["vj_m"]) > 1000 * np.pi / 180).any():
print("-- VELOCITY TOO HIGH ERROR --")
print(m["vj_m"])
- print(np.abs(m["vj_m"]) > 500 * np.pi / 180)
+ print(np.abs(m["vj_m"]) > 1000 * np.pi / 180)
self.error = True
elif (np.abs(self.result.FF) > 3.2).any():
print("-- FEEDFORWARD TORQUES TOO HIGH ERROR --")
diff --git a/python/quadruped_reactive_walking/WB_MPC/ProblemData.py b/python/quadruped_reactive_walking/WB_MPC/ProblemData.py
index 3c44ceaf..a7c25043 100644
--- a/python/quadruped_reactive_walking/WB_MPC/ProblemData.py
+++ b/python/quadruped_reactive_walking/WB_MPC/ProblemData.py
@@ -117,7 +117,7 @@ class ProblemDataFull(problemDataAbstract):
#self.friction_cone_w = 1e3 * 0
self.control_bound_w = 1e3
self.control_reg_w = 1e0
- self.state_reg_w = np.array([1e-3] * 3 + [2*1e-1]*3)
+ self.state_reg_w = np.array([1e-5] * 3 + [ 3* 1e-1]*3)
self.terminal_velocity_w = np.array([0] * 3 + [1e3] * 3 )
self.q0_reduced = self.q0[10:13]
diff --git a/python/quadruped_reactive_walking/WB_MPC/Target.py b/python/quadruped_reactive_walking/WB_MPC/Target.py
index 881e081f..f86a80af 100644
--- a/python/quadruped_reactive_walking/WB_MPC/Target.py
+++ b/python/quadruped_reactive_walking/WB_MPC/Target.py
@@ -29,9 +29,9 @@ class Target:
pin.updateFramePlacements(pd.model, pd.rdata)
self.FR_foot0 = pd.rdata.oMf[pd.rfFootId].translation.copy()
self.A = np.array([0, 0.03, 0.03])
- self.offset = np.array([0.08, 0, 0.06])
- self.freq = np.array([0, 1, 1])
- self.phase = np.array([0, 0, np.pi / 2])
+ self.offset = np.array([0.04, 0, 0.06])
+ self.freq = np.array([0, 0.5, 0.5])
+ self.phase = np.array([0, np.pi / 2, 0])
def patternToId(self, gait):
return tuple(self.pd.allContactIds[i] for i, c in enumerate(gait) if c == 1)
--
GitLab