From e9a1c011dbc5d310b8617335c67c3653a2925dfa Mon Sep 17 00:00:00 2001
From: Ale <alessandroassirell98@gmail.com>
Date: Thu, 7 Jul 2022 15:08:55 +0200
Subject: [PATCH] addeded files to repo
---
config/walk_parameters.yaml | 4 +-
python/CMakeLists.txt | 1 +
.../quadruped_reactive_walking/Controller.py | 44 +++++++++++++++----
.../WB_MPC/CrocoddylOCP.py | 34 +++++++++-----
.../WB_MPC/OcpResult.py | 10 +++++
.../WB_MPC/ProblemData.py | 2 +-
.../WB_MPC_Wrapper.py | 32 ++++++++------
.../main_solo12_control.py | 8 +++-
8 files changed, 96 insertions(+), 39 deletions(-)
create mode 100644 python/quadruped_reactive_walking/WB_MPC/OcpResult.py
diff --git a/config/walk_parameters.yaml b/config/walk_parameters.yaml
index 1aba9ddb..df3b5ffb 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: 180000 # Number of simulated wbc time steps
+ N_SIMULATION: 2000 # 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
@@ -25,7 +25,7 @@ robot:
dt_mpc: 0.02 # 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.0, 3.0, 3.0] # Proportional gains for the PD+
+ Kp_main: [100.0, 100.0, 100.0] # 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+
# Kff_main: 0.0 # Feedforward torques multiplier for the PD+
diff --git a/python/CMakeLists.txt b/python/CMakeLists.txt
index 2674ab28..092f1d00 100644
--- a/python/CMakeLists.txt
+++ b/python/CMakeLists.txt
@@ -31,6 +31,7 @@ set(${PY_NAME}_WB_MPC_PYTHON
ShootingNode.py
ProblemData.py
Target.py
+ OcpResult.py
)
set(${PY_NAME}_TOOLS_PYTHON
diff --git a/python/quadruped_reactive_walking/Controller.py b/python/quadruped_reactive_walking/Controller.py
index 820d5970..b9966a78 100644
--- a/python/quadruped_reactive_walking/Controller.py
+++ b/python/quadruped_reactive_walking/Controller.py
@@ -30,6 +30,8 @@ class DummyDevice:
self.base_position = np.zeros(3)
self.base_position[2] = 0.1944
self.b_base_velocity = np.zeros(3)
+ self.baseState = tuple()
+ self.baseVel = tuple()
class IMU:
def __init__(self):
@@ -57,6 +59,8 @@ class Controller:
self.q_security = np.array([1.2, 2.1, 3.14] * 4)
self.mpc = WB_MPC_Wrapper.MPC_Wrapper(pd, target, params)
+ self.pd = pd
+ self.target = target
self.k = 0
self.error = False
@@ -66,8 +70,8 @@ class Controller:
device = DummyDevice()
device.joints.positions = q_init
- self.compute(device)
-
+ self.guess = {}
+ #self.compute(device)
def compute(self, device, qc=None):
@@ -78,8 +82,10 @@ class Controller:
"""
t_start = time.time()
+ m = self.read_state(device)
+
try:
- self.mpc.solve(self.k, None)
+ self.mpc.solve(self.k, m['x_m'], self.guess)
except ValueError:
self.error = True
print("MPC Problem")
@@ -89,11 +95,15 @@ class Controller:
self.result.P = np.array(self.params.Kp_main.tolist() * 4)
self.result.D = np.array(self.params.Kd_main.tolist() * 4)
- self.result.FF = self.params.Kff_main * np.ones(12)
- self.result.q_des = np.zeros(12)
- self.result.v_des = np.zeros(12)
+ self.result.FF = np.zeros(12)
+ # self.result.FF = self.params.Kff_main * np.ones(12)
+ self.result.q_des = self.mpc_result.q[1]
+ self.result.v_des = self.mpc_result.v[1]
self.result.tau_ff = np.zeros(12)
+ self.guess["xs"] = self.mpc_result.x
+ self.guess["us"] = self.mpc_result.u
+
self.t_wbc = time.time() - t_start
self.clamp_result(device)
@@ -114,7 +124,7 @@ class Controller:
Update position of PyBullet camera on the robot position to do as if it was
attached to the robot
"""
- if self.k > 10 and self.enable_pyb_GUI:
+ if self.k > 10 and self.params.enable_pyb_GUI:
pyb.resetDebugVisualizerCamera(
cameraDistance=0.6,
cameraYaw=45,
@@ -148,10 +158,10 @@ class Controller:
def clamp(self, num, min_value=None, max_value=None):
clamped = False
- if min_value is not None and num <= min_value:
+ if min_value is not None and num.any() <= min_value:
num = min_value
clamped = True
- if max_value is not None and num >= max_value:
+ if max_value is not None and num.any() >= max_value:
num = max_value
clamped = True
return clamped
@@ -208,3 +218,19 @@ class Controller:
self.result.q_des[:] = np.zeros(12)
self.result.v_des[:] = np.zeros(12)
self.result.tau_ff[:] = np.zeros(12)
+
+ def read_state(self, device):
+ qj_m = device.joints.positions
+ vj_m = device.joints.velocities
+ bp_m = self.tuple_to_array(device.baseState)
+ bv_m = self.tuple_to_array(device.baseVel)
+ if self.pd.useFixedBase == 0:
+ x_m = np.concatenate([bp_m, qj_m, bv_m, vj_m])
+ else:
+ x_m = np.concatenate([qj_m[3:6], vj_m[3:6]])
+
+ return {'qj_m': qj_m, 'vj_m': vj_m, 'x_m': x_m}
+
+ def tuple_to_array(self, tup):
+ a = np.array([element for tupl in tup for element in tupl])
+ return a
diff --git a/python/quadruped_reactive_walking/WB_MPC/CrocoddylOCP.py b/python/quadruped_reactive_walking/WB_MPC/CrocoddylOCP.py
index b6c2af65..b4d98589 100644
--- a/python/quadruped_reactive_walking/WB_MPC/CrocoddylOCP.py
+++ b/python/quadruped_reactive_walking/WB_MPC/CrocoddylOCP.py
@@ -1,6 +1,7 @@
from tracemalloc import start
from .ProblemData import ProblemData
from .Target import Target
+from .OcpResult import OcpResult
import crocoddyl
import pinocchio as pin
import numpy as np
@@ -11,6 +12,7 @@ class OCP:
self.pd = pd
self.target = target
self.state = crocoddyl.StateMultibody(self.pd.model)
+ self.results = OcpResult()
if pd.useFixedBase == 0:
self.actuation = crocoddyl.ActuationModelFloatingBase(self.state)
else:
@@ -143,9 +145,28 @@ class OCP:
us = guess['us']
print("Using warmstart")
start_time = time()
- self.ddp.solve(xs, us, 1, False)
+ self.ddp.solve(xs, us, 30, False)
print("Solver time: ", time()- start_time, "\n")
+
+ def get_results(self):
+ self.results.x = self.ddp.xs.tolist()
+ self.results.a = self.get_croco_acc()
+ self.results.u = self.ddp.us.tolist()
+ self.results.K = self.ddp.K
+
+ if self.pd.useFixedBase == 0:
+ self.results.q = np.array(self.results.x)[:, 7: self.pd.nq]
+ self.results.v = np.array(self.results.x)[:, self.pd.nq + 6: ]
+ else:
+ self.results.q = self.pd.q0
+ self.results.v = self.pd.q0
+ self.results.q[3:6] = np.array(self.results.x)[:, : self.pd.nq]
+ self.results.v[3:6] = np.array(self.results.x)[:, self.pd.nq :]
+
+
+ return self.results
+
def get_croco_forces(self):
d = self.ddp.problem.runningDatas[0]
cnames = d.differential.multibody.contacts.contacts.todict().keys()
@@ -179,13 +200,4 @@ class OCP:
for m in self.ddp.problem.runningDatas]
return acc
- def get_results(self):
- x = self.ddp.xs.tolist()
- a = self.get_croco_acc()
- u = self.ddp.us.tolist()
- if self.pd.useFixedBase == 0:
- f_ws = self.get_croco_forces_ws()
- else:
- f_ws = []
-
- return None, x, a, u, f_ws, None
+
\ No newline at end of file
diff --git a/python/quadruped_reactive_walking/WB_MPC/OcpResult.py b/python/quadruped_reactive_walking/WB_MPC/OcpResult.py
new file mode 100644
index 00000000..e5bb3895
--- /dev/null
+++ b/python/quadruped_reactive_walking/WB_MPC/OcpResult.py
@@ -0,0 +1,10 @@
+class OcpResult:
+ def __init__(self):
+ self.x = None
+ self.a = None
+ self.u = None
+ self.fs = None
+ self.K = None
+ self.f_world = None
+ self.q = None
+ self.v = None
\ No newline at end of file
diff --git a/python/quadruped_reactive_walking/WB_MPC/ProblemData.py b/python/quadruped_reactive_walking/WB_MPC/ProblemData.py
index 56c59442..1e47aab4 100644
--- a/python/quadruped_reactive_walking/WB_MPC/ProblemData.py
+++ b/python/quadruped_reactive_walking/WB_MPC/ProblemData.py
@@ -87,7 +87,7 @@ class ProblemData(problemDataAbstract):
self.terminal_velocity_w = np.array([0] * 18 + [1e3] * 18 )
self.control_bound_w = 1e3
- self.x0 = np.array([ 0, 0, 0.23289725, 0, 0, 0, 1, 0.1, 0.8, -1.6,
+ self.x0 = np.array([ 0.0, 0.0, 0.2607495, 0, 0, 0, 1, 0.1, 0.8, -1.6,
-0.1, 0.8, -1.6, 0.1, -0.8, 1.6, -0.1, -0.8, 1.6,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]) # x0 got from PyBullet
diff --git a/python/quadruped_reactive_walking/WB_MPC_Wrapper.py b/python/quadruped_reactive_walking/WB_MPC_Wrapper.py
index c0cb92b9..fc2c766d 100644
--- a/python/quadruped_reactive_walking/WB_MPC_Wrapper.py
+++ b/python/quadruped_reactive_walking/WB_MPC_Wrapper.py
@@ -20,7 +20,8 @@ class DataInCtype(Structure):
# TODO add the data exchanged with the OCP
_fields_ = [
("k", ctypes.c_int64),
- ("blop", ctypes.c_double * 12)
+ ("x0", ctypes.c_double * 6),
+ ("guess", ctypes.c_double * 12),
]
@@ -33,6 +34,8 @@ class MPC_Wrapper:
def __init__(self, pd, target, params):
self.initialized = False
self.params = params
+ self.pd = pd
+ self.target = target
n_nodes = 0
@@ -53,7 +56,7 @@ class MPC_Wrapper:
self.last_available_result = np.zeros((24, n_nodes))
self.last_cost = 0.0
- def solve(self, k, inputs):
+ def solve(self, k, x0, guess=None):
"""
Call either the asynchronous MPC or the synchronous MPC depending on the value
of multiprocessing during the creation of the wrapper
@@ -61,10 +64,11 @@ class MPC_Wrapper:
Args:
k (int): Number of inv dynamics iterations since the start of the simulation
"""
+
if self.multiprocessing:
- self.run_MPC_asynchronous(k, inputs)
+ self.run_MPC_asynchronous(k, x0, guess)
else:
- self.run_MPC_synchronous(inputs)
+ self.run_MPC_synchronous(x0, guess)
def get_latest_result(self):
"""
@@ -82,15 +86,15 @@ class MPC_Wrapper:
self.initialized = True
return self.last_available_result, self.last_cost
- def run_MPC_synchronous(self, inputs):
+ def run_MPC_synchronous(self, x0, guess):
"""
Run the MPC (synchronous version)
"""
- self.ocp.solve(inputs)
- self.last_available_result = self.ocp.get_latest_result()
- self.last_cost = self.ocp.retrieve_cost()
+ self.ocp.solve(x0, guess)
+ self.last_available_result = self.ocp.get_results()
+ #self.last_cost = self.ocp.retrieve_cost()
- def run_MPC_asynchronous(self, k, inputs):
+ def run_MPC_asynchronous(self, k, x0, guess):
"""
Run the MPC (asynchronous version)
"""
@@ -98,7 +102,7 @@ class MPC_Wrapper:
p = Process(target=self.MPC_asynchronous)
p.start()
- self.add_new_data(k, inputs)
+ self.add_new_data(k, x0, guess)
def MPC_asynchronous(self):
"""
@@ -108,17 +112,17 @@ class MPC_Wrapper:
if self.newData.value:
self.newData.value = False
- k, inputs = self.decompress_dataIn(self.dataIn)
+ k, x0, guess = self.decompress_dataIn(self.dataIn)
if k == 0:
- loop_ocp = OCP(self.params)
+ loop_ocp = OCP(self.pd, self.target)
- loop_ocp.solve(inputs)
+ loop_ocp.solve(x0, guess)
self.dataOut[:] = loop_ocp.get_latest_result().ravel(order="F")
self.cost.value = loop_ocp.retrieve_cost()
self.newResult.value = True
- def add_new_data(self, k, inputs):
+ def add_new_data(self, k, x0):
"""
Compress data in a C-type structure that belongs to the shared memory to send
data from the main control loop to the asynchronous MPC and notify the process
diff --git a/python/quadruped_reactive_walking/main_solo12_control.py b/python/quadruped_reactive_walking/main_solo12_control.py
index ec137201..23b6accb 100644
--- a/python/quadruped_reactive_walking/main_solo12_control.py
+++ b/python/quadruped_reactive_walking/main_solo12_control.py
@@ -6,12 +6,13 @@ import numpy as np
import quadruped_reactive_walking as qrw
from .Controller import Controller
from .tools.LoggerControl import LoggerControl
-from .WB_MPC.ProblemData import ProblemData
+from .WB_MPC.ProblemData import ProblemData, ProblemDataFull
from .WB_MPC.Target import Target
params = qrw.Params() # Object that holds all controller parameters
-pd = ProblemData(params)
+pd = ProblemDataFull(params)
target = Target(pd)
+target.update(0)
if params.SIMULATION:
from .tools.PyBulletSimulator import PyBulletSimulator
@@ -165,10 +166,12 @@ def control_loop():
k_log_whole = 0
T_whole = time.time()
dT_whole = 0.0
+ cnt = 0
while (not device.is_timeout) and (t < t_max) and (not controller.error):
t_start_whole = time.time()
device.parse_sensor_data()
+ target.update(cnt)
if controller.compute(device, qc):
break
@@ -198,6 +201,7 @@ def control_loop():
t_log_whole[k_log_whole] = t_end_whole - t_start_whole
k_log_whole += 1
+ cnt += 1
# ****************************************************************
finished = t >= t_max
--
GitLab