diff --git a/config/walk_parameters.yaml b/config/walk_parameters.yaml
index 60ed76ee0fd5a16145cda4c6938657c5a0532953..8559f90cb0a09e582044f1f071536d0946ed3351 100644
--- a/config/walk_parameters.yaml
+++ b/config/walk_parameters.yaml
@@ -7,11 +7,11 @@ robot:
PLOTTING: true # Enable/disable automatic plotting at the end of the experiment
DEMONSTRATION: false # Enable/disable demonstration functionalities
SIMULATION: true # Enable/disable PyBullet simulation or running on real robot
- enable_pyb_GUI: False # Enable/disable PyBullet GUI
+ enable_pyb_GUI: True # Enable/disable PyBullet GUI
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: 5000 # Number of simulated wbc time steps
+ N_SIMULATION: 100 # 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
diff --git a/python/quadruped_reactive_walking/Controller.py b/python/quadruped_reactive_walking/Controller.py
index 24bcca59ea1614ab875483f7523ea2bf815adffc..d518768aa1268531a8c1e2b789506736a38b0431 100644
--- a/python/quadruped_reactive_walking/Controller.py
+++ b/python/quadruped_reactive_walking/Controller.py
@@ -21,7 +21,7 @@ class Result:
self.q_des = np.zeros(12)
self.v_des = np.zeros(12)
self.tau_ff = np.zeros(12)
-
+
class DummyDevice:
def __init__(self):
@@ -68,18 +68,17 @@ class Controller:
self.result = Result(params)
self.params = params
self.q_init = pd.q0
-
+
device = DummyDevice()
device.joints.positions = q_init
try:
- #file = np.load('/tmp/init_guess.npy', allow_pickle=True).item()
- self.guess = {'xs': list(file['xs']), 'us': list(file['us'])}
+ # file = np.load('/tmp/init_guess.npy', allow_pickle=True).item()
+ self.guess = {"xs": list(file["xs"]), "us": list(file["us"])}
print("\nInitial guess loaded\n")
except:
self.guess = {}
- print("\nNo tinitial_guess\n")
- #self.compute(device)
-
+ print("\nNo initial guess\n")
+ # self.compute(device)
def compute(self, device, qc=None):
"""Run one iteration of the main control loop
@@ -95,12 +94,12 @@ class Controller:
self.t_measures = t_measures - t_start
try:
- self.mpc.solve(self.k, m['x_m'], self.guess) # Closed loop mpc
+ self.mpc.solve(self.k, m["x_m"], self.guess) # Closed loop mpc
# Trajectory tracking
- #if self.initialized:
+ # if self.initialized:
# self.mpc.solve(self.k, self.mpc_result.x[1], self.guess)
- #else:
+ # else:
# self.mpc.solve(self.k, m["x_m"], self.guess)
except ValueError:
@@ -109,20 +108,22 @@ class Controller:
t_mpc = time.time()
self.t_mpc = t_mpc - t_measures
-
+
if not self.error:
self.mpc_result, self.mpc_cost = self.mpc.get_latest_result()
- #self.result.P = np.array(self.params.Kp_main.tolist() * 4)
- self.result.P = np.array([5] * 3 + [0] * 3 + [5]*6)
- #self.result.D = np.array(self.params.Kd_main.tolist() * 4)
- self.result.D = np.array([0.3] * 3 + [0] * 3 + [0.3]*6)
+ # self.result.P = np.array(self.params.Kp_main.tolist() * 4)
+ self.result.P = np.array([5] * 3 + [0] * 3 + [5] * 6)
+ # self.result.D = np.array(self.params.Kd_main.tolist() * 4)
+ self.result.D = np.array([0.3] * 3 + [0] * 3 + [0.3] * 6)
tauFF = self.mpc_result.u[0]
- self.result.FF = self.params.Kff_main * np.array([0] * 3 + list(tauFF) + [0]*6)
+ self.result.FF = self.params.Kff_main * np.array(
+ [0] * 3 + list(tauFF) + [0] * 6
+ )
# Keep only the actuated joints and set the other to default values
- self.mpc_result.q = np.array([self.pd.q0] * (self.pd.T + 1))[:, 7: 19]
- self.mpc_result.v = np.array([self.pd.v0] * (self.pd.T +1 ))[:, 6: ]
+ self.mpc_result.q = np.array([self.pd.q0] * (self.pd.T + 1))[:, 7:19]
+ self.mpc_result.v = np.array([self.pd.v0] * (self.pd.T + 1))[:, 6:]
self.mpc_result.q[:, 3:6] = np.array(self.mpc_result.x)[:, : self.pd.nq]
self.mpc_result.v[:, 3:6] = np.array(self.mpc_result.x)[:, self.pd.nq :]
@@ -174,10 +175,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"]) > 500 * 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"]) > 500 * np.pi / 180)
self.error = True
elif (np.abs(self.result.FF) > 3.2).any():
print("-- FEEDFORWARD TORQUES TOO HIGH ERROR --")
@@ -259,12 +260,12 @@ class Controller:
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}
+ return {"qj_m": qj_m, "vj_m": vj_m, "x_m": x_m}
def interpolate_traj(self, device, q_des, v_des, ratio):
measures = self.read_state(device)
- qj_des_i = np.linspace(measures['qj_m'], q_des, ratio)
- vj_des_i = np.linspace(measures['vj_m'], v_des, ratio)
+ qj_des_i = np.linspace(measures["qj_m"], q_des, ratio)
+ vj_des_i = np.linspace(measures["vj_m"], v_des, ratio)
return qj_des_i, vj_des_i
diff --git a/python/quadruped_reactive_walking/WB_MPC/BenchmarkCrocoddylOCP.py b/python/quadruped_reactive_walking/WB_MPC/BenchmarkCrocoddylOCP.py
index 30cd701ad5d216b7e75638967ec0430fb32a407e..21ad05185aee63d0286bdc1333f1350ee27ded60 100644
--- a/python/quadruped_reactive_walking/WB_MPC/BenchmarkCrocoddylOCP.py
+++ b/python/quadruped_reactive_walking/WB_MPC/BenchmarkCrocoddylOCP.py
@@ -23,11 +23,11 @@ def createProblem():
x0 = pd.x0_reduced
ocp = OCP(pd, target)
- problem = ocp.make_ocp(x0)
+ ocp.make_ocp(x0)
xs = [x0] * (ocp.ddp.problem.T + 1)
us = ocp.ddp.problem.quasiStatic([x0] * ocp.ddp.problem.T)
- return xs, us, problem
+ return xs, us, ocp.problem
def runDDPSolveBenchmark(xs, us, problem):
diff --git a/python/quadruped_reactive_walking/WB_MPC/CrocoddylOCP.py b/python/quadruped_reactive_walking/WB_MPC/CrocoddylOCP.py
index 17d70020f476f24cd1327e8151b0a5c2df6f3cd4..fd2a26b52638dc3cb178c0d7e2581e682e678692 100644
--- a/python/quadruped_reactive_walking/WB_MPC/CrocoddylOCP.py
+++ b/python/quadruped_reactive_walking/WB_MPC/CrocoddylOCP.py
@@ -7,104 +7,114 @@ import pinocchio as pin
import numpy as np
from time import time
+
class OCP:
- def __init__(self, pd:ProblemData, target:Target):
+ def __init__(self, pd: ProblemData, target: Target):
self.pd = pd
self.target = target
-
+
self.results = OcpResult()
self.state = crocoddyl.StateMultibody(self.pd.model)
self.initialized = False
self.initialize_models()
-
+
+ self.x0 = self.pd.x0_reduced
+
+ self.problem = crocoddyl.ShootingProblem(
+ self.x0, self.models, self.terminal_model
+ )
+ self.ddp = crocoddyl.SolverFDDP(self.problem)
+
def initialize_models(self):
- self.models = []
+ self.nodes = []
for _ in range(self.pd.T):
- self.models.append(Model(self.pd, self.state, self.target.contactSequence[_])) # RunningModels
- self.models.append(Model(self.pd, self.state, isTerminal=True)) # TerminalModel
-
- def make_ocp(self, x0):
- """ Create a shooting problem for a simple walking gait.
+ self.nodes.append(Node(self.pd, self.state, self.target.contactSequence[_]))
+ self.terminal_node = Node(self.pd, self.state, isTerminal=True)
+
+ self.models = [node.model for node in self.nodes]
+ self.terminal_model = self.terminal_node.model
+
+ def make_ocp(self):
+ """
+ Create a shooting problem for a simple walking gait.
:param x0: initial state
"""
-
+ t_start = time()
+
# Compute the current foot positions
- q0 = x0[:self.pd.nq]
+ q0 = self.x0[: self.pd.nq]
pin.forwardKinematics(self.pd.model, self.pd.rdata, q0)
pin.updateFramePlacements(self.pd.model, self.pd.rdata)
+ t_FK = time()
+ self.t_FK = t_FK - t_start
+
if self.initialized:
self.models = self.models[1:] + [self.models[0]]
+ self.nodes = self.nodes[1:] + [self.nodes[0]]
- for t in range(self.pd.T):
- target = self.target.evaluate_in_t(t)
- freeIds = [idf for idf in self.pd.allContactIds if idf not in self.target.contactSequence[t]]
- self.appendTargetToModel(self.models[t], target, self.target.contactSequence[t], freeIds)
+ t_shift = time()
+ self.t_shift = t_shift - t_FK
- freeIds = [idf for idf in self.pd.allContactIds if idf not in self.target.contactSequence[self.pd.T]]
- #contactIds = self.target.contactSequence[self.pd.T]
- self.appendTargetToModel(self.models[self.pd.T], self.target.evaluate_in_t(self.pd.T),
- self.target.contactSequence[self.pd.T], freeIds, True)
+ # TODO You only need to update nodes[-1]
+ # use gait instead of contactIds and freeIds ?
- problem = crocoddyl.ShootingProblem(x0,
- [m.model for m in self.models[:-1]],
- self.models[-1].model)
- self.ddp = crocoddyl.SolverFDDP(problem)
+ t_update_last_model = time()
+ self.t_update_last_model = t_update_last_model - t_shift
- self.initialized = True
+ # If you need update terminal model
- return problem
+ t_update_terminal_model = time()
+ self.t_update_terminal_model = t_update_terminal_model - t_shift
- def appendTargetToModel(self, model, target, contactIds, freeIds, isTerminal=False):
- """ Action models for a footstep phase.
- :param numKnots: number of knots for the footstep phase
- :param supportFootIds: Ids of the supporting feet
- :param swingFootIds: Ids of the swinging foot
+ self.initialized = True
+
+ def update_model(self, node, target, contactIds, isTerminal=False):
+ """
+ Action models for a footstep phase.
:return footstep action models
"""
- # Action models for the foot swing
- swingFootTask = []
- for i in freeIds:
- try:
- tref = target[i]
- swingFootTask += [[i, pin.SE3(np.eye(3), tref)]]
- except:
- pass
-
- model.update_model(contactIds, swingFootTask, isTerminal=isTerminal)
+ # update translation
+ translation = np.zeros(3)
+ node.update_model(contactIds, translation, isTerminal=isTerminal)
-# Solve
def solve(self, x0, guess=None):
- problem = self.make_ocp(x0)
- self.ddp = crocoddyl.SolverFDDP(problem)
- # self.ddp.setCallbacks([crocoddyl.CallbackVerbose()])
- # for i, c in enumerate(self.ddp.problem.runningModels):
- # print(str(i), c.differential.contacts.contacts.todict().keys())
- # print(str(i+1), self.ddp.problem.terminalModel.differential.contacts.contacts.todict().keys())
- # print("\n")
+ self.x0 = x0
+
+ t_start = time()
+
+ self.make_ocp()
+
+ t_update = time()
+ self.t_update = t_update - t_start
if not guess:
- print("No warmstart provided")
xs = [x0] * (self.ddp.problem.T + 1)
us = self.ddp.problem.quasiStatic([x0] * self.ddp.problem.T)
else:
- xs = guess['xs']
- us = guess['us']
- print("Using warmstart")
- start_time = time()
+ xs = guess["xs"]
+ us = guess["us"]
+
+ t_warm_start = time()
+ self.t_warm_start = t_warm_start - t_update
+
+ # self.ddp.setCallbacks([crocoddyl.CallbackVerbose()])
self.ddp.solve(xs, us, 1, False)
- self.solver_time = time()- start_time
- print("Solver time: ", self.solver_time)
+
+ t_ddp = time()
+ self.t_ddp = t_ddp - t_warm_start
+
+ self.t_solve = time() - t_start
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
- self.results.solver_time = self.solver_time
+ self.results.solver_time = self.t_solve
return self.results
def get_croco_forces(self):
@@ -116,7 +126,7 @@ class OCP:
mdict = m.differential.multibody.contacts.contacts.todict()
for n in cnames:
if n in mdict:
- forces[n] += [(mdict[n].jMf.inverse()*mdict[n].f).linear]
+ forces[n] += [(mdict[n].jMf.inverse() * mdict[n].f).linear]
else:
forces[n] += [np.array([0, 0, 0])]
for f in forces:
@@ -130,23 +140,21 @@ class OCP:
mdict = m.differential.multibody.contacts.contacts.todict()
f_tmp = []
for n in mdict:
- f_tmp += [(mdict[n].jMf.inverse()*mdict[n].f).linear]
+ f_tmp += [(mdict[n].jMf.inverse() * mdict[n].f).linear]
forces += [np.concatenate(f_tmp)]
return forces
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
-
-class Model:
+class Node:
def __init__(self, pd, state, supportFootIds=[], isTerminal=False):
self.pd = pd
- self.supportFootIds=supportFootIds
- self.isTerminal=isTerminal
+ self.supportFootIds = supportFootIds
+ self.isTerminal = isTerminal
self.state = state
if pd.useFixedBase == 0:
@@ -156,6 +164,8 @@ class Model:
self.control = crocoddyl.ControlParametrizationModelPolyZero(self.actuation.nu)
self.nu = self.actuation.nu
+
+
self.createStandardModel()
if isTerminal:
self.make_terminal_model()
@@ -163,7 +173,7 @@ class Model:
self.make_running_model()
def createStandardModel(self):
- """ Action model for a swing foot phase.
+ """Action model for a swing foot phase.
:param timeStep: step duration of the action model
:param supportFootIds: Ids of the constrained feet
@@ -171,43 +181,60 @@ class Model:
:param swingFootTask: swinging foot task
:return action model for a swing foot phase
"""
-
+
self.contactModel = crocoddyl.ContactModelMultiple(self.state, self.nu)
for i in self.supportFootIds:
- supportContactModel = crocoddyl.ContactModel3D(self.state, i, np.array([0., 0., 0.]), self.nu,
- np.array([0., 0.]))
- self.contactModel.addContact(self.pd.model.frames[i].name + "_contact", supportContactModel)
+ supportContactModel = crocoddyl.ContactModel3D(
+ 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
+ )
# 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)
- stateActivation = crocoddyl.ActivationModelWeightedQuad(self.pd.state_reg_w**2)
- stateReg = crocoddyl.CostModelResidual(self.state, stateActivation, stateResidual)
+ stateActivation = crocoddyl.ActivationModelWeightedQuad(
+ self.pd.state_reg_w**2
+ )
+ stateReg = crocoddyl.CostModelResidual(
+ self.state, stateActivation, stateResidual
+ )
costModel.addCost("stateReg", stateReg, 1)
self.costModel = costModel
- self.dmodel = crocoddyl.DifferentialActionModelContactFwdDynamics(self.state, self.actuation, self.contactModel,
- self.costModel, 0., True)
- self.model = crocoddyl.IntegratedActionModelEuler(self.dmodel, self.control, self.pd.dt)
+ self.dmodel = crocoddyl.DifferentialActionModelContactFwdDynamics(
+ self.state, self.actuation, self.contactModel, self.costModel, 0.0, True
+ )
+ self.model = crocoddyl.IntegratedActionModelEuler(
+ self.dmodel, self.control, self.pd.dt
+ )
def update_contact_model(self):
self.remove_contacts()
self.contactModel = crocoddyl.ContactModelMultiple(self.state, self.nu)
for i in self.supportFootIds:
- supportContactModel = crocoddyl.ContactModel3D(self.state, i, np.array([0., 0., 0.]), self.nu,
- np.array([0., 0.]))
- self.dmodel.contacts.addContact(self.pd.model.frames[i].name + "_contact", supportContactModel)
-
+ supportContactModel = crocoddyl.ContactModel3D(
+ 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
+ )
+
def make_terminal_model(self):
- self.remove_running_costs()
+ self.remove_running_costs()
self.update_contact_model()
- self.isTerminal=True
+ self.isTerminal = True
stateResidual = crocoddyl.ResidualModelState(self.state, self.pd.xref, self.nu)
- stateActivation = crocoddyl.ActivationModelWeightedQuad(self.pd.terminal_velocity_w**2)
- stateReg = crocoddyl.CostModelResidual(self.state, stateActivation, stateResidual)
+ stateActivation = crocoddyl.ActivationModelWeightedQuad(
+ self.pd.terminal_velocity_w**2
+ )
+ stateReg = crocoddyl.CostModelResidual(
+ self.state, stateActivation, stateResidual
+ )
self.costModel.addCost("terminalVelocity", stateReg, 1)
def make_running_model(self):
@@ -218,20 +245,34 @@ class Model:
self.update_contact_model()
for i in self.supportFootIds:
cone = crocoddyl.FrictionCone(self.pd.Rsurf, self.pd.mu, 4, False)
- coneResidual = crocoddyl.ResidualModelContactFrictionCone(self.state, i, cone, self.nu)
- coneActivation = crocoddyl.ActivationModelQuadraticBarrier(crocoddyl.ActivationBounds(cone.lb, cone.ub))
- frictionCone = crocoddyl.CostModelResidual(self.state, coneActivation, coneResidual)
- self.costModel.addCost(self.pd.model.frames[i].name + "_frictionCone", frictionCone, self.pd.friction_cone_w)
+ coneResidual = crocoddyl.ResidualModelContactFrictionCone(
+ self.state, i, cone, self.nu
+ )
+ coneActivation = crocoddyl.ActivationModelQuadraticBarrier(
+ crocoddyl.ActivationBounds(cone.lb, cone.ub)
+ )
+ frictionCone = crocoddyl.CostModelResidual(
+ self.state, coneActivation, coneResidual
+ )
+ self.costModel.addCost(
+ self.pd.model.frames[i].name + "_frictionCone",
+ frictionCone,
+ self.pd.friction_cone_w,
+ )
ctrlResidual = crocoddyl.ResidualModelControl(self.state, self.pd.uref)
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_activation = crocoddyl.ActivationModelQuadraticBarrier(crocoddyl.ActivationBounds(-self.pd.effort_limit, self.pd.effort_limit))
- ctrl_bound = crocoddyl.CostModelResidual(self.state, ctrl_bound_activation, ctrl_bound_residual)
+ ctrl_bound_activation = crocoddyl.ActivationModelQuadraticBarrier(
+ 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)
-
+
def remove_running_costs(self):
runningCosts = self.dmodel.costs.active.tolist()
idx = runningCosts.index("stateReg")
@@ -242,7 +283,7 @@ class Model:
def remove_terminal_cost(self):
if "terminalVelocity" in self.dmodel.costs.active.tolist():
- self.dmodel.costs.removeCost("terminalVelocity")
+ self.dmodel.costs.removeCost("terminalVelocity")
def remove_contacts(self):
allContacts = self.dmodel.contacts.contacts.todict()
@@ -250,15 +291,31 @@ class Model:
self.dmodel.contacts.removeContact(c)
def tracking_cost(self, swingFootTask):
+
+ # TODO all this done in initialisation ? (we always have at maximum one swing
+ # foot task so you can just initialize i once and then add or remove it)
if swingFootTask is not None:
for i in swingFootTask:
- frameTranslationResidual = crocoddyl.ResidualModelFrameTranslation(self.state, i[0], i[1].translation,self.nu)
- footTrack = crocoddyl.CostModelResidual(self.state, frameTranslationResidual)
- if self.pd.model.frames[i[0]].name + "_footTrack" in self.dmodel.costs.active.tolist():
- self.dmodel.costs.removeCost(self.pd.model.frames[i[0]].name + "_footTrack")
- self.costModel.addCost(self.pd.model.frames[i[0]].name + "_footTrack", footTrack, self.pd.foot_tracking_w)
-
- def update_model(self, supportFootIds = [], swingFootTask=[], isTerminal = False):
+ frameTranslationResidual = crocoddyl.ResidualModelFrameTranslation(
+ self.state, i[0], i[1].translation, self.nu
+ )
+ footTrack = crocoddyl.CostModelResidual(
+ self.state, frameTranslationResidual
+ )
+ if (
+ self.pd.model.frames[i[0]].name + "_footTrack"
+ in self.dmodel.costs.active.tolist()
+ ):
+ self.dmodel.costs.removeCost(
+ self.pd.model.frames[i[0]].name + "_footTrack"
+ )
+ self.costModel.addCost(
+ self.pd.model.frames[i[0]].name + "_footTrack",
+ footTrack,
+ self.pd.foot_tracking_w,
+ )
+
+ def update_model(self, supportFootIds=[], swingFootTask=[], isTerminal=False):
if isTerminal:
self.supportFootIds = supportFootIds
self.make_terminal_model()
diff --git a/python/quadruped_reactive_walking/WB_MPC/ProblemData.py b/python/quadruped_reactive_walking/WB_MPC/ProblemData.py
index b89848ccb1248e3247297bfaa3d6555107f25960..bc7d0981e7ef9f8119efe54aa42b49ffae97e233 100644
--- a/python/quadruped_reactive_walking/WB_MPC/ProblemData.py
+++ b/python/quadruped_reactive_walking/WB_MPC/ProblemData.py
@@ -8,8 +8,8 @@ class problemDataAbstract:
self.dt_sim = 0.001
self.dt_bldc = 0.0005
self.r1 = int(self.dt / self.dt_sim)
- self.init_steps = 0 # full stand phase
- self.target_steps = 120 # manipulation steps
+ self.init_steps = 0
+ self.target_steps = 120
self.T = self.init_steps + self.target_steps -1
self.robot = erd.load("solo12")
diff --git a/python/quadruped_reactive_walking/WB_MPC/Target.py b/python/quadruped_reactive_walking/WB_MPC/Target.py
index 8c65fb92024542f8429ebff0cfd87b3c976759ea..a8161d276dfc0ff0565d6abe896a5cc53e26a12a 100644
--- a/python/quadruped_reactive_walking/WB_MPC/Target.py
+++ b/python/quadruped_reactive_walking/WB_MPC/Target.py
@@ -2,24 +2,25 @@ import numpy as np
from .ProblemData import ProblemData
import pinocchio as pin
+
class Target:
- def __init__(self, pd:ProblemData):
+ def __init__(self, pd: ProblemData):
self.pd = pd
self.dt = pd.dt
if pd.useFixedBase == 0:
- self.gait = [] \
- + [ [ 1,1,1,1 ] ] * pd.init_steps \
- + [ [ 1,0,1,1 ] ] * pd.target_steps
+ self.gait = (
+ [] + [[1, 1, 1, 1]] * pd.init_steps + [[1, 0, 1, 1]] * pd.target_steps
+ )
elif pd.useFixedBase == 1:
- 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
+ )
else:
print("'useFixedBase' can be either 0 or 1 !")
self.T = pd.T
- self.contactSequence = [ self.patternToId(p) for p in self.gait]
+ self.contactSequence = [self.patternToId(p) for p in self.gait]
self.target = {pd.rfFootId: []}
q = pd.q0_reduced
@@ -28,24 +29,28 @@ class Target:
pin.updateFramePlacements(pd.model, pd.rdata)
self.FR_foot0 = pd.rdata.oMf[pd.rfFootId].translation.copy()
self.A = np.array([0, 0.05, 0.05])
- self.offset = np.array([0.08, 0., 0.06])
- self.freq = np.array([0, 2.5*0 , 2.5*0])
- self.phase = np.array([0, 0, np.pi/2])
+ self.offset = np.array([0.08, 0.0, 0.06])
+ self.freq = np.array([0, 2.5 * 0, 2.5 * 0])
+ self.phase = np.array([0, 0, np.pi / 2])
def patternToId(self, gait):
- return tuple(self.pd.allContactIds[i] for i,c in enumerate(gait) if c==1 )
-
+ return tuple(self.pd.allContactIds[i] for i, c in enumerate(gait) if c == 1)
+
def shift_gait(self):
self.gait.pop(0)
self.gait += [self.gait[-1]]
- self.contactSequence = [ self.patternToId(p) for p in self.gait]
+ self.contactSequence = [self.patternToId(p) for p in self.gait]
def update(self, t):
target = []
- for n in range(self.T+1):
- target += [self.FR_foot0 + self.offset + self.A *
- np.sin(2*np.pi*self.freq * (n+t) * self.dt + self.phase)]
+ for n in range(self.T + 1):
+ target += [
+ self.FR_foot0
+ + self.offset
+ + self.A
+ * np.sin(2 * np.pi * self.freq * (n + t) * self.dt + self.phase)
+ ]
self.target[self.pd.rfFootId] = np.array(target)
-
+
def evaluate_in_t(self, t):
- return {e: self.target[e][t] for e in self.target}
\ No newline at end of file
+ return {e: self.target[e][t] for e in self.target}
diff --git a/python/quadruped_reactive_walking/main_solo12_control.py b/python/quadruped_reactive_walking/main_solo12_control.py
index e81d0e19fd5e8d392d2627695cd1d9cfadc6a707..58900ea0d34880e5fd45fb84250825fad02e257d 100644
--- a/python/quadruped_reactive_walking/main_solo12_control.py
+++ b/python/quadruped_reactive_walking/main_solo12_control.py
@@ -174,7 +174,6 @@ def control_loop():
dT_whole = 0.0
cnt = 0
while (not device.is_timeout) and (t < t_max) and (not controller.error):
- print("ITER #", cnt)
t_start_whole = time.time()
target.update(cnt)
@@ -210,8 +209,6 @@ def control_loop():
k_log_whole += 1
cnt += 1
- print("Total loop time: ", t_log_whole[k_log_whole-1], "\n")
-
# ****************************************************************
finished = t >= t_max
damp_control(device, 12)
@@ -230,20 +227,11 @@ def control_loop():
print("Masterboard timeout detected.")
if params.LOGGING:
- try:
- log_path = Path("/tmp/") / "logs" / sha
- log_path.mkdir(parents=True)
- loggerControl.save(str(log_path / "data"))
- with open(str(log_path / 'readme.txt') , 'w') as f:
- f.write(msg)
-
- if params.PLOTTING:
- loggerControl.plot(True, str(log_path / "data"))
- except:
- print("\nCANNOT SAVE THE LOG\n\
- The folder already exists !!! Please delete the folder with the name of your current commit\n")
-
+ log_path = Path("/tmp") / "logs"
+ loggerControl.save(str(log_path / "data"))
+ if params.PLOTTING:
+ loggerControl.plot()
if params.SIMULATION and params.enable_pyb_GUI:
device.Stop()
diff --git a/python/quadruped_reactive_walking/tools/LoggerControl.py b/python/quadruped_reactive_walking/tools/LoggerControl.py
index d7628b793f017eb188a50b8a71bcea7c5dc8f0e9..c288787dda7a3cb58a0d926d6d7cf1c39356afe3 100644
--- a/python/quadruped_reactive_walking/tools/LoggerControl.py
+++ b/python/quadruped_reactive_walking/tools/LoggerControl.py
@@ -5,9 +5,6 @@ from .kinematics_utils import get_translation, get_translation_array
import matplotlib
import matplotlib.pyplot as plt
-matplotlib.use("QtAgg")
-plt.style.use("seaborn")
-
class LoggerControl:
def __init__(self, pd, log_size=60e3, loop_buffer=False, file=None):
@@ -48,10 +45,20 @@ class LoggerControl:
# Controller timings: MPC time, ...
self.t_measures = np.zeros(size)
- self.t_mpc = np.zeros(size) #solver time #measurement time
- self.t_send = np.zeros(size) #
- self.t_loop = np.zeros(size) #controller time loop
- self.t_whole = np.zeros(size) #controller time loop
+ self.t_mpc = np.zeros(size) # solver time #measurement time
+ self.t_send = np.zeros(size) #
+ self.t_loop = np.zeros(size) # controller time loop
+ self.t_whole = np.zeros(size) # controller time loop
+
+ self.t_ocp_update = np.zeros(size)
+ self.t_ocp_warm_start = np.zeros(size)
+ self.t_ocp_ddp = np.zeros(size)
+ self.t_ocp_solve = np.zeros(size)
+
+ self.t_ocp_update_FK = np.zeros(size)
+ self.t_ocp_shift = np.zeros(size)
+ self.t_ocp_update_last = np.zeros(size)
+ self.t_ocp_update_terminal = np.zeros(size)
# MPC
self.ocp_storage = {
@@ -68,12 +75,6 @@ class LoggerControl:
self.wbc_tau_ff = np.zeros([size, 12]) # feedforward torques
def sample(self, controller, device, qualisys=None):
- # if self.i >= self.size:
- # if self.loop_buffer:
- # self.i = 0
- # else:
- # return
-
# Logging from the device (data coming from the robot)
self.q_mes[self.i] = device.joints.positions
self.v_mes[self.i] = device.joints.velocities
@@ -111,6 +112,21 @@ class LoggerControl:
self.ocp_storage["xs"][self.i] = np.array(controller.mpc.ocp.results.x)
self.ocp_storage["us"][self.i] = np.array(controller.mpc.ocp.results.u)
+ self.t_measures[self.i] = controller.t_measures
+ 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_update[self.i] = controller.mpc.ocp.t_update
+ self.t_ocp_warm_start[self.i] = controller.mpc.ocp.t_warm_start
+ self.t_ocp_ddp[self.i] = controller.mpc.ocp.t_ddp
+ self.t_ocp_solve[self.i] = controller.mpc.ocp.t_solve
+
+ self.t_ocp_update_FK[self.i] = controller.mpc.ocp.t_FK
+ self.t_ocp_shift[self.i] = controller.mpc.ocp.t_shift
+ self.t_ocp_update_last[self.i] = controller.mpc.ocp.t_update_last_model
+ self.t_ocp_update_terminal[self.i] = controller.mpc.ocp.t_update_terminal_model
+
# Logging from whole body control
self.wbc_P[self.i] = controller.result.P
self.wbc_D[self.i] = controller.result.D
@@ -125,12 +141,7 @@ class LoggerControl:
self.i += 1
def plot(self, save=False, fileName="tmp/"):
-
- horizon = self.ocp_storage["xs"].shape[0]
- t15 = np.linspace(0, horizon * self.pd.dt, horizon + 1)
- t1 = np.linspace(0, (horizon) * self.pd.dt, (horizon) * self.pd.r1 + 1)
- t_mpc = np.linspace(0, (horizon) * self.pd.dt, horizon + 1)
- t_range = np.array([k * self.pd.dt_sim for k in range(self.tstamps.shape[0])])
+ import matplotlib.pyplot as plt
all_ocp_feet_p_log = {
idx: [
@@ -142,115 +153,143 @@ class LoggerControl:
for foot in all_ocp_feet_p_log:
all_ocp_feet_p_log[foot] = np.array(all_ocp_feet_p_log[foot])
- """ plt.figure(figsize=(12, 6), dpi=90)
- plt.title("Solver timings")
- plt.hist(self.t_mpc, 30)
- plt.xlabel("times [s]")
- plt.ylabel("Number of cases [#]")
- plt.draw()
- if save:
- plt.savefig(fileName + "_solver_timings") """
-
- legend = ["Hip", "Shoulder", "Knee"]
- plt.figure(figsize=(12, 6), dpi=90)
- i = 0
- for i in range(4):
- plt.subplot(2, 2, i + 1)
- plt.title("Joint position of " + str(i))
- [
- plt.plot(t_range, np.array(self.q_mes)[:, (3 * i + jj)] * 180 / np.pi)
- for jj in range(3)
- ]
- plt.ylabel("Joint position [deg]")
- plt.xlabel("t[s]")
- plt.legend(legend)
- plt.draw()
- if save:
- plt.savefig(fileName + "_joint_positions")
-
- legend = ["Hip", "Shoulder", "Knee"]
- plt.figure(figsize=(12, 6), dpi=90)
- i = 0
- for i in range(4):
- plt.subplot(2, 2, i + 1)
- plt.title("Joint velocity of " + str(i))
- [
- plt.plot(t_range, np.array(self.v_mes)[:, (3 * i + jj)] * 180 / np.pi)
- for jj in range(3)
- ]
- plt.ylabel("Joint velocity [deg/s]")
- plt.xlabel("t[s]")
- plt.legend(legend)
- plt.draw()
- if save:
- plt.savefig(fileName + "_joint_velocities")
-
- legend = ["Hip", "Shoulder", "Knee"]
- plt.figure(figsize=(12, 6), dpi=90)
- i = 0
- for i in range(4):
- plt.subplot(2, 2, i + 1)
- plt.title("Joint torques of " + str(i))
- [
- plt.plot(t_range, np.array(self.torquesFromCurrentMeasurment)
- [:, (3 * i + jj)])
- for jj in range(3)
- ]
- plt.ylabel("Torque [Nm]")
- plt.xlabel("t[s]")
- plt.legend(legend)
- plt.draw()
- if save:
- plt.savefig(fileName + "_joint_torques")
-
-
-
- plt.figure(figsize=(12, 6), dpi=90)
- plt.plot(t_range, self.t_mpc)
- plt.plot(t_range, self.t_measures)
- plt.plot(t_range, self.t_send)
- plt.plot(t_range, self.t_loop, color="rebeccapurple")
- lgd = [ "MPC", "MEASUREMENT", "T SEND", "CONTROLLER"]
+ # plt.figure(figsize=(12, 6), dpi=90)
+ # plt.title("Solver timings")
+ # plt.hist(self.ocp_timings, 30)
+ # plt.xlabel("timee [s]")
+ # plt.ylabel("Number of cases [#]")
+ # plt.draw()
+ # if save:
+ # plt.savefig(fileName + "_solver_timings")
+
+ # legend = ["Hip", "Shoulder", "Knee"]
+ # plt.figure(figsize=(12, 6), dpi=90)
+ # i = 0
+ # for i in range(4):
+ # plt.subplot(2, 2, i + 1)
+ # plt.title("Joint position of " + str(i))
+ # [
+ # plt.plot(np.array(self.q_mes)[:, (3 * i + jj)] * 180 / np.pi)
+ # for jj in range(3)
+ # ]
+ # plt.ylabel("Joint position [deg]")
+ # plt.xlabel("t[s]")
+ # plt.legend(legend)
+ # plt.draw()
+ # if save:
+ # plt.savefig(fileName + "_joint_positions")
+
+ # plt.figure(figsize=(12, 6), dpi=90)
+ # i = 0
+ # for i in range(4):
+ # plt.subplot(2, 2, i + 1)
+ # plt.title("Joint velocity of " + str(i))
+ # [
+ # plt.plot(np.array(self.v_mes)[:, (3 * i + jj)] * 180 / np.pi)
+ # for jj in range(3)
+ # ]
+ # plt.ylabel("Joint velocity [deg/s]")
+ # plt.xlabel("t[s]")
+ # plt.legend(legend)
+ # plt.draw()
+ # if save:
+ # plt.savefig(fileName + "_joint_velocities")
+
+ # plt.figure(figsize=(12, 6), dpi=90)
+ # i = 0
+ # for i in range(4):
+ # plt.subplot(2, 2, i + 1)
+ # plt.title("Joint torques of " + str(i))
+ # [
+ # plt.plot(np.array(self.torquesFromCurrentMeasurment)[:, (3 * i + jj)])
+ # for jj in range(3)
+ # ]
+ # plt.ylabel("Torque [Nm]")
+ # plt.xlabel("t[s]")
+ # plt.legend(legend)
+ # plt.draw()
+ # if save:
+ # plt.savefig(fileName + "_joint_torques")
+
+ self.plot_controller_times()
+ self.plot_OCP_times()
+ self.plot_OCP_update_times()
+
+ plt.show()
+
+ 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])])
+
+ plt.figure()
+ plt.plot(t_range, self.t_measures, "r+")
+ plt.plot(t_range, self.t_mpc, "g+")
+ plt.plot(t_range, self.t_send, "b+")
+ plt.plot(t_range, self.t_loop, "+", color="violet")
+ plt.axhline(y=0.001, color="grey", linestyle=":", lw=1.0)
+ plt.axhline(y=0.01, color="grey", linestyle=":", lw=1.0)
+ lgd = ["Measures", "MPC", "Send", "Whole-loop"]
plt.legend(lgd)
plt.xlabel("Time [s]")
plt.ylabel("Time [s]")
- plt.draw()
- if save:
- plt.savefig(fileName + "_solver_timings")
-
- """ 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(t_mpc, feet_p_log_mpc[18][:, p], linewidth=0.8, color = 'tomato', marker='o')
- #plt.plot(t1, feet_p_log_m[18][:, p], linewidth=2, color = 'lightgreen')
- plt.draw() """
- plt.show()
+ def plot_OCP_times(self):
+ import matplotlib.pyplot as plt
- # TODO add the plots you want
+ 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+")
+ plt.plot(t_range, self.t_ocp_warm_start, "g+")
+ plt.plot(t_range, self.t_ocp_ddp, "b+")
+ plt.plot(t_range, self.t_ocp_solve, "+", color="violet")
+ plt.axhline(y=0.001, color="grey", linestyle=":", lw=1.0)
+ lgd = ["t_ocp_update", "t_ocp_warm_start", "t_ocp_ddp", "t_ocp_solve"]
+ plt.legend(lgd)
+ plt.xlabel("Time [s]")
+ plt.ylabel("Time [s]")
+ 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])])
+
+ plt.figure()
+ plt.plot(t_range, self.t_ocp_update_FK, "r+")
+ plt.plot(t_range, self.t_ocp_shift, "g+")
+ plt.plot(t_range, self.t_ocp_update_last, "b+")
+ plt.plot(t_range, self.t_ocp_update_terminal, "+", color="seagreen")
+ plt.axhline(y=0.001, color="grey", linestyle=":", lw=1.0)
+ lgd = [
+ "t_ocp_update_FK",
+ "t_ocp_shift",
+ "t_ocp_update_last",
+ "t_ocp_update_terminal",
+ ]
+ plt.legend(lgd)
+ plt.xlabel("Time [s]")
+ plt.ylabel("Time [s]")
def save(self, fileName="data"):
- # date_str = datetime.now().strftime("_%Y_%m_%d_%H_%M")
- # name = fileName + date_str + "_" + str(self.type_MPC) + ".npz"
+ date_str = datetime.now().strftime("_%Y_%m_%d_%H_%M")
+ name = fileName + date_str + ".npz"
np.savez_compressed(
- fileName,
- # t_MPC=self.t_MPC,
+ name,
ocp_storage=self.ocp_storage,
- mpc_solving_duration=self.t_mpc,
- t_send = self.t_send,
- t_loop = self.t_loop,
- t_measures = self.t_measures,
- # mpc_cost=self.mpc_cost,
+ t_measures=self.t_measures,
+ t_mpc=self.t_mpc,
+ t_send=self.t_send,
+ t_loop=self.t_loop,
+ t_ocp_update=self.t_ocp_update,
+ t_ocp_warm_start=self.t_ocp_warm_start,
+ t_ocp_ddp=self.t_ocp_ddp,
+ t_ocp_solve=self.t_ocp_solve,
+ t_ocp_update_FK=self.t_ocp_update_FK,
+ t_ocp_shift=self.t_ocp_shift,
+ t_ocp_update_last=self.t_ocp_update_last,
+ t_ocp_update_terminal=self.t_ocp_update_terminal,
wbc_P=self.wbc_P,
wbc_D=self.wbc_D,
wbc_q_des=self.wbc_q_des,
@@ -275,7 +314,7 @@ class LoggerControl:
voltage=self.voltage,
energy=self.energy,
)
- print("Logs and plots saved in " + fileName)
+ print("Logs and plots saved in " + name)
def load(self):
if self.data is None:
@@ -310,6 +349,10 @@ class LoggerControl:
self.ocp_storage = self.data["ocp_storage"].item()
+ self.t_measures = self.data["t_measures"]
+ self.t_mpc = self.data["t_mpc"]
+ self.t_send = self.data["t_send"]
+ self.t_loop = self.data["t_loop"]
self.wbc_P = self.data["wbc_P"]
self.wbc_D = self.data["wbc_D"]
self.wbc_q_des = self.data["wbc_q_des"]