Skip to content
Snippets Groups Projects
Commit 1a3c3c07 authored by Ale's avatar Ale
Browse files

working on walking ocp

parent 890a6f16
No related branches found
No related tags found
1 merge request!32Draft: Reverse-merge casadi-walking into new WBC devel branch
Pipeline #21161 failed
Stage: test
Hide whitespace changes
Inline Side-by-side
Showing
with 110 additions and 326 deletions
config/walk_parameters.yaml
+ 4
− 3
View file @ 1a3c3c07
......@@ -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
......@@ -37,8 +37,9 @@ robot:
# Parameters of Gait
N_periods: 1
gait: [10, 1, 1, 1, 1,
20, 1, 0, 1, 1] # Initial gait matrix
gait: [20, 1, 0, 0, 1,
20, 0, 1, 1, 0,
1, 1, 1, 1, 1] # Initial gait matrix
# Parameters of Joystick
gp_alpha_vel: 0.003 # Coefficient of the low pass filter applied to gamepad velocity
......
python/quadruped_reactive_walking/Controller.py
+ 4
− 11
View file @ 1a3c3c07
......@@ -171,13 +171,9 @@ class Controller:
self.result.FF = self.params.Kff_main * np.ones(12)
self.target = Target(params)
self.footsteps = []
for k in range(self.pd.T * self.pd.mpc_wbc_ratio):
self.target_footstep = self.target.compute(k).copy()
if k % self.pd.mpc_wbc_ratio == 0:
self.footsteps.append(self.target_footstep.copy())
self.velocity_task = self.target.velocity_task
self.mpc = WB_MPC_Wrapper.MPC_Wrapper(pd, params, self.footsteps, self.gait)
self.mpc = WB_MPC_Wrapper.MPC_Wrapper(pd, params, self.velocity_task, self.gait)
self.mpc_solved = False
self.k_result = 0
self.k_solve = 0
......@@ -216,10 +212,6 @@ class Controller:
t_measures = time.time()
self.t_measures = t_measures - t_start
self.target_footstep = self.target.compute(
self.k + self.pd.T * self.pd.mpc_wbc_ratio
)
if self.k % self.pd.mpc_wbc_ratio == 0:
if self.mpc_solved:
self.k_solve = self.k
......@@ -229,7 +221,7 @@ class Controller:
self.mpc.solve(
self.k,
m["x_m"],
self.target_footstep.copy(),
self.velocity_task.copy(),
self.gait,
self.xs_init,
self.us_init,
......@@ -252,6 +244,7 @@ class Controller:
# self.xs_init,
# self.us_init,
# )
self.gait = np.roll(self.gait, -1, axis=0)
except ValueError:
self.error = True
print("MPC Problem")
......
python/quadruped_reactive_walking/WB_MPC/CrocoddylOCP.py
+ 66
− 57
View file @ 1a3c3c07
......@@ -10,9 +10,10 @@ from time import time
class OCP:
def __init__(self, pd: ProblemData, footsteps, gait):
def __init__(self, pd: ProblemData, tasks, gait):
self.pd = pd
self.max_iter = 3
self.max_iter = 100
self.state = crocoddyl.StateMultibody(self.pd.model)
self.initialized = False
......@@ -20,7 +21,7 @@ class OCP:
self.t_update_last_model = 0.0
self.t_shift = 0.0
rm, tm = self.initialize_models(gait, footsteps)
rm, tm = self.initialize_models(gait, tasks)
self.x0 = self.pd.x0
......@@ -28,22 +29,25 @@ class OCP:
self.x0, rm, tm)
self.ddp = crocoddyl.SolverFDDP(self.problem)
def initialize_models(self, gait, footsteps):
def initialize_models(self, gait, tasks):
models = []
for t, step in enumerate(gait[:-1]):
tasks = self.make_task(footsteps[t])
support_feet = [self.pd.allContactIds[i] for i in np.nonzero(step == 1)[0]]
models.append(self.create_model(support_feet, tasks))
prev_support_feet = [self.pd.allContactIds[i] for i in np.nonzero(gait[t-1] == 1)[0]]
switching = True if t==0 or support_feet != prev_support_feet else False
models.append(self.create_model(support_feet, tasks, switching))
support_feet = [self.pd.allContactIds[i] for i in np.nonzero(gait[-1] == 1)[0]]
terminal_model = self.create_model(support_feet, is_terminal=True)
prev_support_feet = [self.pd.allContactIds[i] for i in np.nonzero(gait[self.pd.T] == 1)[0]]
switching = True if support_feet != prev_support_feet else False
terminal_model = self.create_model(support_feet, switching=switching, is_terminal=True)
return models, terminal_model
def solve(self, x0, footstep, gait, xs_init=None, us_init=None):
def solve(self, x0, tasks, gait, xs_init=None, us_init=None):
t_start = time()
self.x0 = x0
self.make_ocp(footstep, gait)
self.make_ocp(tasks, gait)
t_update = time()
self.t_update = t_update - t_start
......@@ -66,7 +70,7 @@ class OCP:
self.t_solve = time() - t_start
def make_ocp(self, footstep, gait):
def make_ocp(self, tasks, gait):
"""
Create a shooting problem for a simple walking gait.
......@@ -77,11 +81,7 @@ class OCP:
pin.updateFramePlacements(self.pd.model, self.pd.rdata)
if self.initialized:
tasks = self.make_task(footstep)
support_feet = [
self.pd.allContactIds[i] for i in np.nonzero(gait[-1] == 1)[0]
]
self.update_model(self.problem.runningModels[0], tasks, support_feet)
self.update_command_costs(self.problem.runningModels[0].differential.costs, tasks)
self.problem.circularAppend(
self.problem.runningModels[0],
......@@ -92,14 +92,6 @@ class OCP:
self.initialized = True
def make_task(self, footstep):
task = [[], []]
for foot in range(4):
if footstep[:, foot].any():
task[0].append(self.pd.allContactIds[foot])
task[1].append(footstep[:, foot])
return task
def get_results(self):
return (
self.ddp.xs.tolist().copy(),
......@@ -141,15 +133,7 @@ class OCP:
for m in self.ddp.problem.runningDatas]
return acc
def update_model(self, model, tasks, support_feet):
for i in self.pd.allContactIds:
name = self.pd.model.frames[i].name + "_contact"
model.differential.contacts.changeContactStatus(name, i in support_feet)
self.update_tracking_costs(model.differential.costs, tasks, support_feet)
def create_model(self, support_feet=[], tasks=[], is_terminal=False):
def create_model(self, support_feet=[], tasks=[], switching=False, is_terminal=False):
"""
Create the action model
......@@ -159,7 +143,7 @@ class OCP:
:param isTterminal: true for the terminal node
:return action model for a swing foot phase
"""
model = self.create_standard_model(support_feet)
model = self.create_standard_model(support_feet, switching)
if is_terminal:
self.make_terminal_model(model)
else:
......@@ -167,7 +151,7 @@ class OCP:
return model
def create_standard_model(self, support_feet):
def create_standard_model(self, support_feet, switching):
"""
Create a standard action model
......@@ -178,20 +162,51 @@ class OCP:
actuation = crocoddyl.ActuationModelFloatingBase(self.state)
nu = actuation.nu
costs = crocoddyl.CostModelSum(self.state, nu)
control = crocoddyl.ControlParametrizationModelPolyZero(nu)
contacts = crocoddyl.ContactModelMultiple(self.state, nu)
for i in self.pd.allContactIds:
for i in support_feet:
pin.forwardKinematics(self.pd.model, self.pd.rdata, self.pd.q0)
pin.updateFramePlacements(self.pd.model, self.pd.rdata)
start_pos = self.pd.rdata.oMf[i].translation
# Basic contact
name = self.pd.model.frames[i].name + "_contact"
contact = crocoddyl.ContactModel3D(
self.state, i, np.zeros(3), nu, self.pd.baumgarte_gains
self.state, i, start_pos, nu, self.pd.baumgarte_gains
)
contacts.addContact(name, contact)
if i not in support_feet:
contacts.changeContactStatus(name, False)
costs = crocoddyl.CostModelSum(self.state, nu)
# In case of landing feet land at right height
if switching:
impactResidual = crocoddyl.ResidualModelFrameTranslation(self.state, i, start_pos, actuation.nu)
impactAct = crocoddyl.ActivationModelWeightedQuad(np.array([0, 0, 1]))
impactCost = crocoddyl.CostModelResidual(self.state, impactAct, impactResidual)
costs.addCost(
"%s_altitudeimpact" % self.pd.model.frames[i].name,
impactCost,
self.pd.impact_altitude_weight / self.pd.dt,
)
impactVelResidual = crocoddyl.ResidualModelFrameVelocity(
self.state,
i,
pin.Motion.Zero(),
pin.ReferenceFrame.LOCAL,
actuation.nu,
)
impactVelCost = crocoddyl.CostModelResidual(self.state, impactVelResidual)
costs.addCost(
"%s_velimpact" % self.pd.model.frames[i].name,
impactVelCost,
self.pd.impact_velocity_weight / self.pd.dt,
)
residual = crocoddyl.ResidualModelState(self.state, self.pd.xref, nu)
activation = crocoddyl.ActivationModelWeightedQuad(self.pd.state_reg_w**2)
state_cost = crocoddyl.CostModelResidual(self.state, activation, residual)
......@@ -222,7 +237,7 @@ class OCP:
"""
nu = model.differential.actuation.nu
costs = model.differential.costs
for i in self.pd.allContactIds:
for i in support_feet:
cone = crocoddyl.FrictionCone(self.pd.Rsurf, self.pd.mu, 4, False, 3)
residual = crocoddyl.ResidualModelContactFrictionCone(
self.state, i, cone, nu
......@@ -237,14 +252,12 @@ class OCP:
costs.addCost(friction_name, friction_cone, self.pd.friction_cone_w)
costs.changeCostStatus(friction_name, i in support_feet)
name = self.pd.model.frames[i].name + "_foot_tracking"
residual = crocoddyl.ResidualModelFrameTranslation(
self.state, i, np.zeros(3), nu
)
foot_tracking = crocoddyl.CostModelResidual(self.state, residual)
costs.addCost(name, foot_tracking, self.pd.foot_tracking_w)
costs.changeCostStatus(name, False)
name = "base_velocity_tracking"
ref = pin.Motion(tasks[0], tasks[1])
residual_base_velocity = crocoddyl.ResidualModelFrameVelocity(
self.state, self.pd.baseId, ref, pin.LOCAL, nu)
base_velocity = crocoddyl.CostModelResidual(self.state, residual_base_velocity)
costs.addCost(name, base_velocity, self.pd.base_velocity_tracking_w)
control_residual = crocoddyl.ResidualModelControl(self.state, self.pd.uref)
control_reg = crocoddyl.CostModelResidual(self.state, control_residual)
......@@ -259,15 +272,11 @@ class OCP:
)
costs.addCost("control_bound", control_bound, self.pd.control_bound_w)
self.update_tracking_costs(costs, tasks, support_feet)
self.update_command_costs(costs, tasks)
def update_tracking_costs(self, costs, tasks, supportFootIds):
for i in self.pd.allContactIds:
name = self.pd.model.frames[i].name + "_foot_tracking"
index = 0
if i in tasks[0] and i not in supportFootIds:
costs.changeCostStatus(name, True)
costs.costs[name].cost.residual.reference = tasks[1][index]
index += 1
else:
costs.changeCostStatus(name, False)
def update_command_costs(self, costs, tasks):
name = "base_velocity_tracking"
costs.changeCostStatus(name, True)
costs.costs[name].cost.residual.reference = pin.Motion(tasks[0], tasks[1])
python/quadruped_reactive_walking/WB_MPC/ProblemData.py
+ 11
− 7
View file @ 1a3c3c07
......@@ -66,7 +66,6 @@ class problemDataAbstract:
self.rfFootId = self.model.getFrameId(self.rfFoot)
self.lhFootId = self.model.getFrameId(self.lhFoot)
self.rhFootId = self.model.getFrameId(self.rhFoot)
self.Rsurf = np.eye(3)
def freeze(self):
......@@ -88,21 +87,25 @@ class ProblemData(problemDataAbstract):
self.useFixedBase = 0
self.baseId = self.model.getFrameId("base_link")
# Cost function weights
# Cost function weights
self.mu = 0.7
self.foot_tracking_w = 1e2
self.friction_cone_w = 1e4
self.base_velocity_tracking_w = 1e2
self.friction_cone_w = 1e4
self.impact_altitude_weight = 1e3
self.impact_velocity_weight = 1e3
self.control_bound_w = 1e3
self.control_reg_w = 1e0
self.state_reg_w = np.array([0] * 3
+ [1e1] * 3
+ [1e1] * 3
+ [1e-3] * 3
+ [1e1] * 6
+ [1e1] * 12
+ [0] * 6
+ [1e1] * 3
+ [1e-1] * 3
+ [1e1] * 3
+ [1e1] * 6
)
self.terminal_velocity_w = np.array(
......@@ -110,6 +113,7 @@ class ProblemData(problemDataAbstract):
self.force_reg_w = 1e0
self.xref = self.x0
self.uref =self.u0
......
python/quadruped_reactive_walking/WB_MPC/Target.py
+ 3
− 226
View file @ 1a3c3c07
......@@ -10,229 +10,6 @@ class Target:
self.dt_wbc = params.dt_wbc
self.k_per_step = 160
self.position = np.array(params.footsteps_under_shoulders).reshape(
(3, 4), order="F"
)
if params.movement == "circle":
self.A = np.array([0, 0.03, 0.03])
self.offset = np.array([0, 0, 0.1])
self.freq = np.array([0, 0.5 *0, 0.5*0])
self.phase = np.array([0, np.pi / 2, 0])
elif params.movement == "step":
self.initial = self.position[:, 1].copy()
self.target = self.position[:, 1].copy() + np.array([0.1, 0.0, 0.0])
self.vert_time = params.vert_time
self.max_height = params.max_height
self.T = self.k_per_step * self.dt_wbc
self.A = np.zeros((6, 3))
self.update_time = -1
else:
self.target_footstep = self.position + np.array([0.0, 0.0, 0.10])
def compute(self, k):
footstep = np.zeros((3, 4))
if self.params.movement == "circle":
footstep[:, 1] = self.evaluate_circle(k)
elif self.params.movement == "step":
footstep[:, 1] = self.evaluate_step(1, k)
else:
footstep = self.target_footstep.copy()
return footstep
def evaluate_circle(self, k):
return (
self.position[:, 1]
+ self.offset
+ self.A * np.sin(2 * np.pi * self.freq * k * self.dt_wbc + self.phase)
)
def evaluate_step(self, j, k):
n_step = k // self.k_per_step
if n_step % 2 == 0:
return self.initial.copy() if (n_step % 4 == 0) else self.target.copy()
if n_step % 4 == 1:
initial = self.initial
target = self.target
else:
initial = self.target
target = self.initial
k_step = k % self.k_per_step
if n_step != self.update_time:
self.update_polynomial(initial, target)
self.update_time = n_step
t = k_step * self.dt_wbc
return self.compute_position(j, t)
def update_polynomial(self, initial, target):
x0 = initial[0]
y0 = initial[1]
x1 = target[0]
y1 = target[1]
# elapsed time
t = 0
d = self.T - 2 * self.vert_time
A = np.zeros((6, 3))
A[0, 0] = 12 * (x0 - x1) / (2 * (t - d) ** 5)
A[1, 0] = 30 * (x1 - x0) * (t + d) / (2 * (t - d) ** 5)
A[2, 0] = 20 * (x0 - x1) * (t**2 + d**2 + 4 * t * d) / (2 * (t - d) ** 5)
A[3, 0] = 60 * (x1 - x0) * (t * d**2 + t**2 * d) / (2 * (t - d) ** 5)
A[4, 0] = 60 * (x0 - x1) * (t**2 * d**2) / (2 * (t - d) ** 5)
A[5, 0] = (
2 * x1 * t**5
- 10 * x1 * t**4 * d
+ 20 * x1 * t**3 * d**2
- 20 * x0 * t**2 * d**3
+ 10 * x0 * t * d**4
- 2 * x0 * d**5
) / (2 * (t - d) ** 5)
A[0, 1] = 12 * (y0 - y1) / (2 * (t - d) ** 5)
A[1, 1] = 30 * (y1 - y0) * (t + d) / (2 * (t - d) ** 5)
A[2, 1] = 20 * (y0 - y1) * (t**2 + d**2 + 4 * t * d) / (2 * (t - d) ** 5)
A[3, 1] = 60 * (y1 - y0) * (t * d**2 + t**2 * d) / (2 * (t - d) ** 5)
A[4, 1] = 60 * (y0 - y1) * (t**2 * d**2) / (2 * (t - d) ** 5)
A[5, 1] = (
2 * y1 * t**5
- 10 * y1 * t**4 * d
+ 20 * y1 * t**3 * d**2
- 20 * y0 * t**2 * d**3
+ 10 * y0 * t * d**4
- 2 * y0 * d**5
) / (2 * (t - d) ** 5)
A[0, 2] = -self.max_height / ((self.T / 2) ** 6)
A[1, 2] = 3 * self.T * self.max_height / ((self.T / 2) ** 6)
A[2, 2] = -3 * self.T**2 * self.max_height / ((self.T / 2) ** 6)
A[3, 2] = self.T**3 * self.max_height / ((self.T / 2) ** 6)
self.A = A
def compute_position(self, j, t):
A = self.A.copy()
t_xy = t - self.vert_time
t_xy = max(0.0, t_xy)
t_xy = min(t_xy, self.T - 2 * self.vert_time)
self.position[:2, j] = (
A[5, :2]
+ A[4, :2] * t_xy
+ A[3, :2] * t_xy**2
+ A[2, :2] * t_xy**3
+ A[1, :2] * t_xy**4
+ A[0, :2] * t_xy**5
)
self.position[2, j] = (
A[3, 2] * t**3 + A[2, 2] * t**4 + A[1, 2] * t**5 + A[0, 2] * t**6
)
return self.position[:, j]
def evaluate_circle(self, k):
return (
self.position[:, 1]
+ self.offset
+ self.A * np.sin(2 * np.pi * self.freq * k * self.dt_wbc + self.phase)
)
def evaluate_step(self, j, k):
n_step = k // self.k_per_step
if n_step % 2 == 0:
return self.initial.copy() if (n_step % 4 == 0) else self.target.copy()
if n_step % 4 == 1:
initial = self.initial
target = self.target
else:
initial = self.target
target = self.initial
k_step = k % self.k_per_step
if n_step != self.update_time:
self.update_polynomial(initial, target)
self.update_time = n_step
t = k_step * self.dt_wbc
return self.compute_position(j, t)
def update_polynomial(self, initial, target):
x0 = initial[0]
y0 = initial[1]
x1 = target[0]
y1 = target[1]
# elapsed time
t = 0
d = self.T - 2 * self.vert_time
A = np.zeros((6, 3))
A[0, 0] = 12 * (x0 - x1) / (2 * (t - d) ** 5)
A[1, 0] = 30 * (x1 - x0) * (t + d) / (2 * (t - d) ** 5)
A[2, 0] = 20 * (x0 - x1) * (t**2 + d**2 + 4 * t * d) / (2 * (t - d) ** 5)
A[3, 0] = 60 * (x1 - x0) * (t * d**2 + t**2 * d) / (2 * (t - d) ** 5)
A[4, 0] = 60 * (x0 - x1) * (t**2 * d**2) / (2 * (t - d) ** 5)
A[5, 0] = (
2 * x1 * t**5
- 10 * x1 * t**4 * d
+ 20 * x1 * t**3 * d**2
- 20 * x0 * t**2 * d**3
+ 10 * x0 * t * d**4
- 2 * x0 * d**5
) / (2 * (t - d) ** 5)
A[0, 1] = 12 * (y0 - y1) / (2 * (t - d) ** 5)
A[1, 1] = 30 * (y1 - y0) * (t + d) / (2 * (t - d) ** 5)
A[2, 1] = 20 * (y0 - y1) * (t**2 + d**2 + 4 * t * d) / (2 * (t - d) ** 5)
A[3, 1] = 60 * (y1 - y0) * (t * d**2 + t**2 * d) / (2 * (t - d) ** 5)
A[4, 1] = 60 * (y0 - y1) * (t**2 * d**2) / (2 * (t - d) ** 5)
A[5, 1] = (
2 * y1 * t**5
- 10 * y1 * t**4 * d
+ 20 * y1 * t**3 * d**2
- 20 * y0 * t**2 * d**3
+ 10 * y0 * t * d**4
- 2 * y0 * d**5
) / (2 * (t - d) ** 5)
A[0, 2] = -self.max_height / ((self.T / 2) ** 6)
A[1, 2] = 3 * self.T * self.max_height / ((self.T / 2) ** 6)
A[2, 2] = -3 * self.T**2 * self.max_height / ((self.T / 2) ** 6)
A[3, 2] = self.T**3 * self.max_height / ((self.T / 2) ** 6)
self.A = A
def compute_position(self, j, t):
A = self.A.copy()
t_xy = t - self.vert_time
t_xy = max(0.0, t_xy)
t_xy = min(t_xy, self.T - 2 * self.vert_time)
self.position[:2, j] = (
A[5, :2]
+ A[4, :2] * t_xy
+ A[3, :2] * t_xy**2
+ A[2, :2] * t_xy**3
+ A[1, :2] * t_xy**4
+ A[0, :2] * t_xy**5
)
self.position[2, j] = (
A[3, 2] * t**3 + A[2, 2] * t**4 + A[1, 2] * t**5 + A[0, 2] * t**6
)
return self.position[:, j]
self.linear_velocity = np.array([10, 0, 0])
self.angular_velocity = np.array([0, 0, 0])
self.velocity_task = [self.linear_velocity, self.angular_velocity]
\ No newline at end of file
python/quadruped_reactive_walking/WB_MPC_Wrapper.py
+ 16
− 16
View file @ 1a3c3c07
......@@ -55,7 +55,7 @@ class MPC_Wrapper:
self.last_available_result = Result(pd)
self.new_result = Value("b", False)
def solve(self, k, x0, footstep, gait, xs=None, us=None):
def solve(self, k, x0, tasks, gait, xs=None, us=None):
"""
Call either the asynchronous MPC or the synchronous MPC depending on the value
of multiprocessing during the creation of the wrapper
......@@ -64,9 +64,9 @@ class MPC_Wrapper:
k (int): Number of inv dynamics iterations since the start of the simulation
"""
if self.multiprocessing:
self.run_MPC_asynchronous(k, x0, footstep, gait, xs, us)
self.run_MPC_asynchronous(k, x0, tasks, gait, xs, us)
else:
self.run_MPC_synchronous(x0, footstep, gait, xs, us)
self.run_MPC_synchronous(x0, tasks, gait, xs, us)
def get_latest_result(self):
"""
......@@ -91,11 +91,11 @@ class MPC_Wrapper:
return self.last_available_result
def run_MPC_synchronous(self, x0, footstep, gait, xs, us):
def run_MPC_synchronous(self, x0, tasks, gait, xs, us):
"""
Run the MPC (synchronous version)
"""
self.ocp.solve(x0, footstep, gait, xs, us)
self.ocp.solve(x0, tasks, gait, xs, us)
(
self.last_available_result.xs,
self.last_available_result.us,
......@@ -104,7 +104,7 @@ class MPC_Wrapper:
) = self.ocp.get_results()
self.new_result.value = True
def run_MPC_asynchronous(self, k, x0, footstep, gait, xs, us):
def run_MPC_asynchronous(self, k, x0, tasks, gait, xs, us):
"""
Run the MPC (asynchronous version)
"""
......@@ -113,7 +113,7 @@ class MPC_Wrapper:
p = Process(target=self.MPC_asynchronous)
p.start()
self.add_new_data(k, x0, footstep, gait, xs, us)
self.add_new_data(k, x0, tasks, gait, xs, us)
def MPC_asynchronous(self):
"""
......@@ -125,27 +125,27 @@ class MPC_Wrapper:
self.new_data.value = False
k, x0, footstep, gait, xs, us = self.decompress_dataIn()
k, x0, tasks, gait, xs, us = self.decompress_dataIn()
if k == 0:
loop_ocp = OCP(self.pd, self.footsteps_plan, self.initial_gait)
loop_ocp.solve(x0, footstep, gait, xs, us)
loop_ocp.solve(x0, tasks, gait, xs, us)
xs, us, K, solving_time = loop_ocp.get_results()
self.compress_dataOut(xs, us, K, solving_time)
self.new_result.value = True
def add_new_data(self, k, x0, footstep, gait, xs, us):
def add_new_data(self, k, x0, tasks, gait, xs, us):
"""
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
that there is a new data
"""
self.compress_dataIn(k, x0, footstep, gait, xs, us)
self.compress_dataIn(k, x0, tasks, gait, xs, us)
self.new_data.value = True
def compress_dataIn(self, k, x0, footstep, gait, xs, us):
def compress_dataIn(self, k, x0, tasks, gait, xs, us):
"""
Decompress data from a C-type structure that belongs to the shared memory to
retrieve data from the main control loop in the asynchronous MPC
......@@ -156,7 +156,7 @@ class MPC_Wrapper:
with self.in_x0.get_lock():
np.frombuffer(self.in_x0.get_obj()).reshape(self.pd.nx)[:] = x0
with self.in_footstep.get_lock():
np.frombuffer(self.in_footstep.get_obj()).reshape((3, 4))[:, :] = footstep
np.frombuffer(self.in_footstep.get_obj()).reshape((3, 4))[:, :] = tasks
with self.in_gait.get_lock():
np.frombuffer(self.in_gait.get_obj()).reshape((self.pd.T + 1, 4))[:, :] = gait
......@@ -183,12 +183,12 @@ class MPC_Wrapper:
with self.in_x0.get_lock():
x0 = np.frombuffer(self.in_x0.get_obj()).reshape(self.pd.nx)
with self.in_footstep.get_lock():
footstep = np.frombuffer(self.in_footstep.get_obj()).reshape((3, 4))
tasks = np.frombuffer(self.in_footstep.get_obj()).reshape((3, 4))
with self.in_gait.get_lock():
gait = np.frombuffer(self.in_gait.get_obj()).reshape((self.pd.T + 1, 4))
if not self.in_warm_start.value:
return k, x0, footstep, gait, None, None
return k, x0, tasks, gait, None, None
with self.in_xs.get_lock():
xs = list(
......@@ -199,7 +199,7 @@ class MPC_Wrapper:
np.frombuffer(self.in_us.get_obj()).reshape((self.pd.T, self.pd.nu))
)
return k, x0, footstep, gait, xs, us
return k, x0, tasks, gait, xs, us
def compress_dataOut(self, xs, us, K, solving_time):
"""
......
python/quadruped_reactive_walking/main_solo12_control.py
+ 4
− 4
View file @ 1a3c3c07
......@@ -104,10 +104,10 @@ def damp_control(device, nb_motors):
device.parse_sensor_data()
# Set desired quantities for the actuators
device.joints.set_position_gains(np.zeros(nb_motors))
device.joints.set_velocity_gains(0.1 * np.ones(nb_motors))
device.joints.set_desired_positions(np.zeros(nb_motors))
device.joints.set_desired_velocities(np.zeros(nb_motors))
# device.joints.set_position_gains(np.zeros(nb_motors))
# device.joints.set_velocity_gains(0.1 * np.ones(nb_motors))
# device.joints.set_desired_positions(np.zeros(nb_motors))
# device.joints.set_desired_velocities(np.zeros(nb_motors))
device.joints.set_torques(np.zeros(nb_motors))
# Send command to the robot
......
python/quadruped_reactive_walking/tools/LoggerControl.py
+ 2
− 2
View file @ 1a3c3c07
......@@ -121,9 +121,9 @@ class LoggerControl:
if self.i == 0:
for i in range(self.pd.T * self.pd.mpc_wbc_ratio):
self.target[i] = controller.footsteps[i // self.pd.mpc_wbc_ratio][:, 1]
self.target[i] = controller.velocity_task[0]
if self.i + self.pd.T * self.pd.mpc_wbc_ratio < self.log_size:
self.target[self.i + self.pd.T * self.pd.mpc_wbc_ratio] = controller.target_footstep[:, 1]
self.target[self.i + self.pd.T * self.pd.mpc_wbc_ratio] = controller.velocity_task[0]
if not self.params.enable_multiprocessing:
self.t_ocp_update[self.i] = controller.mpc.ocp.t_update
......
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment