diff --git a/python/quadruped_reactive_walking/Controller.py b/python/quadruped_reactive_walking/Controller.py
index ac963fffed265388620de2d9920b48908bd6b487..41df48b04be4fdcf2a40b67eac85b7d78af8a6a9 100644
--- a/python/quadruped_reactive_walking/Controller.py
+++ b/python/quadruped_reactive_walking/Controller.py
@@ -23,6 +23,79 @@ class Result:
self.tau_ff = np.zeros(12)
+class Interpolation:
+ def __init__(self):
+ pass
+
+ def load_data(self, q, v):
+ self.v0 = v[0, :]
+ self.q0 = q[0, :]
+ self.v1 = v[1, :]
+ self.q1 = q[1, :]
+
+ def interpolate(self, t):
+ # Perfect match, but wrong
+ # if (self.q1-self.q0 == 0).any():
+ # alpha = np.zeros(len(self.q0))
+ # else:
+ # alpha = 2 * 1/2* (self.v1**2 - self.v0**2)/(self.q1 - self.q0)
+
+ # beta = self.v0
+ # gamma = self.q0
+
+ # v_t = beta + alpha * t
+ # q_t = gamma + beta * t + alpha * t**2
+
+ # Linear
+ beta = self.v1
+ gamma = self.q0
+
+ v_t = beta
+ q_t = gamma + beta * t
+
+ # Linear Wrong
+ # beta = self.v1
+ # gamma = self.q0
+
+ # v_t = self.v0 + self.v1*(self.v1 - self.v0)/(self.q1 - self.q0) * t
+ # q_t = self.q0 + self.v1 * t
+
+ # Quadratic
+ # if (self.q1-self.q0 == 0).any():
+ # alpha = np.zeros(len(self.q0))
+ # else:
+ # alpha = self.v1*(self.v1 - self.v0)/(self.q1 - self.q0)
+
+ # beta = self.v0
+ # gamma = self.q0
+
+ # v_t = beta + alpha * t
+ # q_t = gamma + beta * t + 1/2 * alpha * t**2
+
+ return q_t, v_t
+
+ def plot_interpolation(self, n, dt):
+ import matplotlib.pyplot as plt
+ plt.style.use("seaborn")
+ t = np.linspace(0, n*dt, n + 1)
+ q_t = np.array([self.interpolate((i) * dt)[0] for i in range(n+1)])
+ v_t = np.array([self.interpolate((i) * dt)[1] for i in range(n+1)])
+ for i in range(3):
+ plt.subplot(3, 2, (i*2) + 1)
+ plt.title("Position interpolation")
+ plt.plot(t, q_t[:, i])
+ plt.scatter(y=self.q0[i], x=t[0], color="violet", marker="+")
+ plt.scatter(y=self.q1[i], x=t[-1], color="violet", marker="+")
+
+ plt.subplot(3, 2, (i*2) + 2)
+ plt.title("Velocity interpolation")
+ plt.plot(t, v_t[:, i])
+ plt.scatter(y=self.v0[i], x=t[0], color="violet", marker="+")
+ plt.scatter(y=self.v1[i], x=t[-1], color="violet", marker="+")
+
+ plt.show()
+
+
class DummyDevice:
def __init__(self):
self.imu = self.IMU()
@@ -46,8 +119,6 @@ class DummyDevice:
self.velocities = np.zeros(12)
-90
-
class Controller:
def __init__(self, pd, target, params, q_init, t):
@@ -73,6 +144,7 @@ class Controller:
self.cnt_wbc = 0
self.error = False
self.initialized = False
+ self.interpolator = Interpolation()
self.result = Result(params)
self.result.q_des = self.pd.q0[7:].copy()
self.result.v_des = self.pd.v0[6:].copy()
@@ -134,7 +206,14 @@ class Controller:
self.result.tau_ff = np.array([0] * 3 + list(actuated_tau_ff) + [0] * 6)
if self.params.interpolate_mpc:
- q, v = self.interpolate_x(self.cnt_wbc * self.pd.dt_wbc)
+ # load the data to be interpolated only once per mpc solution
+ if self.cnt_wbc == 0:
+ x = np.array(self.mpc_result.xs)
+ self.interpolator.load_data(
+ x[:, : self.pd.nq], x[:, self.pd.nq:])
+
+ q, v = self.interpolator.interpolate((self.cnt_wbc +1) * self.pd.dt_wbc)
+ #self.interpolator.plot_interpolation(self.pd.r1, self.pd.dt_wbc)
else:
q, v = self.integrate_x(m)
@@ -147,8 +226,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()
@@ -186,10 +265,10 @@ class Controller:
print(m["qj_m"])
print(np.abs(m["qj_m"]) > self.q_security)
self.error = True
- elif (np.abs(m["vj_m"]) > 500 * np.pi / 180).any():
+ 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 --")
@@ -285,27 +364,6 @@ class Controller:
tau = self.mpc_result.us[0] + np.dot(self.mpc_result.K[0], x_diff)
return tau
- def interpolate_x(self, t):
- q = np.array(self.mpc_result.xs)[:, : self.pd.nq]
- v = np.array(self.mpc_result.xs)[:, self.pd.nq :]
- v0 = v[0, :]
- q0 = q[0, :]
- v1 = v[1, :]
- q1 = q[1, :]
-
- if (q1 - q0 == 0).any():
- alpha = np.zeros(len(q0))
- else:
- alpha = (v1**2 - v0**2) / (q1 - q0)
-
- beta = v0
- gamma = q0
-
- v_t = beta + alpha * t
- q_t = gamma + beta * t + 1 / 2 * alpha * t**2
-
- return q_t, v_t
-
def integrate_x(self, m):
"""
Integrate the position and velocity using the acceleration computed from the
diff --git a/python/quadruped_reactive_walking/WB_MPC/CrocoddylOCP.py b/python/quadruped_reactive_walking/WB_MPC/CrocoddylOCP.py
index 4a9284569bf55e41f93325b1ebaa153887b0aad7..1eddd689455c48efd83577e373d9f4572ad60cff 100644
--- a/python/quadruped_reactive_walking/WB_MPC/CrocoddylOCP.py
+++ b/python/quadruped_reactive_walking/WB_MPC/CrocoddylOCP.py
@@ -29,7 +29,6 @@ class OCP:
)
self.ddp = crocoddyl.SolverFDDP(self.problem)
-
def initialize_models(self):
self.nodes = []
for t in range(self.pd.T):
@@ -67,7 +66,8 @@ class OCP:
self.target.evaluate_in_t(self.pd.T - 1),
self.target.contactSequence[self.pd.T - 1],
) # model without contact for this task
- self.nodes[0].update_model(self.target.contactSequence[self.pd.T - 1], task)
+ self.nodes[0].update_model(
+ self.target.contactSequence[self.pd.T - 1], task)
t_update_last_model = time()
self.t_update_last_model = t_update_last_model - t_FK
@@ -165,7 +165,8 @@ class OCP:
def get_croco_acc(self):
acc = []
- [acc.append(m.differential.xout) for m in self.ddp.problem.runningDatas]
+ [acc.append(m.differential.xout)
+ for m in self.ddp.problem.runningDatas]
return acc
@@ -181,7 +182,8 @@ class Node:
self.actuation = crocoddyl.ActuationModelFloatingBase(self.state)
else:
self.actuation = crocoddyl.ActuationModelFull(self.state)
- self.control = crocoddyl.ControlParametrizationModelPolyZero(self.actuation.nu)
+ self.control = crocoddyl.ControlParametrizationModelPolyZero(
+ self.actuation.nu)
self.nu = self.actuation.nu
self.createStandardModel(supportFootIds)
@@ -203,7 +205,8 @@ class Node:
self.contactModel = crocoddyl.ContactModelMultiple(self.state, self.nu)
for i in supportFootIds:
supportContactModel = crocoddyl.ContactModel3D(
- self.state, i, np.array([0.0, 0.0, 0.0]), self.nu, np.array([0.0, 0.0])
+ self.state, i, np.array(
+ [0.0, 0.0, 0.0]), self.nu, np.array([0.0, 0.0])
)
self.contactModel.addContact(
self.pd.model.frames[i].name + "_contact", supportContactModel
@@ -212,7 +215,8 @@ class Node:
# Creating the cost model for a contact phase
costModel = crocoddyl.CostModelSum(self.state, self.nu)
- stateResidual = crocoddyl.ResidualModelState(self.state, self.pd.xref, self.nu)
+ stateResidual = crocoddyl.ResidualModelState(
+ self.state, self.pd.xref, self.nu)
stateActivation = crocoddyl.ActivationModelWeightedQuad(
self.pd.state_reg_w**2
)
@@ -235,7 +239,8 @@ class Node:
self.contactModel = crocoddyl.ContactModelMultiple(self.state, self.nu)
for i in supportFootIds:
supportContactModel = crocoddyl.ContactModel3D(
- self.state, i, np.array([0.0, 0.0, 0.0]), self.nu, np.array([0.0, 0.0])
+ self.state, i, np.array(
+ [0.0, 0.0, 0.0]), self.nu, np.array([0.0, 0.0])
)
self.dmodel.contacts.addContact(
self.pd.model.frames[i].name + "_contact", supportContactModel
@@ -243,7 +248,8 @@ class Node:
def make_terminal_model(self):
self.isTerminal = True
- stateResidual = crocoddyl.ResidualModelState(self.state, self.pd.xref, self.nu)
+ stateResidual = crocoddyl.ResidualModelState(
+ self.state, self.pd.xref, self.nu)
stateActivation = crocoddyl.ActivationModelWeightedQuad(
self.pd.terminal_velocity_w**2
)
@@ -274,14 +280,17 @@ class Node:
ctrlReg = crocoddyl.CostModelResidual(self.state, ctrlResidual)
self.costModel.addCost("ctrlReg", ctrlReg, self.pd.control_reg_w)
- ctrl_bound_residual = crocoddyl.ResidualModelControl(self.state, self.nu)
+ ctrl_bound_residual = crocoddyl.ResidualModelControl(
+ self.state, self.nu)
ctrl_bound_activation = crocoddyl.ActivationModelQuadraticBarrier(
- crocoddyl.ActivationBounds(-self.pd.effort_limit, self.pd.effort_limit)
+ crocoddyl.ActivationBounds(-self.pd.effort_limit,
+ self.pd.effort_limit)
)
ctrl_bound = crocoddyl.CostModelResidual(
self.state, ctrl_bound_activation, ctrl_bound_residual
)
- self.costModel.addCost("ctrlBound", ctrl_bound, self.pd.control_bound_w)
+ self.costModel.addCost("ctrlBound", ctrl_bound,
+ self.pd.control_bound_w)
self.tracking_cost(swingFootTask)
diff --git a/python/quadruped_reactive_walking/WB_MPC/ProblemData.py b/python/quadruped_reactive_walking/WB_MPC/ProblemData.py
index 0f4e8f9ace4c9a0c4de6168f187b63dd1deca87f..ea0021cf68faa2388786d1783238c70ea0086889 100644
--- a/python/quadruped_reactive_walking/WB_MPC/ProblemData.py
+++ b/python/quadruped_reactive_walking/WB_MPC/ProblemData.py
@@ -7,7 +7,7 @@ class problemDataAbstract:
self.dt = param.dt_mpc # OCP dt
self.dt_wbc = param.dt_wbc
self.init_steps = 0
- self.target_steps = 50
+ self.target_steps = 150
self.T = self.init_steps + self.target_steps -1
self.robot = erd.load("solo12")
@@ -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-5] * 3 + [ 3* 1e-1]*3)
+ self.state_reg_w = np.array([1e-2] * 3 + [1e0]*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 36fef7e0d7677e0e6114c0cc13d9710cb3f71472..d06cc0178201f9f6bb7814a67316be2f8db9ab4b 100644
--- a/python/quadruped_reactive_walking/WB_MPC/Target.py
+++ b/python/quadruped_reactive_walking/WB_MPC/Target.py
@@ -8,9 +8,9 @@ class Target:
self.pd = pd
self.dt = pd.dt
- self.gait = ([] + \
- [[0, 0, 0, 0]] * pd.init_steps + \
- [[0, 0, 0, 0]] * pd.target_steps )
+ self.gait = ([] +
+ [[0, 0, 0, 0]] * pd.init_steps +
+ [[0, 0, 0, 0]] * pd.target_steps)
self.T = pd.T
self.contactSequence = [self.patternToId(p) for p in self.gait]
diff --git a/python/quadruped_reactive_walking/main_solo12_control.py b/python/quadruped_reactive_walking/main_solo12_control.py
index 4773623a2abfcb4fede9e34dc9d2c409952965da..609b997e05d56499441a37f65260d5deebd0dac7 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)
diff --git a/python/quadruped_reactive_walking/tools/LoggerControl.py b/python/quadruped_reactive_walking/tools/LoggerControl.py
index 7a9ba9786ba5a2c93fe0579b18abf891d4891a61..e0106f5bf4c322b9ef18b8687f761bebd4b3edf6 100644
--- a/python/quadruped_reactive_walking/tools/LoggerControl.py
+++ b/python/quadruped_reactive_walking/tools/LoggerControl.py
@@ -122,7 +122,7 @@ class LoggerControl:
self.t_mpc[self.i] = controller.t_mpc
self.t_send[self.i] = controller.t_send
self.t_loop[self.i] = controller.t_loop
-
+
self.t_ocp_ddp[self.i] = controller.mpc_result.solving_duration
if not self.params.enable_multiprocessing:
self.t_ocp_update[self.i] = controller.mpc.ocp.t_update
@@ -163,7 +163,7 @@ class LoggerControl:
plt.show()
- def plot_states(self, save = False, fileName = '/tmp'):
+ def plot_states(self, save=False, fileName='/tmp'):
import matplotlib.pyplot as plt
legend = ["Hip", "Shoulder", "Knee"]
@@ -199,7 +199,7 @@ class LoggerControl:
if save:
plt.savefig(fileName + "/joint_velocities")
- def plot_torques(self, save=False, fileName = '/tmp'):
+ def plot_torques(self, save=False, fileName='/tmp'):
import matplotlib.pyplot as plt
legend = ["Hip", "Shoulder", "Knee"]
@@ -209,7 +209,8 @@ class LoggerControl:
plt.subplot(2, 2, i + 1)
plt.title("Joint torques of " + str(i))
[
- plt.plot(np.array(self.torquesFromCurrentMeasurment)[:, (3 * i + jj)])
+ plt.plot(np.array(self.torquesFromCurrentMeasurment)
+ [:, (3 * i + jj)])
for jj in range(3)
]
plt.ylabel("Torque [Nm]")
@@ -222,9 +223,11 @@ class LoggerControl:
def plot_target(self, save=False, fileName='/tmp'):
import matplotlib.pyplot as plt
- x_mes = np.concatenate([self.q_mes[:, 3:6], self.v_mes[:, 3:6]], axis=1)
+ x_mes = np.concatenate(
+ [self.q_mes[:, 3:6], self.v_mes[:, 3:6]], axis=1)
- horizon = self.ocp_xs.shape[0]
+ horizon = int(self.ocp_xs.shape[0] / self.pd.r1)
+ t_scale = np.linspace(0, (horizon)*self.pd.dt, (horizon)*self.pd.r1)
x_mpc = [self.ocp_xs[0][0, :]]
[x_mpc.append(x[1, :]) for x in self.ocp_xs[:-1]]
@@ -232,7 +235,8 @@ class LoggerControl:
# Feet positions calcuilated by every ocp
all_ocp_feet_p_log = {
- idx: [get_translation_array(self.pd, x, idx)[0] for x in self.ocp_xs]
+ idx: [get_translation_array(self.pd, self.ocp_xs[i * self.pd.r1], idx)[0]
+ for i in range(horizon)]
for idx in self.pd.allContactIds
}
for foot in all_ocp_feet_p_log:
@@ -263,28 +267,28 @@ class LoggerControl:
if save:
plt.savefig(fileName + "/target")
-
- """ legend = ['x', 'y', 'z']
- plt.figure(figsize=(12, 18), dpi = 90)
- for p in range(3):
- plt.subplot(3,1, p+1)
- plt.title('Free foot on ' + legend[p])
- for i in range(horizon-1):
- t = np.linspace(i*self.pd.dt, (self.pd.T+ i)*self.pd.dt, self.pd.T+1)
- y = all_ocp_feet_p_log[self.pd.rfFootId][i+1][:,p]
- for j in range(len(y) - 1):
- plt.plot(t[j:j+2], y[j:j+2], color='royalblue', linewidth = 3, marker='o' ,alpha=max([1 - j/len(y), 0]))
- plt.plot(self.target[:, p]) """
-
+ # legend = ['x', 'y', 'z']
+ # plt.figure(figsize=(12, 18), dpi = 90)
+ # for p in range(3):
+ # plt.subplot(3,1, p+1)
+ # plt.title('Free foot on ' + legend[p])
+ # plt.plot(t_scale, self.target[:, p], color="tomato")
+ # plt.plot(t_scale, m_feet_p_log[self.pd.rfFootId][:, p], color="lightgreen")
+ # for i in range(horizon-1):
+ # t = np.linspace(i*self.pd.dt, (self.pd.T+ i)*self.pd.dt, self.pd.T+1)
+ # y = all_ocp_feet_p_log[self.pd.rfFootId][i][:,p]
+ # for j in range(len(y) - 1):
+ # plt.plot(t[j:j+2], y[j:j+2], color='royalblue', linewidth = 3, marker='o' ,alpha=max([1 - j/len(y), 0]))
+
def plot_riccati_gains(self, n, save=False, fileName='/tmp'):
import matplotlib.pyplot as plt
# Equivalent Stiffness Damping plots
legend = ["Hip", "Shoulder", "Knee"]
- plt.figure(figsize=(12, 18), dpi = 90)
+ plt.figure(figsize=(12, 18), dpi=90)
for p in range(3):
- plt.subplot(3,1, p+1)
+ plt.subplot(3, 1, p+1)
plt.title('Joint: ' + legend[p])
plt.plot(self.MPC_equivalent_Kp[:, p])
plt.plot(self.MPC_equivalent_Kd[:, p])
@@ -294,9 +298,8 @@ class LoggerControl:
if save:
plt.savefig(fileName + "/diagonal_Riccati_gains")
-
# Riccati gains
- plt.figure(figsize=(12, 18), dpi = 90)
+ plt.figure(figsize=(12, 18), dpi=90)
plt.title("Riccati gains at step: " + str(n))
plt.imshow(self.ocp_K[n])
plt.colorbar()
@@ -306,7 +309,8 @@ class LoggerControl:
def plot_controller_times(self):
import matplotlib.pyplot as plt
- t_range = np.array([k * self.pd.dt for k in range(self.tstamps.shape[0])])
+ t_range = np.array(
+ [k * self.pd.dt for k in range(self.tstamps.shape[0])])
plt.figure()
plt.plot(t_range, self.t_measures, "r+")
@@ -319,11 +323,12 @@ class LoggerControl:
plt.legend(lgd)
plt.xlabel("Time [s]")
plt.ylabel("Time [s]")
-
+
def plot_OCP_times(self):
import matplotlib.pyplot as plt
- t_range = np.array([k * self.pd.dt for k in range(self.tstamps.shape[0])])
+ t_range = np.array(
+ [k * self.pd.dt for k in range(self.tstamps.shape[0])])
plt.figure()
plt.plot(t_range, self.t_ocp_update, "r+")
@@ -339,7 +344,8 @@ class LoggerControl:
def plot_OCP_update_times(self):
import matplotlib.pyplot as plt
- t_range = np.array([k * self.pd.dt for k in range(self.tstamps.shape[0])])
+ t_range = np.array(
+ [k * self.pd.dt for k in range(self.tstamps.shape[0])])
plt.figure()
plt.plot(t_range, self.t_ocp_update_FK, "r+")
@@ -364,9 +370,9 @@ class LoggerControl:
name,
ocp_xs=self.ocp_xs,
ocp_us=self.ocp_us,
- ocp_K = self.ocp_K,
- MPC_equivalent_Kp = self.MPC_equivalent_Kp,
- MPC_equivalent_Kd = self.MPC_equivalent_Kd,
+ ocp_K=self.ocp_K,
+ MPC_equivalent_Kp=self.MPC_equivalent_Kp,
+ MPC_equivalent_Kd=self.MPC_equivalent_Kd,
t_measures=self.t_measures,
t_mpc=self.t_mpc,
t_send=self.t_send,