From 76042cc540edbaaea401e52ff2438b5f6bbeeadf Mon Sep 17 00:00:00 2001
From: Ale <alessandroassirell98@gmail.com>
Date: Tue, 9 Aug 2022 09:15:56 +0200
Subject: [PATCH] preparing whole body
---
config/walk_parameters.yaml | 4 +-
.../quadruped_reactive_walking/Controller.py | 15 ++-
.../WB_MPC/CrocoddylOCP.py | 5 +-
.../WB_MPC/ProblemData.py | 98 +++++--------------
.../WB_MPC/Target.py | 2 +-
.../main_solo12_control.py | 2 +-
6 files changed, 40 insertions(+), 86 deletions(-)
diff --git a/config/walk_parameters.yaml b/config/walk_parameters.yaml
index 5e51ab10..f670bd48 100644
--- a/config/walk_parameters.yaml
+++ b/config/walk_parameters.yaml
@@ -13,7 +13,7 @@ robot:
predefined_vel: true # If we are using a predefined reference velocity (True) or a joystick (False)
N_SIMULATION: 10000 # Number of simulated wbc time steps
enable_corba_viewer: false # Enable/disable Corba Viewer
- enable_multiprocessing: true # Enable/disable running the MPC in another process in parallel of the main loop
+ 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
# General control parameters
@@ -25,7 +25,7 @@ robot:
dt_mpc: 0.015 # 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
save_guess: false # true to interpolate the impedance quantities between nodes of the MPC
- interpolate_mpc: true # true to interpolate the impedance quantities between nodes of the MPC
+ interpolate_mpc: false # true to interpolate the impedance quantities between nodes of the MPC
interpolation_type: 3 # 0,1,2,3 decide which kind of interpolation is used
# Kp_main: [0.0, 0.0, 0.0] # Proportional gains for the PD+
# Kd_main: [0., 0., 0.] # Derivative gains for the PD+
diff --git a/python/quadruped_reactive_walking/Controller.py b/python/quadruped_reactive_walking/Controller.py
index 7986e453..190ef617 100644
--- a/python/quadruped_reactive_walking/Controller.py
+++ b/python/quadruped_reactive_walking/Controller.py
@@ -122,8 +122,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()
+ self.baseState = ((0.0, 0.0, 0.2607495), (0.0, 0.0, 0.0, 1.0))
+ self.baseVel = ((0.0, 0.0, 0.0), (0.0, 0.0, 0.0))
class IMU:
def __init__(self):
@@ -138,6 +138,7 @@ class DummyDevice:
self.velocities = np.zeros(12)
+
class Controller:
def __init__(self, pd, params, q_init, t):
"""
@@ -171,7 +172,7 @@ class Controller:
self.k_solve = 0
if self.params.interpolate_mpc:
self.interpolator = Interpolator(
- params, np.concatenate([self.result.q_des[3:6], self.result.v_des[3:6]])
+ params, np.concatenate([self.result.q_des, self.result.v_des])
)
try:
file = np.load("/tmp/init_guess.npy", allow_pickle=True).item()
@@ -404,10 +405,16 @@ class Controller:
)
print("Initial guess saved")
+ def tuple_to_array(self, tup):
+ a = np.array([element for tupl in tup for element in tupl])
+ return a
+
def read_state(self, device):
qj_m = device.joints.positions
vj_m = device.joints.velocities
- x_m = np.concatenate([qj_m, vj_m])
+ bp_m = self.tuple_to_array(device.baseState)
+ bv_m = self.tuple_to_array(device.baseVel)
+ x_m = np.concatenate([bp_m, qj_m, bv_m, vj_m])
return {"qj_m": qj_m, "vj_m": vj_m, "x_m": x_m}
def compute_torque(self, m):
diff --git a/python/quadruped_reactive_walking/WB_MPC/CrocoddylOCP.py b/python/quadruped_reactive_walking/WB_MPC/CrocoddylOCP.py
index dbce972c..5f15386d 100644
--- a/python/quadruped_reactive_walking/WB_MPC/CrocoddylOCP.py
+++ b/python/quadruped_reactive_walking/WB_MPC/CrocoddylOCP.py
@@ -146,10 +146,7 @@ class Node:
self.isTerminal = isTerminal
self.state = state
- if pd.useFixedBase == 0:
- self.actuation = crocoddyl.ActuationModelFloatingBase(self.state)
- else:
- self.actuation = crocoddyl.ActuationModelFull(self.state)
+ self.actuation = crocoddyl.ActuationModelFloatingBase(self.state)
self.control = crocoddyl.ControlParametrizationModelPolyZero(self.actuation.nu)
self.nu = self.actuation.nu
diff --git a/python/quadruped_reactive_walking/WB_MPC/ProblemData.py b/python/quadruped_reactive_walking/WB_MPC/ProblemData.py
index 60c2979b..f2eeeadc 100644
--- a/python/quadruped_reactive_walking/WB_MPC/ProblemData.py
+++ b/python/quadruped_reactive_walking/WB_MPC/ProblemData.py
@@ -14,6 +14,13 @@ class problemDataAbstract:
self.robot = erd.load("solo12")
self.q0 = self.robot.q0
+ self.q0[:7] = np.array([0.0,
+ 0.0,
+ 0.2607495,
+ 0,
+ 0,
+ 0,
+ 1])
self.q0[7:] = param.q_init
self.model = self.robot.model
@@ -77,85 +84,28 @@ class ProblemData(problemDataAbstract):
def __init__(self, param):
super().__init__(param)
- self.useFixedBase = 0
+ self.useFixedBase = 1
+
+ # Cost function weights
# Cost function weights
self.mu = 0.7
- self.foot_tracking_w = 1e2
- self.friction_cone_w = 1e3
+ self.foot_tracking_w = 1e3
+ # self.friction_cone_w = 1e3 * 0
self.control_bound_w = 1e3
self.control_reg_w = 1e0
- self.state_reg_w = np.array(
- [0] * 3
- + [1e1] * 3
- + [1e0] * 3
- + [1e-3] * 3
- + [1e0] * 6
- + [0] * 6
- + [1e1] * 3
- + [3 * 1e-1] * 3
- + [1e1] * 6
- )
- self.terminal_velocity_w = np.array([0] * 18 + [1e3] * 18)
- self.control_bound_w = 1e3
+ self.state_reg_w = np.array([0] * 3
+ + [1e1] * 3
+ + [1e0] * 3
+ + [1e-5] * 3
+ + [1e0] * 6
+ + [0] * 6
+ + [1e1] * 3
+ + [1e0] * 3
+ + [1e1] * 6
+ )
+ self.terminal_velocity_w = np.array(
+ [0] * self.nq + [1e3] * self.nv)
- self.x0 = np.array(
- [
- 0.0,
- 0.0,
- 0.2607495,
- 0,
- 0,
- 0,
- 1,
- 0,
- 0.7,
- -1.4,
- 0.0,
- 0.7,
- -1.4,
- 0.0,
- -0.7,
- 1.4,
- 0.0,
- -0.7,
- 1.4,
- 0,
- 0,
- 0,
- 0,
- 0,
- 0,
- 0,
- 0,
- 0,
- 0,
- 0,
- 0,
- 0,
- 0,
- 0,
- 0,
- 0,
- 0,
- ]
- ) # x0 got from PyBullet
-
- self.u0 = np.array(
- [
- -0.02615051,
- -0.25848605,
- 0.51696646,
- 0.0285894,
- -0.25720605,
- 0.51441775,
- -0.02614404,
- 0.25848271,
- -0.51697107,
- 0.02859587,
- 0.25720939,
- -0.51441314,
- ]
- ) # quasi static control
self.xref = self.x0
self.uref = self.u0
diff --git a/python/quadruped_reactive_walking/WB_MPC/Target.py b/python/quadruped_reactive_walking/WB_MPC/Target.py
index fa0f055c..90f34a01 100644
--- a/python/quadruped_reactive_walking/WB_MPC/Target.py
+++ b/python/quadruped_reactive_walking/WB_MPC/Target.py
@@ -6,7 +6,7 @@ import pinocchio as pin
class Target:
def __init__(self, pd: ProblemData):
self.dt = pd.dt
- pin.forwardKinematics(pd.model, pd.rdata, pd.q0_reduced, pd.v0_reduced)
+ pin.forwardKinematics(pd.model, pd.rdata, pd.q0, pd.v0)
pin.updateFramePlacements(pd.model, pd.rdata)
self.foot_pose = pd.rdata.oMf[pd.rfFootId].translation.copy()
self.A = np.array([0, 0.03, 0.03])
diff --git a/python/quadruped_reactive_walking/main_solo12_control.py b/python/quadruped_reactive_walking/main_solo12_control.py
index 0e0ce5b3..032de58c 100644
--- a/python/quadruped_reactive_walking/main_solo12_control.py
+++ b/python/quadruped_reactive_walking/main_solo12_control.py
@@ -12,7 +12,7 @@ from .WB_MPC.ProblemData import ProblemData, ProblemDataFull
params = qrw.Params() # Object that holds all controller parameters
-pd = ProblemDataFull(params)
+pd = ProblemData(params)
repo = git.Repo(search_parent_directories=True)
sha = repo.head.object.hexsha
--
GitLab