diff --git a/config/walk_parameters.yaml b/config/walk_parameters.yaml
index c26f46823d39af6107648b447fbdaa1e7a8e9d9c..d5081da65080c02b52792e4eb165166fb201f25b 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: 10000 # Number of simulated wbc time steps
+ N_SIMULATION: 5000 # 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
perfect_estimator: true # Enable/disable perfect estimator by using data directly from PyBullet
@@ -25,6 +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
+ movement: "circle" # name of the movement to perform
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+
diff --git a/include/qrw/Params.hpp b/include/qrw/Params.hpp
index f03660169bfa45002728413e51cfea066d01c2bf..4202a401fb299cbb6109cd8a88a3ec0ecc278d9e 100644
--- a/include/qrw/Params.hpp
+++ b/include/qrw/Params.hpp
@@ -93,6 +93,7 @@ class Params {
int N_periods; // Number of gait periods in the MPC prediction horizon
int type_MPC; // Which MPC solver you want to use: 0 for OSQP MPC, 1, 2, 3 for Crocoddyl MPCs
bool save_guess; // true to save the initial result of the mpc
+ std::string movement; // Name of the mmovemnet to perform
bool interpolate_mpc; // true to interpolate the impedance quantities, otherwise integrate
int interpolation_type; // type of interpolation used
bool kf_enabled; // Use complementary filter (False) or kalman filter (True) for the estimator
diff --git a/python/CMakeLists.txt b/python/CMakeLists.txt
index 8ee1216eacd51158d5d3f5d3062acfde6fdab66f..8739abe9a404e94cb21cbd9b0461469ed9d79064 100644
--- a/python/CMakeLists.txt
+++ b/python/CMakeLists.txt
@@ -40,6 +40,7 @@ set(${PY_NAME}_TOOLS_PYTHON
__init__.py
LoggerControl.py
PyBulletSimulator.py
+ Utils.py
qualisysClient.py
kinematics_utils.py
)
diff --git a/python/Params.cpp b/python/Params.cpp
index 0a7b5cd81dfa51d94a78aadc31c94f0e7ba5dc24..60740528acd840e45442523cb8365343d4d1c9c9 100644
--- a/python/Params.cpp
+++ b/python/Params.cpp
@@ -27,6 +27,7 @@ struct ParamsVisitor : public bp::def_visitor<ParamsVisitor<Params>> {
.def_readwrite("use_flat_plane", &Params::use_flat_plane)
.def_readwrite("predefined_vel", &Params::predefined_vel)
.def_readwrite("save_guess", &Params::save_guess)
+ .def_readwrite("movement", &Params::movement)
.def_readwrite("interpolate_mpc", &Params::interpolate_mpc)
.def_readwrite("interpolation_type", &Params::interpolation_type)
.def_readwrite("kf_enabled", &Params::kf_enabled)
@@ -50,6 +51,7 @@ struct ParamsVisitor : public bp::def_visitor<ParamsVisitor<Params>> {
.def_readwrite("CoM_offset", &Params::CoM_offset)
.def_readwrite("h_ref", &Params::h_ref)
.def_readwrite("shoulders", &Params::shoulders)
+ .def_readwrite("max_height", &Params::max_height)
.def_readwrite("lock_time", &Params::lock_time)
.def_readwrite("vert_time", &Params::vert_time)
.def_readwrite("footsteps_init", &Params::footsteps_init)
diff --git a/python/quadruped_reactive_walking/Controller.py b/python/quadruped_reactive_walking/Controller.py
index 04df1d8cf7292d15bd4a6c051293ff4aa5795349..e4e2069e7e9490d9fd1519f888204fb57a901238 100644
--- a/python/quadruped_reactive_walking/Controller.py
+++ b/python/quadruped_reactive_walking/Controller.py
@@ -6,6 +6,7 @@ import pybullet as pyb
from . import WB_MPC_Wrapper
from .WB_MPC.Target import Target
+from .tools.Utils import init_robot
COLORS = ["#1f77b4", "#ff7f0e", "#2ca02c"]
@@ -154,6 +155,7 @@ class Controller:
self.params = params
self.gait = np.repeat(np.array([0, 0, 0, 0]).reshape((1, 4)), self.pd.T, axis=0)
self.q_init = pd.q0
+ init_robot(q_init, params)
self.k = 0
self.error = False
@@ -163,9 +165,14 @@ class Controller:
self.result.q_des = self.pd.q0[7:].copy()
self.result.v_des = self.pd.v0[6:].copy()
- self.target = Target(pd)
- footsteps = [self.target.footstep(t) for t in range(pd.T)]
- self.mpc = WB_MPC_Wrapper.MPC_Wrapper(pd, params, footsteps, self.gait)
+ 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.mpc = WB_MPC_Wrapper.MPC_Wrapper(pd, params, self.footsteps, self.gait)
self.mpc_solved = False
self.k_result = 0
self.k_solve = 0
@@ -201,27 +208,29 @@ 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:
- self.target.shift()
if self.mpc_solved:
self.k_solve = self.k
self.mpc_solved = False
try:
- footstep = self.target.footstep(self.pd.T)
- self.mpc.solve(self.k,
- m["x_m"],
- footstep,
- self.gait,
- self.xs_init,
- self.us_init
- )
- # OPEN LOOP MPC
+ self.mpc.solve(
+ self.k,
+ m["x_m"],
+ self.target_footstep.copy(),
+ self.gait,
+ self.xs_init,
+ self.us_init,
+ )
# if self.initialized:
# self.mpc.solve(
# self.k,
# self.mpc_result.xs[1],
- # footstep,
+ # self.target_footstep.copy(),
# self.gait,
# self.xs_init,
# self.us_init,
@@ -230,7 +239,7 @@ class Controller:
# self.mpc.solve(
# self.k,
# m["x_m"],
- # footstep,
+ # self.target_footstep.copy(),
# self.gait,
# self.xs_init,
# self.us_init,
@@ -257,6 +266,12 @@ class Controller:
self.result.FF = self.params.Kff_main * np.ones(12)
self.result.tau_ff = self.compute_torque(m)[:]
+ if self.params.interpolate_mpc:
+ if self.mpc_result.new_result:
+ if self.params.interpolation_type == 3:
+ self.interpolator.update(xs[0], xs[1], xs[2])
+ # self.interpolator.plot(self.pd.mpc_wbc_ratio, self.pd.dt_wbc)
+
if self.params.interpolate_mpc:
if self.mpc_result.new_result:
if self.params.interpolation_type == 3:
@@ -268,9 +283,6 @@ class Controller:
else:
q, v = self.integrate_x(m)
- q = xs[1][: self.pd.nq]
- v = xs[1][self.pd.nq :]
-
self.result.q_des = q[:]
self.result.v_des = v[:]
diff --git a/python/quadruped_reactive_walking/WB_MPC/BenchmarkCrocoddylOCP.py b/python/quadruped_reactive_walking/WB_MPC/BenchmarkCrocoddylOCP.py
index 16ea9c2086260ef56b2c224e54432a303a384cb7..c8423ab79ab970bc5e441439ff41622b7bb8cda8 100644
--- a/python/quadruped_reactive_walking/WB_MPC/BenchmarkCrocoddylOCP.py
+++ b/python/quadruped_reactive_walking/WB_MPC/BenchmarkCrocoddylOCP.py
@@ -17,7 +17,7 @@ MAXITER = 1
def createProblem():
params = qrw.Params()
pd = ProblemDataFull(params)
- target = Target(pd)
+ target = Target(params)
x0 = pd.x0_reduced
diff --git a/python/quadruped_reactive_walking/WB_MPC/CrocoddylOCP.py b/python/quadruped_reactive_walking/WB_MPC/CrocoddylOCP.py
index dbce972c0c1ced1ea7888a0fc6f5d78669d552ab..571dde8e3cd5ed413ef72f596b1ee19f3183ef9b 100644
--- a/python/quadruped_reactive_walking/WB_MPC/CrocoddylOCP.py
+++ b/python/quadruped_reactive_walking/WB_MPC/CrocoddylOCP.py
@@ -1,4 +1,5 @@
from tracemalloc import start
+from xxlimited import foo
from .ProblemData import ProblemData
from .Target import Target
@@ -19,27 +20,25 @@ class OCP:
self.t_update_last_model = 0.0
self.t_shift = 0.0
- self.initialize_models(gait, footsteps)
+ rm, tm = self.initialize_models(gait, footsteps)
self.x0 = self.pd.x0_reduced
self.problem = crocoddyl.ShootingProblem(
- self.x0, self.models, self.terminal_model
- )
+ self.x0, rm, tm)
self.ddp = crocoddyl.SolverFDDP(self.problem)
def initialize_models(self, gait, footsteps):
- self.nodes = []
+ models = []
for t, step in enumerate(gait):
- task = self.make_task(footsteps[t])
+ tasks = self.make_task(footsteps[t])
support_feet = [self.pd.allContactIds[i] for i in np.nonzero(step == 1)[0]]
- self.nodes.append(Node(self.pd, self.state, support_feet, task))
+ models.append(self.create_model(support_feet, tasks))
support_feet = [self.pd.allContactIds[i] for i in np.nonzero(gait[-1] == 1)[0]]
- self.terminal_node = Node(self.pd, self.state, support_feet, isTerminal=True)
+ terminal_model = self.create_model(support_feet, is_terminal=True)
- self.models = [node.model for node in self.nodes]
- self.terminal_model = self.terminal_node.model
+ return models, terminal_model
def solve(self, x0, footstep, gait, xs_init=None, us_init=None):
t_start = time()
@@ -77,13 +76,15 @@ class OCP:
pin.updateFramePlacements(self.pd.model, self.pd.rdata)
if self.initialized:
- task = self.make_task(np.reshape(footstep, (3, 4), order="F"))
- support_feet = [self.pd.allContactIds[i] for i in np.nonzero(gait[-1] == 1)[0]]
-
- self.nodes[0].update_model(support_feet, task)
+ 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.problem.circularAppend(
- self.nodes[0].model, self.nodes[0].model.createData()
+ self.problem.runningModels[0],
+ self.problem.runningModels[0].createData(),
)
self.problem.x0 = self.x0
@@ -137,163 +138,132 @@ class OCP:
[acc.append(m.differential.xout) for m in self.ddp.problem.runningDatas]
return acc
+ def update_model(self, model, tasks, support_feet):
+ if tasks is not None:
+ for (id, pose) in tasks:
+ name = self.pd.model.frames[id].name + "_foot_tracking"
+ if name in model.differential.costs.active.tolist():
+ model.differential.costs.costs[name].cost.residual.reference = pose
+ else:
+ residual = crocoddyl.ResidualModelFrameTranslation(
+ self.state, id, pose, nu
+ )
+ cost = crocoddyl.CostModelResidual(self.state, residual)
+ model.differential.costs.addCost(
+ name, cost, self.pd.foot_tracking_w
+ )
-class Node:
- def __init__(
- self, pd, state, supportFootIds=[], swingFootTask=[], isTerminal=False
- ):
- self.pd = pd
- self.isTerminal = isTerminal
-
- self.state = state
- if pd.useFixedBase == 0:
- self.actuation = crocoddyl.ActuationModelFloatingBase(self.state)
- else:
- self.actuation = crocoddyl.ActuationModelFull(self.state)
- self.control = crocoddyl.ControlParametrizationModelPolyZero(self.actuation.nu)
- self.nu = self.actuation.nu
-
- self.createStandardModel(supportFootIds)
- if isTerminal:
- self.make_terminal_model()
- else:
- self.make_running_model(supportFootIds, swingFootTask)
+ for i in self.pd.allContactIds:
+ name = self.pd.model.frames[i].name + "_contact"
+ model.differential.contacts.changeContactStatus(name, i in support_feet)
- def createStandardModel(self, supportFootIds):
- """Action model for a swing foot phase.
+ def create_model(self, support_feet=[], tasks=[], is_terminal=False):
+ """
+ Create the action model
- :param timeStep: step duration of the action model
- :param supportFootIds: Ids of the constrained feet
- :param comTask: CoM task
- :param swingFootTask: swinging foot task
+ :param state: swinging foot task
+ :param support_feet: list of support feet ids
+ :param task: list of support feet ids and associated tracking reference
+ :param isTterminal: true for the terminal node
:return action model for a swing foot phase
"""
+ model = self.create_standard_model(support_feet)
+ if is_terminal:
+ self.make_terminal_model(model)
+ else:
+ self.make_running_model(model, support_feet, tasks)
- 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.contactModel.addContact(
- self.pd.model.frames[i].name + "_contact", supportContactModel
- )
+ return model
- # Creating the cost model for a contact phase
- costModel = crocoddyl.CostModelSum(self.state, self.nu)
+ def create_standard_model(self, support_feet):
+ """
+ Create a standard action model
- 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
- )
- costModel.addCost("stateReg", stateReg, 1)
+ :param state: swinging foot task
+ :param support_feet: list of support feet ids
+ :return action model for a swing foot phase
+ """
+ if self.pd.useFixedBase == 0:
+ actuation = crocoddyl.ActuationModelFloatingBase(self.state)
+ else:
+ actuation = crocoddyl.ActuationModelFull(self.state)
+ nu = actuation.nu
- self.costModel = costModel
+ control = crocoddyl.ControlParametrizationModelPolyZero(nu)
- 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
+ contacts = crocoddyl.ContactModelMultiple(self.state, nu)
+ for i in self.pd.allContactIds:
+ name = self.pd.model.frames[i].name + "_contact"
+ contact = crocoddyl.ContactModel3D(
+ self.state, i, np.zeros(3), nu, np.zeros(2)
+ )
+ contacts.addContact(name, contact)
+ if i not in support_feet:
+ contacts.changeContactStatus(name, False)
+
+ costs = crocoddyl.CostModelSum(self.state, nu)
+ 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)
+ costs.addCost("state_reg", state_cost, 1)
+
+ differential = crocoddyl.DifferentialActionModelContactFwdDynamics(
+ self.state, actuation, contacts, costs, 0.0, True
)
+ model = crocoddyl.IntegratedActionModelEuler(differential, control, self.pd.dt)
- def update_contact_model(self, supportFootIds):
- self.remove_contacts()
- 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.dmodel.contacts.addContact(
- self.pd.model.frames[i].name + "_contact", supportContactModel
- )
+ return model
- def make_terminal_model(self):
- self.isTerminal = True
- stateResidual = crocoddyl.ResidualModelState(self.state, self.pd.xref, self.nu)
- stateActivation = crocoddyl.ActivationModelWeightedQuad(
+ def make_terminal_model(self, model):
+ """
+ Add the final velocity cost to the terminal model
+ """
+ nu = model.differential.actuation.nu
+ residual = crocoddyl.ResidualModelState(self.state, self.pd.xref, nu)
+ activation = crocoddyl.ActivationModelWeightedQuad(
self.pd.terminal_velocity_w**2
)
- stateReg = crocoddyl.CostModelResidual(
- self.state, stateActivation, stateResidual
- )
- self.costModel.addCost("terminalVelocity", stateReg, 1)
+ state_cost = crocoddyl.CostModelResidual(self.state, activation, residual)
+ model.differential.costs.addCost("terminal_velocity", state_cost, 1)
- def make_running_model(self, supportFootIds, swingFootTask):
- for i in supportFootIds:
+ def make_running_model(self, model, support_feet, tasks):
+ """
+ Add all the costs to the running models
+ """
+ nu = model.differential.actuation.nu
+ costs = model.differential.costs
+ for i in support_feet:
cone = crocoddyl.FrictionCone(self.pd.Rsurf, self.pd.mu, 4, False)
- coneResidual = crocoddyl.ResidualModelContactFrictionCone(
- self.state, i, cone, self.nu
+ residual = crocoddyl.ResidualModelContactFrictionCone(
+ self.state, i, cone, nu
)
- coneActivation = crocoddyl.ActivationModelQuadraticBarrier(
+ activation = 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,
+ friction_cone = crocoddyl.CostModelResidual(
+ self.state, activation, residual
)
+ friction_name = self.pd.model.frames[id].name + "_friction_cone"
+ costs.addCost(friction_name, friction_cone, 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)
+ control_residual = crocoddyl.ResidualModelControl(self.state, self.pd.uref)
+ control_reg = crocoddyl.CostModelResidual(self.state, control_residual)
+ costs.addCost("control_reg", control_reg, self.pd.control_reg_w)
- ctrl_bound_residual = crocoddyl.ResidualModelControl(self.state, self.nu)
- ctrl_bound_activation = crocoddyl.ActivationModelQuadraticBarrier(
+ control_bound_residual = crocoddyl.ResidualModelControl(self.state, nu)
+ control_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
+ control_bound = crocoddyl.CostModelResidual(
+ self.state, control_bound_activation, control_bound_residual
)
- self.costModel.addCost("ctrlBound", ctrl_bound, self.pd.control_bound_w)
-
- self.tracking_cost(swingFootTask)
-
- def remove_running_costs(self):
- runningCosts = self.dmodel.costs.active.tolist()
- idx = runningCosts.index("stateReg")
- runningCosts.pop(idx)
- for cost in runningCosts:
- if cost in self.dmodel.costs.active.tolist():
- self.dmodel.costs.removeCost(cost)
-
- def remove_terminal_cost(self):
- if "terminalVelocity" in self.dmodel.costs.active.tolist():
- self.dmodel.costs.removeCost("terminalVelocity")
-
- def remove_contacts(self):
- allContacts = self.dmodel.contacts.contacts.todict()
- for c in allContacts:
- self.dmodel.contacts.removeContact(c)
-
- def tracking_cost(self, task):
- if task is not None:
- for (id, pose) in task:
- frameTranslationResidual = crocoddyl.ResidualModelFrameTranslation(
- self.state, id, pose, self.nu
- )
- footTrack = crocoddyl.CostModelResidual(
- self.state, frameTranslationResidual
- )
- if (
- self.pd.model.frames[id].name + "_footTrack"
- in self.dmodel.costs.active.tolist()
- ):
- self.dmodel.costs.removeCost(
- self.pd.model.frames[id].name + "_footTrack"
- )
- self.costModel.addCost(
- self.pd.model.frames[id].name + "_footTrack",
- footTrack,
- self.pd.foot_tracking_w,
- )
+ costs.addCost("control_bound", control_bound, self.pd.control_bound_w)
- def update_model(self, support_feet=[], task=[]):
- self.update_contact_model(support_feet)
- if not self.isTerminal:
- self.tracking_cost(task)
+ if tasks is not None:
+ for (id, pose) in tasks:
+ name = self.pd.model.frames[id].name + "_foot_tracking"
+ residual = crocoddyl.ResidualModelFrameTranslation(
+ self.state, id, pose, nu
+ )
+ foot_tracking = crocoddyl.CostModelResidual(self.state, residual)
+ costs.addCost(name, foot_tracking, self.pd.foot_tracking_w)
diff --git a/python/quadruped_reactive_walking/WB_MPC/ProblemData.py b/python/quadruped_reactive_walking/WB_MPC/ProblemData.py
index 5958260e62bed8d14d03bdd4cd7cc8b45c3a348f..e2a66019b2ce63fa4e4d26916263d8332a0480bf 100644
--- a/python/quadruped_reactive_walking/WB_MPC/ProblemData.py
+++ b/python/quadruped_reactive_walking/WB_MPC/ProblemData.py
@@ -9,7 +9,7 @@ class problemDataAbstract:
self.dt_wbc = param.dt_wbc
self.mpc_wbc_ratio = int(self.dt / self.dt_wbc)
self.init_steps = 0
- self.target_steps = 150
+ self.target_steps = 100
self.T = self.init_steps + self.target_steps - 1
self.robot = erd.load("solo12")
@@ -172,7 +172,7 @@ class ProblemDataFull(problemDataAbstract):
# Cost function weights
# Cost function weights
self.mu = 0.7
- self.foot_tracking_w = 1e3
+ self.foot_tracking_w = 1e4
# self.friction_cone_w = 1e3 * 0
self.control_bound_w = 1e3
self.control_reg_w = 1e0
diff --git a/python/quadruped_reactive_walking/WB_MPC/Target.py b/python/quadruped_reactive_walking/WB_MPC/Target.py
index fa0f055c02f664db8bea4dd60e966c33e158e171..a5b84bd6f97c8bd1e6a4fe9fa458eeef80ef9ae0 100644
--- a/python/quadruped_reactive_walking/WB_MPC/Target.py
+++ b/python/quadruped_reactive_walking/WB_MPC/Target.py
@@ -1,32 +1,138 @@
+from tracemalloc import take_snapshot
import numpy as np
from .ProblemData import ProblemData
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.updateFramePlacements(pd.model, pd.rdata)
- self.foot_pose = pd.rdata.oMf[pd.rfFootId].translation.copy()
- self.A = np.array([0, 0.03, 0.03])
- self.offset = np.array([0.05, -0.02, 0.06])
- self.freq = np.array([0, 0.5, 0.5])
- self.phase = np.array([0, np.pi / 2, 0])
- self.t_offset = 0
-
- def shift(self):
- self.t_offset += 1
-
- def evaluate_circle(self, t):
- return (
- self.foot_pose
- + self.offset
- + self.A
- * np.sin(2 * np.pi * self.freq * (self.t_offset + t) * self.dt + self.phase)
+ def __init__(self, params):
+ self.params = params
+ self.dt_wbc = params.dt_wbc
+ self.k_per_step = 160
+
+ self.position = np.array(params.footsteps_under_shoulders).reshape(
+ (3, 4), order="F"
)
- def footstep(self, t):
+ if params.movement == "circle":
+ self.A = np.array([0, 0.03, 0.03])
+ self.offset = np.array([0.05, -0.02, 0.06])
+ self.freq = np.array([0, 0.5, 0.5])
+ 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))
+ else:
+ self.target_footstep = self.position + np.array([0.0, 0.0, 0.10])
+
+ def compute(self, k):
footstep = np.zeros((3, 4))
- footstep[:, 1] = self.evaluate_circle(t)
+ 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 k_step == 0:
+ self.update_polynomial(initial, target)
+
+ 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]
diff --git a/python/quadruped_reactive_walking/WB_MPC_Wrapper.py b/python/quadruped_reactive_walking/WB_MPC_Wrapper.py
index 9de9dff2d4dfa9ab5c210303d7fcdfb371df3be8..25b65a510f81fe8c25d8e06529271fe49ac0cddc 100644
--- a/python/quadruped_reactive_walking/WB_MPC_Wrapper.py
+++ b/python/quadruped_reactive_walking/WB_MPC_Wrapper.py
@@ -112,6 +112,7 @@ class MPC_Wrapper:
self.last_available_result.xs = [x0 for _ in range(self.pd.T + 1)]
p = Process(target=self.MPC_asynchronous)
p.start()
+
self.add_new_data(k, x0, footstep, gait, xs, us)
def MPC_asynchronous(self):
diff --git a/python/quadruped_reactive_walking/main_solo12_control.py b/python/quadruped_reactive_walking/main_solo12_control.py
index 0e0ce5b30f33ee9477b5fb46ad58c0ef9803a5b7..41e0281876d44da730a5eeeb20944a4b3e0ccef2 100644
--- a/python/quadruped_reactive_walking/main_solo12_control.py
+++ b/python/quadruped_reactive_walking/main_solo12_control.py
@@ -161,7 +161,6 @@ def control_loop():
put_on_the_floor(device, q_init)
# CONTROL LOOP ***************************************************
-
t = 0.0
t_max = (params.N_SIMULATION - 1) * params.dt_wbc
diff --git a/python/quadruped_reactive_walking/tools/LoggerControl.py b/python/quadruped_reactive_walking/tools/LoggerControl.py
index 06a6cc2166497d84b343ccc0b95856f515005b82..468c4d41a1d088b6021cb9add3ace7dcac2ffbbb 100644
--- a/python/quadruped_reactive_walking/tools/LoggerControl.py
+++ b/python/quadruped_reactive_walking/tools/LoggerControl.py
@@ -100,7 +100,6 @@ class LoggerControl:
self.mocapOrientationQuat[self.i] = device.baseState[1]
# Controller timings: MPC time, ...
- self.target[self.i] = controller.target.evaluate_circle(0)
self.t_mpc[self.i] = controller.t_mpc
self.t_send[self.i] = controller.t_send
self.t_loop[self.i] = controller.t_loop
@@ -119,6 +118,13 @@ class LoggerControl:
self.t_loop[self.i] = controller.t_loop
self.t_ocp_ddp[self.i] = controller.mpc_result.solving_duration
+
+ 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]
+ 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]
+
if not self.params.enable_multiprocessing:
self.t_ocp_update[self.i] = controller.mpc.ocp.t_update
self.t_ocp_warm_start[self.i] = controller.mpc.ocp.t_warm_start
diff --git a/python/quadruped_reactive_walking/tools/Utils.py b/python/quadruped_reactive_walking/tools/Utils.py
new file mode 100644
index 0000000000000000000000000000000000000000..1af8310e6872778cfb4693ddd2fb2fb2e0ffb3f3
--- /dev/null
+++ b/python/quadruped_reactive_walking/tools/Utils.py
@@ -0,0 +1,70 @@
+from example_robot_data import load
+import numpy as np
+import pinocchio as pin
+
+
+def init_robot(q_init, params):
+ """Load the solo model and initialize the Gepetto viewer if it is enabled
+
+ Args:
+ q_init (array): the default position of the robot actuators
+ params (object): store parameters
+ """
+ # Load robot model and data
+ solo = load("solo12")
+ q = solo.q0.reshape((-1, 1))
+
+ # Initialisation of the position of footsteps to be under the shoulder
+ # There is a lateral offset of around 7 centimeters
+ fsteps_under_shoulders = np.zeros((3, 4))
+ indexes = [
+ solo.model.getFrameId("FL_FOOT"),
+ solo.model.getFrameId("FR_FOOT"),
+ solo.model.getFrameId("HL_FOOT"),
+ solo.model.getFrameId("HR_FOOT"),
+ ]
+ q[7:, 0] = 0.0
+ pin.framesForwardKinematics(solo.model, solo.data, q)
+ for i in range(4):
+ fsteps_under_shoulders[:, i] = solo.data.oMf[indexes[i]].translation
+ fsteps_under_shoulders[2, :] = 0.0
+
+ # Initial angular positions of actuators
+ q[7:, 0] = q_init
+
+ # Initialisation of model quantities
+ pin.centerOfMass(solo.model, solo.data, q, np.zeros((18, 1)))
+ pin.updateFramePlacements(solo.model, solo.data)
+ pin.crba(solo.model, solo.data, solo.q0)
+
+ # Initialisation of the position of footsteps
+ fsteps_init = np.zeros((3, 4))
+ h_init = 0.0
+ for i in range(4):
+ fsteps_init[:, i] = solo.data.oMf[indexes[i]].translation
+ h = (solo.data.oMf[1].translation - solo.data.oMf[indexes[i]].translation)[2]
+ if h > h_init:
+ h_init = h
+ fsteps_init[2, :] = 0.0
+
+ # Initialisation of the position of shoulders
+ shoulders_init = np.zeros((3, 4))
+ indexes = [4, 12, 20, 28] # Shoulder indexes
+ for i in range(4):
+ shoulders_init[:, i] = solo.data.oMf[indexes[i]].translation
+
+ # Saving data
+ params.h_ref = h_init
+ # Mass of the whole urdf model (also = to Ycrb[1].mass)
+ params.mass = solo.data.mass[0]
+ # Composite rigid body inertia in q_init position
+ params.I_mat = solo.data.Ycrb[1].inertia.ravel().tolist()
+ params.CoM_offset = (solo.data.com[0][:3] - q[0:3, 0]).tolist()
+ params.CoM_offset[1] = 0.0
+
+ # Â Use initial feet pos as reference
+ for i in range(4):
+ for j in range(3):
+ params.shoulders[3 * i + j] = shoulders_init[j, i]
+ params.footsteps_init[3 * i + j] = fsteps_init[j, i]
+ params.footsteps_under_shoulders[3 * i + j] = fsteps_init[j, i]
diff --git a/src/Params.cpp b/src/Params.cpp
index fdea1b25e5d787666f2fb382b8580ac644b8e907..e20658378d5d2d85033a8738697f45e2a616715b 100644
--- a/src/Params.cpp
+++ b/src/Params.cpp
@@ -24,6 +24,7 @@ Params::Params()
N_periods(0),
type_MPC(0),
save_guess(false),
+ movement(""),
interpolate_mpc(true),
interpolation_type(0),
kf_enabled(false),
@@ -144,6 +145,9 @@ void Params::initialize(const std::string& file_path) {
assert_yaml_parsing(robot_node, "robot", "save_guess");
save_guess = robot_node["save_guess"].as<bool>();
+ assert_yaml_parsing(robot_node, "robot", "movement");
+ movement = robot_node["movement"].as<std::string>();
+
assert_yaml_parsing(robot_node, "robot", "interpolate_mpc");
interpolate_mpc = robot_node["interpolate_mpc"].as<bool>();