diff --git a/python/quadruped_reactive_walking/WB_MPC/CasadiOCP.py b/python/quadruped_reactive_walking/WB_MPC/CasadiOCP.py
index 885cc9555534d3f54df88414e70c85d59082244d..b1346863f51af787341ce0360237449a9340d1d5 100644
--- a/python/quadruped_reactive_walking/WB_MPC/CasadiOCP.py
+++ b/python/quadruped_reactive_walking/WB_MPC/CasadiOCP.py
@@ -12,27 +12,27 @@ import pinocchio.casadi as cpin
class NodeData:
def __init__(self, isTerminal=False):
- self.x = None
- self.cost = None
- self.isTerminal = isTerminal
+ self.x = None # State vector (q + dq)
+ self.cost = None # Cost for this node
+ self.isTerminal = isTerminal # True if last node of shooting problem
if not isTerminal:
- self.a = None
- self.u = None
- self.f = None
- self.xnext = None
+ self.a = None # Acceleration slack variables (ddq)
+ self.u = None # Control vector (tau)
+ self.f = None # Contact forces
+ self.xnext = None # Next state vector
class OcpResult:
def __init__(self):
- self.xs = None
- self.a = None
- self.us = None
- self.fs = None
- self.K = None
- self.f_world = None
- self.q = None
- self.v = None
- self.solver_time = None
+ self.xs = None # State trajectory (q + dq)
+ self.a = None # Acceleration trajectory (ddq)
+ self.us = None # Control trajectory (tau)
+ self.fs = None # Contact forces trajectories
+ # self.K = None
+ # self.f_world = None
+ # self.q = None
+ # self.v = None
+ # self.solver_time = None
class OCP:
@@ -50,16 +50,24 @@ class OCP:
self.results = OcpResult()
# build the different shooting nodes
- self.shooting_nodes = {contacts: ShootingNode(pd, contacts)
- for contacts in set([tuple(self.pd.allContactIds[i] for i in np.nonzero(step == 1)[0]) for step in gait[:-1]])}
+ self.shooting_nodes = {
+ contacts: ShootingNode(pd, contacts)
+ for contacts in set(
+ [
+ tuple(self.pd.allContactIds[i] for i in np.nonzero(step == 1)[0])
+ for step in gait[:-1]
+ ]
+ )
+ }
def solve(self, x0, tasks, gait, guess={}):
""" Solve the NLP
:param guess: ditctionary in the form: {'xs':array([T+1, n_states]), 'acs':array([T, nv]),\
'us':array([T, nu]), 'fs': [array([T, nv]),]}
"""
- self.runningModels = [self.shooting_nodes[self.pattern_to_id(gait[t])]
- for t in range(self.pd.T)]
+ self.runningModels = [
+ self.shooting_nodes[self.pattern_to_id(gait[t])] for t in range(self.pd.T)
+ ]
self.terminalModel = self.shooting_nodes[self.pattern_to_id(gait[self.pd.T])]
self.datas = []
for i in range(self.pd.T + 1):
@@ -75,14 +83,15 @@ class OCP:
self.opti.subject_to(eq_constraints == 0)
p_opts = {"expand": False}
- s_opts = {"tol": 1e-4,
- "acceptable_tol": 1e-4,
- # "max_iter": 21,
- # "compl_inf_tol": 1e-2,
- # "constr_viol_tol": 1e-2
- # "resto_failure_feasibility_threshold": 1
- # "linear_solver": "ma57"
- }
+ s_opts = {
+ "tol": 1e-4,
+ "acceptable_tol": 1e-4,
+ # "max_iter": 21,
+ # "compl_inf_tol": 1e-2,
+ # "constr_viol_tol": 1e-2
+ # "resto_failure_feasibility_threshold": 1
+ # "linear_solver": "ma57"
+ }
self.opti.solver("ipopt", p_opts, s_opts)
@@ -102,12 +111,16 @@ class OCP:
opti = casadi.Opti()
self.opti = opti
# Optimization variables
- self.dxs = [opti.variable(self.pd.ndx)
- for _ in self.runningModels+[self.terminalModel]]
+ self.dxs = [
+ opti.variable(self.pd.ndx)
+ for _ in self.runningModels + [self.terminalModel]
+ ]
self.acs = [opti.variable(self.pd.nv) for _ in self.runningModels]
self.us = [opti.variable(self.pd.nu) for _ in self.runningModels]
- self.xs = [m.integrate(x0, dx) for m, dx in zip(
- self.runningModels+[self.terminalModel], self.dxs)]
+ self.xs = [
+ m.integrate(x0, dx)
+ for m, dx in zip(self.runningModels + [self.terminalModel], self.dxs)
+ ]
self.fs = []
for m in self.runningModels:
f_tmp = [opti.variable(3) for _ in range(len(m.contactIds))]
@@ -121,29 +134,32 @@ class OCP:
for t in range(self.pd.T):
# These change every time! Do not use runningModels[0].x to get the state!!!! use xs[0]
self.runningModels[t].init(
- self.datas[t], self.xs[t], self.acs[t], self.us[t], self.fs[t], False)
+ self.datas[t], self.xs[t], self.acs[t], self.us[t], self.fs[t], False
+ )
- xnext, rcost = self.runningModels[t].calc(x_ref=self.pd.xref,
- u_ref=self.pd.uref,
- target=tasks)
+ xnext, rcost = self.runningModels[t].calc(
+ x_ref=self.pd.xref, u_ref=self.pd.uref, target=tasks
+ )
# If it is landing
- if (gait[t] != gait[t-1]).any():
- print('Contact on ', str(self.runningModels[t].contactIds))
- self.runningModels[t].constraint_landing_feet_cost(
- self.pd.xref)
+ if (gait[t] != gait[t - 1]).any():
+ print("Contact on ", str(self.runningModels[t].contactIds))
+ self.runningModels[t].constraint_landing_feet_cost(self.pd.xref)
# Constraints
eq.append(self.runningModels[t].constraint_standing_feet_eq())
eq.append(self.runningModels[t].constraint_dynamics_eq())
- eq.append(self.runningModels[t].difference(
- self.xs[t + 1], xnext) - np.zeros(self.pd.ndx))
+ eq.append(
+ self.runningModels[t].difference(self.xs[t + 1], xnext)
+ - np.zeros(self.pd.ndx)
+ )
totalcost += rcost
- eq.append(self.xs[self.pd.T][self.pd.nq:])
+ eq.append(self.xs[self.pd.T][self.pd.nq :])
self.terminalModel.init(
- data=self.datas[self.pd.T], x=self.xs[self.pd.T], isTerminal=True)
+ data=self.datas[self.pd.T], x=self.xs[self.pd.T], isTerminal=True
+ )
totalcost += self.terminalModel.calc(x_ref=self.pd.xref)[1]
totalcost += rcost
@@ -158,16 +174,21 @@ class OCP:
return 0
try:
- xs_g = np.array(guess['xs'])
- us_g = np.array(guess['us'])
- acs_g = np.array(guess['acs'])
- fs_g = guess['fs']
+ xs_g = np.array(guess["xs"])
+ us_g = np.array(guess["us"])
+ acs_g = np.array(guess["acs"])
+ fs_g = guess["fs"]
def xdiff(x1, x2):
- return np.concatenate([
- pin.difference(
- self.pd.model, x1[:self.pd.nq], x2[:self.pd.nq]),
- x2[self.pd.nq:]-x1[self.pd.nq:]])
+ return np.concatenate(
+ [
+ pin.difference(
+ self.pd.model, x1[: self.pd.nq], x2[: self.pd.nq]
+ ),
+ x2[self.pd.nq :] - x1[self.pd.nq :],
+ ]
+ )
+
for x, xg in zip(self.dxs, xs_g):
self.opti.set_initial(x, xdiff(x0, xg))
for a, ag in zip(self.acs, acs_g):
@@ -196,13 +217,22 @@ class OCP:
for i, sw_foot in enumerate(self.runningModels[t].freeIds):
fsol[sw_foot].append(np.zeros(3))
- fsol_to_ws.append(np.concatenate([self.opti.value(
- self.fs[t][j]) for j in range(len(self.runningModels[t].contactIds))]))
+ fsol_to_ws.append(
+ np.concatenate(
+ [
+ self.opti.value(self.fs[t][j])
+ for j in range(len(self.runningModels[t].contactIds))
+ ]
+ )
+ )
pin.framesForwardKinematics(
- self.pd.model, self.pd.rdata, np.array(xs_sol)[t, : self.pd.nq])
- [fs_world[foot].append(
- self.pd.rdata.oMf[foot].rotation @ fsol[foot][t]) for foot in fs_world]
+ self.pd.model, self.pd.rdata, np.array(xs_sol)[t, : self.pd.nq]
+ )
+ [
+ fs_world[foot].append(self.pd.rdata.oMf[foot].rotation @ fsol[foot][t])
+ for foot in fs_world
+ ]
for foot in fs_world:
fs_world[foot] = np.array(fs_world[foot])
@@ -214,7 +244,7 @@ class OCP:
return self.results.x, self.results.a, self.results.u, self.results.fs, 0
def get_feet_position(self, xs_sol):
- """ Get the feet positions computed durnig one ocp
+ """Get the feet positions computed durnig one ocp
:param xs_sol: Array of dimension [n_steps, n_states]
@@ -229,7 +259,7 @@ class OCP:
return feet_log
def get_feet_velocity(self, xs_sol):
- """ Get the feet velocities in LOCAL_WORLD_ALIGNED frame computed durnig one ocp
+ """Get the feet velocities in LOCAL_WORLD_ALIGNED frame computed durnig one ocp
:param xs_sol: Array of dimension [n_steps, n_states]
@@ -244,16 +274,20 @@ class OCP:
return feet_log
def get_base_log(self, xs_sol):
- """ Get the base positions computed durnig one ocp
+ """Get the base positions computed durnig one ocp
:param xs_sol: Array of dimension [n_steps, n_states]
"""
base_pos_log = []
- [base_pos_log.append(self.terminalModel.baseTranslation(
- xs_sol[i]).full()[:, 0]) for i in range(len(xs_sol))]
+ [
+ base_pos_log.append(
+ self.terminalModel.baseTranslation(xs_sol[i]).full()[:, 0]
+ )
+ for i in range(len(xs_sol))
+ ]
base_pos_log = np.array(base_pos_log)
return base_pos_log
-
+
def pattern_to_id(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)
diff --git a/python/quadruped_reactive_walking/WB_MPC/ProblemData.py b/python/quadruped_reactive_walking/WB_MPC/ProblemData.py
index c55049357318541a71062447c102140f4b2827fd..37d20b728b99051c961f037b59db82a76a78977d 100644
--- a/python/quadruped_reactive_walking/WB_MPC/ProblemData.py
+++ b/python/quadruped_reactive_walking/WB_MPC/ProblemData.py
@@ -14,13 +14,7 @@ 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] = np.array([0.0, 0.0, 0.2607495, 0, 0, 0, 1])
self.q0[7:] = param.q_init
self.model = self.robot.model
@@ -30,8 +24,7 @@ class problemDataAbstract:
self.frozen_names = frozen_names
if frozen_names != []:
- self.frozen_idxs = [self.model.getJointId(
- id) for id in frozen_names]
+ self.frozen_idxs = [self.model.getJointId(id) for id in frozen_names]
self.freeze()
self.nq = self.model.nq
@@ -92,29 +85,29 @@ class ProblemData(problemDataAbstract):
# Cost function weights
# Cost function weights
self.mu = 0.7
- self.base_velocity_tracking_w = 1e2*0
- self.ground_collision_w = 1e3*0
+ self.base_velocity_tracking_w = 1e2 * 0
+ self.ground_collision_w = 1e3 * 0
self.friction_cone_w = 1e4
- self.impact_altitude_w= 1e3
+ self.impact_altitude_w = 1e3
self.impact_velocity_w = 1e3
- self.control_bound_w = 1e3*0
+ self.control_bound_w = 1e3 * 0
self.control_reg_w = 1e0
- self.state_reg_w = np.array([0] * 3
- + [1e1] * 3
- + [1e1] * 12
- + [0] * 6
- + [1e1] * 3
- + [1e1] * 3
- + [1e1] * 6
- )
- self.terminal_velocity_w = np.array(
- [0] * self.nv + [1e4] * self.nv)
+ self.state_reg_w = np.array(
+ [0] * 3
+ + [1e1] * 3
+ + [1e1] * 12
+ + [0] * 6
+ + [1e1] * 3
+ + [1e1] * 3
+ + [1e1] * 6
+ )
+ self.terminal_velocity_w = np.array([0] * self.nv + [1e4] * self.nv)
self.force_reg_w = 1e0
self.xref = self.x0
- self.uref =self.u0
+ self.uref = self.u0
class ProblemDataFull(problemDataAbstract):
@@ -132,8 +125,7 @@ class ProblemDataFull(problemDataAbstract):
self.control_bound_w = 1e3
self.control_reg_w = 1e0
self.state_reg_w = np.array(
- [1e2] * 3 + [1e-2] * 3 + [1e2] * 6 +
- [1e1] * 3 + [1e0] * 3 + [1e1] * 6
+ [1e2] * 3 + [1e-2] * 3 + [1e2] * 6 + [1e1] * 3 + [1e0] * 3 + [1e1] * 6
)
self.terminal_velocity_w = np.array([0] * 12 + [1e3] * 12)
diff --git a/python/quadruped_reactive_walking/WB_MPC/ShootingNode.py b/python/quadruped_reactive_walking/WB_MPC/ShootingNode.py
index a9b9c4c77abfed8c8eea0f8473757a48a2e7d914..4bff371a932e07a9b38c41dc1da8e7d1a55dc2e8 100644
--- a/python/quadruped_reactive_walking/WB_MPC/ShootingNode.py
+++ b/python/quadruped_reactive_walking/WB_MPC/ShootingNode.py
@@ -8,7 +8,7 @@ from pinocchio import casadi as cpin
from .ProblemData import ProblemData
-class ShootingNode():
+class ShootingNode:
def __init__(self, pd: ProblemData, contactIds):
self.pd = pd
@@ -18,13 +18,12 @@ class ShootingNode():
cmodel = cpin.Model(pd.model)
cdata = cmodel.createData()
- self.baseID = pd.model.getFrameId('base_link')
+ self.baseID = pd.model.getFrameId("base_link")
pin.framesForwardKinematics(pd.model, pd.rdata, pd.q0)
- self.robotweight = - \
- sum([Y.mass for Y in pd.model.inertias]) * \
- pd.model.gravity.linear[2]
- [self.freeIds.append(idf)
- for idf in pd.allContactIds if idf not in contactIds]
+ self.robotweight = (
+ -sum([Y.mass for Y in pd.model.inertias]) * pd.model.gravity.linear[2]
+ )
+ [self.freeIds.append(idf) for idf in pd.allContactIds if idf not in contactIds]
cx = casadi.SX.sym("x", pd.nx, 1)
cq = casadi.SX.sym("q", pd.nq, 1)
@@ -34,87 +33,144 @@ class ShootingNode():
ca = casadi.SX.sym("a", pd.nv, 1)
ctau = casadi.SX.sym("tau", pd.ntau, 1)
cdx = casadi.SX.sym("dx", pd.ndx, 1)
- cfs = [casadi.SX.sym("f"+cmodel.frames[idf].name, 3, 1)
- for idf in self.contactIds]
- R = casadi.SX.sym('R', 3, 3)
- R_ref = casadi.SX.sym('R_ref', 3, 3)
+ cfs = [
+ casadi.SX.sym("f" + cmodel.frames[idf].name, 3, 1)
+ for idf in self.contactIds
+ ]
+ R = casadi.SX.sym("R", 3, 3)
+ R_ref = casadi.SX.sym("R_ref", 3, 3)
# Build force list for ABA
forces = [cpin.Force.Zero() for _ in cmodel.joints]
# I am supposing here that all contact frames are on separate joints. This is asserted below:
- assert(len(set([cmodel.frames[idf].parentJoint for idf in contactIds])) == len(
- contactIds))
+ assert len(set([cmodel.frames[idf].parentJoint for idf in contactIds])) == len(
+ contactIds
+ )
for f, idf in zip(cfs, self.contactIds):
# Contact forces introduced in ABA as spatial forces at joint frame. F (opti variable) is defined at contact frame
- forces[cmodel.frames[idf].parentJoint] = cmodel.frames[idf].placement * \
- cpin.Force(f, 0*f)
+ forces[cmodel.frames[idf].parentJoint] = cmodel.frames[
+ idf
+ ].placement * cpin.Force(f, 0 * f)
self.forces = cpin.StdVec_Force()
for f in forces:
self.forces.append(f)
- acc = cpin.aba(cmodel, cdata, cx[:pd.nq],
- cx[pd.nq:], ctau, self.forces)
+ acc = cpin.aba(cmodel, cdata, cx[: pd.nq], cx[pd.nq :], ctau, self.forces)
# Casadi Functions to wrap pinocchio methods (needed cause pinocchio.casadi methods can't handle MX variables used in potimization)
# acceleration(x,u,f) = ABA(q,v,tau,f) with x=q,v, tau=u, and f built using StdVec_Force syntaxt
- self.acc = casadi.Function('acc', [cx, ctau]+cfs, [acc])
+ self.acc = casadi.Function("acc", [cx, ctau] + cfs, [acc])
# integrate(x,dx) = [q+dq,v+dv], with the q+dq function implemented with pin.integrate.
- self.integrate = casadi.Function('xplus', [cx, cdx], [casadi.vertcat(cpin.integrate(cmodel, cx[:pd.nq], cdx[:pd.nv]),
- cx[-pd.nv:]+cdx[-pd.nv:])])
+ self.integrate = casadi.Function(
+ "xplus",
+ [cx, cdx],
+ [
+ casadi.vertcat(
+ cpin.integrate(cmodel, cx[: pd.nq], cdx[: pd.nv]),
+ cx[-pd.nv :] + cdx[-pd.nv :],
+ )
+ ],
+ )
# integrate_q(q,dq) = pin.integrate(q,dq)
self.integrate_q = casadi.Function(
- 'qplus', [cq, cv], [cpin.integrate(cmodel, cq, cv)])
+ "qplus", [cq, cv], [cpin.integrate(cmodel, cq, cv)]
+ )
# Lie difference(x1,x2) = [ pin.difference(q1,q2),v2-v1 ]
- self.difference = casadi.Function('xminus', [cx, cx2], [casadi.vertcat(cpin.difference(cmodel, cx2[:pd.nq], cx[:pd.nq]),
- cx2[pd.nq:]-cx[pd.nq:])])
+ self.difference = casadi.Function(
+ "xminus",
+ [cx, cx2],
+ [
+ casadi.vertcat(
+ cpin.difference(cmodel, cx2[: pd.nq], cx[: pd.nq]),
+ cx2[pd.nq :] - cx[pd.nq :],
+ )
+ ],
+ )
# Reference frame in which velocities of base will be expressed (for locomotion)
vel_reference_frame = pin.LOCAL
- cpin.forwardKinematics(cmodel, cdata, cx[:pd.nq], cx[pd.nq:], ca)
+ cpin.forwardKinematics(cmodel, cdata, cx[: pd.nq], cx[pd.nq :], ca)
cpin.updateFramePlacements(cmodel, cdata)
# com(x) = centerOfMass(x[:nq])
self.com = casadi.Function(
- 'com', [cx], [cpin.centerOfMass(cmodel, cdata, cx[:pd.nq])])
+ "com", [cx], [cpin.centerOfMass(cmodel, cdata, cx[: pd.nq])]
+ )
# Base link position and orientation
self.baseTranslation = casadi.Function(
- 'base_translation', [cx], [cdata.oMf[self.baseID].translation])
+ "base_translation", [cx], [cdata.oMf[self.baseID].translation]
+ )
self.baseRotation = casadi.Function(
- 'base_rotation', [cx], [cdata.oMf[self.baseID].rotation])
+ "base_rotation", [cx], [cdata.oMf[self.baseID].rotation]
+ )
# Base velocity
- self.baseVelocityLin = casadi.Function('base_velocity_linear', [cx],
- [cpin.getFrameVelocity(cmodel, cdata, self.baseID, vel_reference_frame).linear])
- self.baseVelocityAng = casadi.Function('base_velocity_angular', [cx],
- [cpin.getFrameVelocity(cmodel, cdata, self.baseID, vel_reference_frame).angular])
+ self.baseVelocityLin = casadi.Function(
+ "base_velocity_linear",
+ [cx],
+ [
+ cpin.getFrameVelocity(
+ cmodel, cdata, self.baseID, vel_reference_frame
+ ).linear
+ ],
+ )
+ self.baseVelocityAng = casadi.Function(
+ "base_velocity_angular",
+ [cx],
+ [
+ cpin.getFrameVelocity(
+ cmodel, cdata, self.baseID, vel_reference_frame
+ ).angular
+ ],
+ )
# feet[c](x) = position of the foot <c> at configuration q=x[:nq]
- self.feet = {idf: casadi.Function(
- 'foot'+cmodel.frames[idf].name, [cx], [cdata.oMf[idf].translation]) for idf in pd.allContactIds}
+ self.feet = {
+ idf: casadi.Function(
+ "foot" + cmodel.frames[idf].name, [cx], [cdata.oMf[idf].translation]
+ )
+ for idf in pd.allContactIds
+ }
# Rfeet[c](x) = orientation of the foot <c> at configuration q=x[:nq]
- self.Rfeet = {idf: casadi.Function(
- 'Rfoot'+cmodel.frames[idf].name, [cx], [cdata.oMf[idf].rotation]) for idf in pd.allContactIds}
+ self.Rfeet = {
+ idf: casadi.Function(
+ "Rfoot" + cmodel.frames[idf].name, [cx], [cdata.oMf[idf].rotation]
+ )
+ for idf in pd.allContactIds
+ }
# vfeet[c](x) = linear velocity of the foot <c> at configuration q=x[:nq] with vel v=x[nq:]
- self.vfeet = {idf: casadi.Function('vfoot'+cmodel.frames[idf].name,
- [cx], [cpin.getFrameVelocity(cmodel, cdata, idf, pin.LOCAL).linear])
- for idf in pd.allContactIds}
+ self.vfeet = {
+ idf: casadi.Function(
+ "vfoot" + cmodel.frames[idf].name,
+ [cx],
+ [cpin.getFrameVelocity(cmodel, cdata, idf, pin.LOCAL).linear],
+ )
+ for idf in pd.allContactIds
+ }
# vfeet[c](x,a) = linear acceleration of the foot <c> at configuration q=x[:nq] with vel v=x[nq:] and acceleration a
- self.afeet = {idf: casadi.Function('afoot'+cmodel.frames[idf].name,
- [cx, ca], [cpin.getFrameClassicalAcceleration(cmodel, cdata, idf,
- pin.LOCAL).linear])
- for idf in pd.allContactIds}
+ self.afeet = {
+ idf: casadi.Function(
+ "afoot" + cmodel.frames[idf].name,
+ [cx, ca],
+ [
+ cpin.getFrameClassicalAcceleration(
+ cmodel, cdata, idf, pin.LOCAL
+ ).linear
+ ],
+ )
+ for idf in pd.allContactIds
+ }
# wrapper for the inverse of the exponential map
- self.log3 = casadi.Function(
- 'log', [R, R_ref], [cpin.log3(R.T @ R_ref)])
+ self.log3 = casadi.Function("log", [R, R_ref], [cpin.log3(R.T @ R_ref)])
def init(self, data, x, a=None, u=None, fs=None, isTerminal=False):
self.data = data
@@ -132,28 +188,27 @@ class ShootingNode():
self.fs = fs
def calc(self, x_ref, u_ref=None, target={}):
- '''
+ """
This function return xnext,cost
- '''
+ """
dt = self.pd.dt
# Euler integration, using directly the acceleration <a> introduced as a slack variable.
use_rk2 = False
if not use_rk2:
- vnext = self.x[self.pd.nq:] + self.a*dt
- qnext = self.integrate_q(self.x[:self.pd.nq], vnext*dt)
+ vnext = self.x[self.pd.nq :] + self.a * dt
+ qnext = self.integrate_q(self.x[: self.pd.nq], vnext * dt)
else:
# half-dt step over x=(q,v)
- vm = self.x[self.pd.nq:] + self.a*.5*dt
- qm = self.integrate_q(
- self.x[:self.pd.nq], .5 * self.x[self.pd.nq:]*dt)
+ vm = self.x[self.pd.nq :] + self.a * 0.5 * dt
+ qm = self.integrate_q(self.x[: self.pd.nq], 0.5 * self.x[self.pd.nq :] * dt)
xm = casadi.vertcat(qm, vm)
amid = self.acc(xm, self.tau, *self.fs)
# then simple Euler step over (qm, vm)
- qnext = self.integrate_q(qm, vm*dt)
- vnext = vm + amid*dt
+ qnext = self.integrate_q(qm, vm * dt)
+ vnext = vm + amid * dt
xnext = casadi.vertcat(qnext, vnext)
self.data.xnext = xnext
@@ -167,18 +222,27 @@ class ShootingNode():
def constraint_landing_feet_cost(self, x_ref):
eq = []
for stFoot in self.contactIds:
- self.cost += 0.5 * self.pd.impact_altitude_w * \
- casadi.sumsqr(self.feet[stFoot](self.x)[2]
- - self.feet[stFoot](x_ref)[2]) * self.pd.dt
- self.cost += 0.5 * self.pd.impact_velocity_w * \
- casadi.sumsqr(self.vfeet[stFoot](self.x)) * self.pd.dt
+ self.cost += (
+ 0.5
+ * self.pd.impact_altitude_w
+ * casadi.sumsqr(
+ self.feet[stFoot](self.x)[2] - self.feet[stFoot](x_ref)[2]
+ )
+ * self.pd.dt
+ )
+ self.cost += (
+ 0.5
+ * self.pd.impact_velocity_w
+ * casadi.sumsqr(self.vfeet[stFoot](self.x))
+ * self.pd.dt
+ )
def constraint_standing_feet_eq(self):
eq = []
for stFoot in self.contactIds:
eq.append(self.afeet[stFoot](self.x, self.a)) # stiff contact
- return(casadi.vertcat(*eq))
+ return casadi.vertcat(*eq)
def constraint_standing_feet_ineq(self):
# Friction cone
@@ -193,7 +257,7 @@ class ShootingNode():
ineq.append( fw[1] - self.pd.mu*fw[2] )
ineq.append( -fw[1] - self.pd.mu*fw[2] ) """
- return (casadi.vertcat(*ineq))
+ return casadi.vertcat(*ineq)
def constraint_standing_feet_cost(self):
# Friction cone
@@ -202,72 +266,114 @@ class ShootingNode():
f_ = self.fs[i]
fw = f_ # PREMULTIPLY BY R !!!!!
- A = np.matrix([[1, 0, -self.pd.mu], [-1, 0, -self.pd.mu], [6.1234234 * 1e-17, 1, -self.pd.mu],
- [-6.1234234*1e-17, -1, -self.pd.mu], [0, 0, 1]])
- lb = np.array([-casadi.inf, -casadi.inf, -
- casadi.inf, -casadi.inf, 0])
+ A = np.matrix(
+ [
+ [1, 0, -self.pd.mu],
+ [-1, 0, -self.pd.mu],
+ [6.1234234 * 1e-17, 1, -self.pd.mu],
+ [-6.1234234 * 1e-17, -1, -self.pd.mu],
+ [0, 0, 1],
+ ]
+ )
+ lb = np.array([-casadi.inf, -casadi.inf, -casadi.inf, -casadi.inf, 0])
ub = np.array([0, 0, 0, 0, casadi.inf])
r = A @ fw
- self.cost += self.pd.friction_cone_w * \
- casadi.sumsqr(casadi.if_else(r <= lb, r, 0))/2 * self.pd.dt
- self.cost += self.pd.friction_cone_w * \
- casadi.sumsqr(casadi.if_else(r >= ub, r, 0))/2 * self.pd.dt
+ self.cost += (
+ self.pd.friction_cone_w
+ * casadi.sumsqr(casadi.if_else(r <= lb, r, 0))
+ / 2
+ * self.pd.dt
+ )
+ self.cost += (
+ self.pd.friction_cone_w
+ * casadi.sumsqr(casadi.if_else(r >= ub, r, 0))
+ / 2
+ * self.pd.dt
+ )
def constraint_control_cost(self):
- self.cost += self.pd.control_bound_w * \
- casadi.sumsqr(casadi.if_else(
- self.u <= -self.pd.effort_limit, self.u, 0))/2 * self.pd.dt
- self.cost += self.pd.control_bound_w * \
- casadi.sumsqr(casadi.if_else(
- self.u >= self.pd.effort_limit, self.u, 0))/2 * self.pd.dt
+ self.cost += (
+ self.pd.control_bound_w
+ * casadi.sumsqr(casadi.if_else(self.u <= -self.pd.effort_limit, self.u, 0))
+ / 2
+ * self.pd.dt
+ )
+ self.cost += (
+ self.pd.control_bound_w
+ * casadi.sumsqr(casadi.if_else(self.u >= self.pd.effort_limit, self.u, 0))
+ / 2
+ * self.pd.dt
+ )
def constraint_dynamics_eq(self):
eq = []
eq.append(self.acc(self.x, self.tau, *self.fs) - self.a)
- return(casadi.vertcat(*eq))
+ return casadi.vertcat(*eq)
def constraint_swing_feet_cost(self, x_ref):
ineq = []
for sw_foot in self.freeIds:
r = self.feet[sw_foot](self.x)[2] - self.feet[sw_foot](x_ref)[2]
- c = 0.5 * self.pd.dt * \
- self.pd.ground_collision_w * casadi.sumsqr(r)
+ c = 0.5 * self.pd.dt * self.pd.ground_collision_w * casadi.sumsqr(r)
self.cost += casadi.if_else(r < 0, c, 0)
# ineq.append(self.vfeet[sw_foot](self.x)[
# 0:2] - k * self.feet[sw_foot](self.x)[2])
- #ineq.append(self.vfeet[sw_foot](self.x)[1] - k[1]* self.feet[sw_foot](self.x)[2])
+ # ineq.append(self.vfeet[sw_foot](self.x)[1] - k[1]* self.feet[sw_foot](self.x)[2])
def force_reg_cost(self):
for i, stFoot in enumerate(self.contactIds):
R = self.Rfeet[stFoot](self.x)
f_ = self.fs[i]
fw = R @ f_
- self.cost += self.pd.force_reg_w * casadi.norm_2(fw[2] -
- self.robotweight/len(self.contactIds)) * self.pd.dt
+ self.cost += (
+ self.pd.force_reg_w
+ * casadi.norm_2(fw[2] - self.robotweight / len(self.contactIds))
+ * self.pd.dt
+ )
def control_cost(self, u_ref):
- self.cost += 1/2*self.pd.control_reg_w * \
- casadi.sumsqr(self.u - u_ref) * self.pd.dt
+ self.cost += (
+ 1 / 2 * self.pd.control_reg_w * casadi.sumsqr(self.u - u_ref) * self.pd.dt
+ )
def body_reg_cost(self, x_ref):
if self.isTerminal:
- self.cost += 1/2 * \
- casadi.sumsqr(self.pd.state_reg_w *
- self.difference(self.x, x_ref))
- self.cost += 1/2 * \
- casadi.sumsqr(self.pd.terminal_velocity_w *
- self.difference(self.x, x_ref))
+ self.cost += (
+ 1
+ / 2
+ * casadi.sumsqr(self.pd.state_reg_w * self.difference(self.x, x_ref))
+ )
+ self.cost += (
+ 1
+ / 2
+ * casadi.sumsqr(
+ self.pd.terminal_velocity_w * self.difference(self.x, x_ref)
+ )
+ )
else:
- self.cost += 1/2 * \
- casadi.sumsqr(self.pd.state_reg_w *
- self.difference(self.x, x_ref)) * self.pd.dt
+ self.cost += (
+ 1
+ / 2
+ * casadi.sumsqr(self.pd.state_reg_w * self.difference(self.x, x_ref))
+ * self.pd.dt
+ )
def target_cost(self, task):
- self.cost += casadi.sumsqr(self.pd.base_velocity_tracking_w*(
- self.baseVelocityLin(self.x) - task[0])) * self.pd.dt
- self.cost += casadi.sumsqr(self.pd.base_velocity_tracking_w*(
- self.baseVelocityAng(self.x) - task[1])) * self.pd.dt
+ self.cost += (
+ casadi.sumsqr(
+ self.pd.base_velocity_tracking_w
+ * (self.baseVelocityLin(self.x) - task[0])
+ )
+ * self.pd.dt
+ )
+ self.cost += (
+ casadi.sumsqr(
+ self.pd.base_velocity_tracking_w
+ * (self.baseVelocityAng(self.x) - task[1])
+ )
+ * self.pd.dt
+ )
def compute_cost(self, x_ref, u_ref, target):
self.cost = 0
diff --git a/python/quadruped_reactive_walking/WB_MPC/Target.py b/python/quadruped_reactive_walking/WB_MPC/Target.py
index 713ded9218b40fb676556776df1e343289bb1298..cc05e1c97df1a0c9297bd260d389b701d20f22fd 100644
--- a/python/quadruped_reactive_walking/WB_MPC/Target.py
+++ b/python/quadruped_reactive_walking/WB_MPC/Target.py
@@ -12,4 +12,4 @@ class Target:
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
+ self.velocity_task = [self.linear_velocity, self.angular_velocity]