From bb2f10897372ed3e65544ff6b032fd9445dff700 Mon Sep 17 00:00:00 2001
From: Ale <alessandroassirell98@gmail.com>
Date: Wed, 10 Aug 2022 09:11:38 +0200
Subject: [PATCH] rising foot
---
.../quadruped_reactive_walking/Controller.py | 68 ++++++++-----------
.../WB_MPC/CrocoddylOCP.py | 2 +-
2 files changed, 30 insertions(+), 40 deletions(-)
diff --git a/python/quadruped_reactive_walking/Controller.py b/python/quadruped_reactive_walking/Controller.py
index b6496bab..5478d284 100644
--- a/python/quadruped_reactive_walking/Controller.py
+++ b/python/quadruped_reactive_walking/Controller.py
@@ -37,17 +37,16 @@ class Interpolator:
self.update(x0, x0)
def update(self, x0, x1, x2=None):
- self.q0 = x0[:3]
- self.q1 = x1[:3]
- self.v0 = x0[3:]
- self.v1 = x1[3:]
+ self.q0 = x0[:12]
+ self.q1 = x1[:12]
+ self.v0 = x0[12:]
+ self.v1 = x1[12:]
if self.type == 0: # Linear
self.alpha = 0.0
self.beta = self.v1
self.gamma = self.q0
elif self.type == 1: # Quadratic fixed velocity
- self.alpha = 2 * (self.q1 - self.q0 -
- self.v0 * self.dt) / self.dt**2
+ self.alpha = 2 * (self.q1 - self.q0 - self.v0 * self.dt) / self.dt**2
self.beta = self.v0
self.gamma = self.q0
elif self.type == 2: # Quadratic time variable
@@ -70,8 +69,8 @@ class Interpolator:
from scipy.interpolate import KroghInterpolator
if x2 is not None:
- self.q2 = x2[:3]
- self.v2 = x2[3:]
+ self.q2 = x2[:12]
+ self.v2 = x2[12:]
self.y = [self.q0, self.v0, self.q1, self.v1, self.q2, self.v2]
self.krog = KroghInterpolator(self.ts, np.array(self.y))
else:
@@ -103,8 +102,7 @@ class Interpolator:
plt.scatter(y=self.q0[i], x=0.0, color="violet", marker="+")
plt.scatter(y=self.q1[i], x=self.dt, color="violet", marker="+")
if self.type == 3 and self.q2 is not None:
- plt.scatter(y=self.q2[i], x=2 * self.dt,
- color="violet", marker="+")
+ plt.scatter(y=self.q2[i], x=2 * self.dt, color="violet", marker="+")
plt.subplot(3, 2, (i * 2) + 2)
plt.title("Velocity interpolation")
@@ -112,12 +110,10 @@ class Interpolator:
plt.scatter(y=self.v0[i], x=0.0, color="violet", marker="+")
plt.scatter(y=self.v1[i], x=self.dt, color="violet", marker="+")
if self.type == 3 and self.v2 is not None:
- plt.scatter(y=self.v2[i], x=2 * self.dt,
- color="violet", marker="+")
+ plt.scatter(y=self.v2[i], x=2 * self.dt, color="violet", marker="+")
plt.show()
-
class DummyDevice:
def __init__(self):
self.imu = self.IMU()
@@ -213,25 +209,14 @@ class Controller:
try:
footstep = self.target.footstep(self.pd.T)
- # self.mpc.solve(self.k, m["x_m"], self.xs_init, self.us_init)
- if self.initialized:
- self.mpc.solve(
- self.k,
- self.mpc_result.xs[1],
- footstep,
- self.gait,
- self.xs_init,
- self.us_init,
- )
- else:
- self.mpc.solve(
- self.k,
- m["x_m"],
- footstep,
- self.gait,
- self.xs_init,
- self.us_init,
- )
+ self.mpc.solve(
+ self.k,
+ m["x_m"],
+ footstep,
+ self.gait,
+ self.xs_init,
+ self.us_init,
+ )
except ValueError:
self.error = True
print("MPC Problem")
@@ -241,7 +226,8 @@ class Controller:
if not self.error:
self.mpc_result = self.mpc.get_latest_result()
- xs = self.mpc_result.xs
+ xs = np.array(self.mpc_result.xs)
+ xs_actuated = np.concatenate([xs[:, 7:19], xs[:, 19+6:]], axis=1)
if self.mpc_result.new_result:
self.mpc_solved = True
self.k_new = self.k
@@ -257,13 +243,17 @@ class Controller:
# if self.params.interpolate_mpc:
# if self.mpc_result.new_result:
# if self.params.interpolation_type == 3:
- # self.interpolator.update(xs[0], xs[1], xs[2])
+ # self.interpolator.update(
+ # xs_actuated[0], xs_actuated[1], xs_actuated[2])
# # self.interpolator.plot(self.pd.mpc_wbc_ratio, self.pd.dt_wbc)
# t = (self.k - self.k_solve + 1) * self.pd.dt_wbc
# q, v = self.interpolator.interpolate(t)
# else:
# q, v = self.integrate_x(m)
+ # q = q[7: ]
+ # v = v[6 :]
+
q = xs[1][7: self.pd.nq]
v = xs[1][6 + self.pd.nq:]
@@ -436,7 +426,7 @@ class Controller:
]
)
# x_diff = self.mpc_result.xs[0] - m["x_m"]
- tau = self.mpc_result.us[0] + np.dot(self.mpc_result.K[0], x_diff)
+ tau = self.mpc_result.us[0] - np.dot(self.mpc_result.K[0], x_diff)
# tau = self.mpc_result.us[0]
return tau
@@ -445,14 +435,14 @@ class Controller:
Integrate the position and velocity using the acceleration computed from the
feedforward torque
"""
- q0 = m["qj_m"][3:6].copy()
- v0 = m["vj_m"][3:6].copy()
- tau = self.result.tau_ff[3:6].copy()
+ q0 = m["x_m"].copy()[: 19]
+ v0 = m["x_m"].copy()[19 :]
+ tau = np.concatenate([np.zeros(6), self.result.tau_ff.copy()])
a = pin.aba(self.pd.model, self.pd.rdata, q0, v0, tau)
v = v0 + a * self.params.dt_wbc
- q = q0 + v * self.params.dt_wbc
+ q = pin.integrate(self.pd.model, q0, v * self.params.dt_wbc)
return q, v
diff --git a/python/quadruped_reactive_walking/WB_MPC/CrocoddylOCP.py b/python/quadruped_reactive_walking/WB_MPC/CrocoddylOCP.py
index 481fd703..feda43db 100644
--- a/python/quadruped_reactive_walking/WB_MPC/CrocoddylOCP.py
+++ b/python/quadruped_reactive_walking/WB_MPC/CrocoddylOCP.py
@@ -11,7 +11,7 @@ from time import time
class OCP:
def __init__(self, pd: ProblemData, footsteps, gait):
self.pd = pd
- self.max_iter = 1000
+ self.max_iter = 100
self.state = crocoddyl.StateMultibody(self.pd.model)
self.initialized = False
--
GitLab