diff --git a/python/quadruped_reactive_walking/WB_MPC/CasadiOCP.py b/python/quadruped_reactive_walking/WB_MPC/CasadiOCP.py
index e69de29bb2d1d6434b8b29ae775ad8c2e48c5391..0d5470aadfa2a6b731a941c1101fa32aa2f84ddd 100644
--- a/python/quadruped_reactive_walking/WB_MPC/CasadiOCP.py
+++ b/python/quadruped_reactive_walking/WB_MPC/CasadiOCP.py
@@ -0,0 +1,247 @@
+import os
+from time import time
+
+import casadi
+import numpy as np
+import pinocchio as pin
+from .ShootingNode import ShootingNode
+from .ProblemData import ProblemData
+from .Target import Target
+from .OcpResult import OcpResult
+
+
+class NodeData:
+ def __init__(self, isTerminal=False):
+ self.x = None
+ self.cost = None
+ self.isTerminal = isTerminal
+ if not isTerminal:
+ self.a = None
+ self.u = None
+ self.f = None
+ self.xnext = None
+
+
+
+class CasadiOCP:
+ def __init__(self, pd: ProblemData, target:Target):
+ """Define an optimal ocntrol problem.
+ :param robot: Pinocchio RobotWrapper instance
+ :param gait: list of list containing contact pattern i.e. for two steps[ [1,1,1,1], [1,0,0,1] ]. \
+ Its length determines the horizon of the ocp
+ :param x0: starting configuration
+ :param x_ref: reference configuration
+ :param target: array of target positions
+ :param dt: timestep integration
+ """
+ self.pd = pd
+ self.target = target
+ self.results = OcpResult()
+
+ # build the different shooting nodes
+ self.shooting_nodes = {contacts: ShootingNode(pd=pd, contactIds=contacts)
+ for contacts in set(target.contactSequence)}
+
+ def solve(self, x0, 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.target.contactSequence[t]]
+ for t in range(self.pd.T)]
+ self.terminalModel = self.shooting_nodes[self.target.contactSequence[self.pd.T]]
+ self.datas = []
+ for i in range(self.pd.T + 1):
+ if i == self.pd.T:
+ self.datas += [NodeData(True)]
+ else:
+ self.datas += [NodeData()]
+
+ self.make_opti_variables(x0)
+ self.cost, eq_constraints, ineq_constraints = self.make_ocp()
+
+ self.opti.minimize(self.cost)
+ self.opti.subject_to(eq_constraints == 0)
+ self.opti.subject_to(ineq_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"
+ }
+
+ self.opti.solver("ipopt", p_opts, s_opts)
+
+ self.warmstart(x0, guess)
+
+ # SOLVE
+ self.opti.solve_limited()
+
+ for data in self.datas:
+ data.x = self.opti.value(data.x)
+ data.cost = self.opti.value(data.cost)
+ if not data.isTerminal:
+ data.u = self.opti.value(data.u)
+ data.xnext = self.opti.value(data.xnext)
+
+ def make_opti_variables(self, x0):
+ opti = casadi.Opti()
+ self.opti = opti
+ # Optimization variables
+ 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.fs = []
+ for m in self.runningModels:
+ f_tmp = [opti.variable(3) for _ in range(len(m.contactIds))]
+ self.fs.append(f_tmp)
+ self.fs = self.fs
+
+ def make_ocp(self):
+ totalcost = 0
+ eq = []
+ ineq = []
+
+ eq.append(self.dxs[0])
+ 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)
+
+ # If it is landing
+ if (self.target.contactSequence[t] != self.target.contactSequence[t-1] and t >= 1):
+ print('Contact on ', str(self.runningModels[t].contactIds))
+ eq.append(self.runningModels[t].constraint_landing_feet_eq(self.pd.xref))
+
+ xnext, rcost = self.runningModels[t].calc(x_ref=self.pd.xref,
+ u_ref=self.pd.uref,
+ target=self.target.target)
+
+ # 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))
+
+ ineq.append(self.runningModels[t].constraint_swing_feet_ineq(self.pd.xref, k = self.pd.k))
+ ineq.append(self.runningModels[t].constraint_standing_feet_ineq())
+
+ totalcost += rcost
+
+ 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)
+ _, rcost = self.terminalModel.calc(x_ref=self.pd.xref)
+ #totalcost += self.terminalModel.calc(x_ref=self.pd.xref)[1]
+ totalcost += rcost
+
+ eq_constraints = casadi.vertcat(*eq)
+ ineq_constraints = casadi.vertcat(*ineq)
+
+
+ return totalcost, eq_constraints, ineq_constraints
+
+ def warmstart(self, x0, guess=None):
+ for g in guess:
+ if guess[g] == []:
+ print("No warmstart provided")
+ 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']
+
+ 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:]])
+ 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):
+ self.opti.set_initial(a, ag)
+ for u, ug in zip(self.us, us_g):
+ self.opti.set_initial(u, ug)
+ for f, fg in zip(self.fs, fs_g):
+ fgsplit = np.split(fg, len(f))
+ fgc = []
+ [fgc.append(f) for f in fgsplit]
+ [self.opti.set_initial(f[i], fgc[i]) for i in range(len(f))]
+ print("Got warm start")
+ except:
+ print("Can't load warm start")
+
+ def get_results(self):
+ xs_sol = [self.opti.value(x) for x in self.xs]
+ us_sol = [self.opti.value(u) for u in self.us]
+ acs_sol = [self.opti.value(a) for a in self.acs]
+ fsol = {name: [] for name in self.pd.allContactIds}
+ fs_world = {name: [] for name in self.pd.allContactIds}
+ fsol_to_ws = []
+ for t in range(self.pd.T):
+ for i, st_foot in enumerate(self.runningModels[t].contactIds):
+ fsol[st_foot].append(self.opti.value(self.fs[t][i]))
+ 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))]))
+
+ 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]
+
+ for foot in fs_world:
+ fs_world[foot] = np.array(fs_world[foot])
+
+ self.results.x = xs_sol
+ self.results.a = acs_sol
+ self.results.u = us_sol
+ self.results.fs = fsol_to_ws
+ return self.results
+
+
+ def get_feet_position(self, xs_sol):
+ """ Get the feet positions computed durnig one ocp
+
+ :param xs_sol: Array of dimension [n_steps, n_states]
+
+ """
+ feet_log = {i: [] for i in self.pd.allContactIds}
+ for foot in feet_log:
+ tmp = []
+ for i in range(len(xs_sol)):
+ tmp += [self.terminalModel.feet[foot](xs_sol[i]).full()[:, 0]]
+ feet_log[foot] = np.array(tmp)
+
+ return feet_log
+
+ def get_feet_velocity(self, xs_sol):
+ """ Get the feet velocities in LOCAL_WORLD_ALIGNED frame computed durnig one ocp
+
+ :param xs_sol: Array of dimension [n_steps, n_states]
+
+ """
+ feet_log = {i: [] for i in self.pd.allContactIds}
+ for foot in feet_log:
+ tmp = []
+ for i in range(len(xs_sol)):
+ tmp += [self.terminalModel.vfeet[foot](xs_sol[i]).full()[:, 0]]
+ feet_log[foot] = np.array(tmp)
+
+ return feet_log
+
+ def get_base_log(self, xs_sol):
+ """ 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 = np.array(base_pos_log)
+ return base_pos_log
+
diff --git a/python/quadruped_reactive_walking/WB_MPC/CrocoddylOCP.py b/python/quadruped_reactive_walking/WB_MPC/CrocoddylOCP.py
index 69494179fcd35ad94e42903c52e318100f473827..3f8b9ce9bc3b8e8a559c7b7e37e8b7cd4466dd8b 100644
--- a/python/quadruped_reactive_walking/WB_MPC/CrocoddylOCP.py
+++ b/python/quadruped_reactive_walking/WB_MPC/CrocoddylOCP.py
@@ -159,6 +159,10 @@ class OCP:
self.make_running_model(model, support_feet, tasks)
return model
+ if isTerminal:
+ self.make_terminal_model()
+ else:
+ self.make_running_model()
def create_standard_model(self, support_feet, switching):
"""
@@ -294,6 +298,10 @@ class OCP:
self.pd.ground_collision_weight,
)
+ def remove_contacts(self):
+ allContacts = self.dmodel.contacts.contacts.todict()
+ for c in allContacts:
+ self.dmodel.contacts.removeContact(c)
name = "base_velocity_tracking"
ref = pin.Motion(tasks[0], tasks[1])
residual_base_velocity = crocoddyl.ResidualModelFrameVelocity(
diff --git a/python/quadruped_reactive_walking/WB_MPC/ShootingNode.py b/python/quadruped_reactive_walking/WB_MPC/ShootingNode.py
index e69de29bb2d1d6434b8b29ae775ad8c2e48c5391..be1b27200607c8a4040909f96d820f10d82a5d27 100644
--- a/python/quadruped_reactive_walking/WB_MPC/ShootingNode.py
+++ b/python/quadruped_reactive_walking/WB_MPC/ShootingNode.py
@@ -0,0 +1,255 @@
+import os
+from time import time
+
+import casadi
+import numpy as np
+import pinocchio as pin
+from pinocchio import casadi as cpin
+from .ProblemData import ProblemData
+
+class ShootingNode():
+ def __init__(self, pd: ProblemData, contactIds):
+
+ self.pd = pd
+ self.contactIds = contactIds
+ self.freeIds = []
+
+ cmodel = cpin.Model(pd.model)
+ cdata = cmodel.createData()
+
+ 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]
+
+ cx = casadi.SX.sym("x", pd.nx, 1)
+ cq = casadi.SX.sym("q", pd.nq, 1)
+ cv = casadi.SX.sym("v", pd.nv, 1)
+ cx2 = casadi.SX.sym("x2", pd.nx, 1)
+ cu = casadi.SX.sym("u", pd.nu, 1)
+ 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)
+
+ # 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))
+ 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)
+ 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)
+
+ # 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])
+
+ # 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:])])
+
+ # integrate_q(q,dq) = pin.integrate(q,dq)
+ self.integrate_q = casadi.Function('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:])])
+
+ vel_reference_frame = pin.LOCAL #Reference frame in which velocities of base will be expressed (for locomotion)
+
+ 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])])
+
+ # Base link position and orientation
+ self.baseTranslation = casadi.Function('base_translation', [cx], [cdata.oMf[self.baseID].translation])
+ self.baseRotation = casadi.Function('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])
+
+ # 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}
+
+ # 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}
+
+ # 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_WORLD_ALIGNED).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_WORLD_ALIGNED).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)])
+
+ def init(self, data, x, a=None, u=None, fs=None, isTerminal=False):
+ self.data = data
+ self.data.x = x
+ self.data.a = a
+ self.data.u = u
+ self.data.f = fs
+
+ self.x = x
+ self.isTerminal = isTerminal
+ if not isTerminal:
+ self.a = a
+ self.u = u
+ self.tau = casadi.vertcat(np.zeros(6), u)
+ 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)
+ 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)
+ 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
+
+ xnext = casadi.vertcat(qnext, vnext)
+ self.data.xnext = xnext
+
+ # Cost functions:
+ self.compute_cost(x_ref, u_ref, target)
+ self.data.cost = self.cost
+
+ return xnext, self.cost
+
+ def constraint_landing_feet_eq(self, x_ref):
+ eq = []
+ for stFoot in self.contactIds:
+ eq.append(self.feet[stFoot](self.x)[2] - self.feet[stFoot](x_ref)[2])
+ eq.append(self.vfeet[stFoot](self.x))
+ return(casadi.vertcat(*eq))
+
+ 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))
+
+ def constraint_standing_feet_ineq(self):
+ # Friction cone
+ ineq = []
+ for i, stFoot in enumerate(self.contactIds):
+ R = self.Rfeet[stFoot](self.x)
+ f_ = self.fs[i]
+ fw = R @ f_
+ ineq.append(-fw[2])
+ """ ineq.append( fw[0] - self.pd.mu*fw[2] )
+ ineq.append( -fw[0] - self.pd.mu*fw[2] )
+ ineq.append( fw[1] - self.pd.mu*fw[2] )
+ ineq.append( -fw[1] - self.pd.mu*fw[2] ) """
+
+ return (casadi.vertcat(*ineq))
+
+ def constraint_standing_feet_cost(self):
+ # Friction cone
+ for i, stFoot in enumerate(self.contactIds):
+ R = self.Rfeet[stFoot](self.x)
+ 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])
+ 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
+
+ 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
+
+ def constraint_dynamics_eq(self):
+ eq = []
+ eq.append(self.acc(self.x, self.tau, *self.fs) - self.a)
+ return(casadi.vertcat(*eq))
+
+ def constraint_swing_feet_ineq(self, x_ref, k):
+ ineq = []
+ for sw_foot in self.freeIds:
+ ineq.append(-self.feet[sw_foot](self.x)[2] + self.feet[sw_foot](x_ref)[2])
+ 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])
+
+ return(casadi.vertcat(*ineq))
+
+ 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
+
+ def control_cost(self, u_ref):
+ 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))
+ else:
+ self.cost += 1/2 * \
+ casadi.sumsqr(self.pd.state_reg_w *
+ self.difference(self.x, x_ref)) * self.pd.dt
+
+ def target_cost(self, target):
+ # I am Assuming just FR FOOt to be free
+ for obj in target:
+ self.cost += casadi.sumsqr(self.pd.linear_velocity_w*(self.baseVelocityLin(self.x) - target[obj]["linear_velocity"])) * self.pd.dt
+ self.cost += casadi.sumsqr(self.pd.angular_velocity_w*(self.baseVelocityAng(self.x) - target[obj]["angular_velocity"])) * self.pd.dt
+
+ def compute_cost(self, x_ref, u_ref, target):
+ self.cost = 0
+ if not self.isTerminal:
+ self.control_cost(u_ref)
+ #self.constraint_control_cost()
+ self.target_cost(target)
+
+ self.body_reg_cost(x_ref=x_ref)
+
+ return self.cost
\ No newline at end of file
diff --git a/python/quadruped_reactive_walking/WB_MPC_Wrapper.py b/python/quadruped_reactive_walking/WB_MPC_Wrapper.py
index cbcb8aa74b7bdc08145265b462d4dfd0589912ad..87c8cc58ebc8784102982f7c93fcf5e5cd0c5f13 100644
--- a/python/quadruped_reactive_walking/WB_MPC_Wrapper.py
+++ b/python/quadruped_reactive_walking/WB_MPC_Wrapper.py
@@ -6,7 +6,7 @@ from time import time, sleep
import numpy as np
-from .WB_MPC.CrocoddylOCP import OCP
+from .WB_MPC.CasadiOCP import CasadiOCP
import quadruped_reactive_walking as qrw
diff --git a/python/quadruped_reactive_walking/tools/PyBulletSimulator.py b/python/quadruped_reactive_walking/tools/PyBulletSimulator.py
index 22e140b9860d75287fb39f91c3f98c6dc6f79436..3bffd88dfb8dafab1c0f9cbcf8849a332c554b26 100644
--- a/python/quadruped_reactive_walking/tools/PyBulletSimulator.py
+++ b/python/quadruped_reactive_walking/tools/PyBulletSimulator.py
@@ -323,6 +323,42 @@ class pybullet_simulator:
pin.Quaternion(pin.rpy.rpyToMatrix(p_roll, p_pitch, 0.0)).coeffs(),
)
+ # Progressively raise the base to achieve proper contact (take into account radius of the foot)
+ while (
+ pyb.getClosestPoints(
+ self.robotId,
+ self.planeId,
+ distance=0.005,
+ linkIndexA=feetLinksID[i_min],
+ )
+ )[0][8] < -0.001:
+ z_min -= 0.001
+ pyb.resetBasePositionAndOrientation(
+ self.robotId,
+ [0.0, 0.0, -z_min],
+ pin.Quaternion(pin.rpy.rpyToMatrix(p_roll, p_pitch, 0.0)).coeffs(),
+ )
+ # Get position of feet in world frame with base at (0, 0, 0)
+ feetLinksID = [3, 7, 11, 15]
+ linkStates = pyb.getLinkStates(self.robotId, feetLinksID)
+
+ # Get minimum height of feet (they are in the ground since base is at 0, 0, 0)
+ z_min = linkStates[0][4][2]
+ i_min = 0
+ i = 1
+ for link in linkStates[1:]:
+ if link[4][2] < z_min:
+ z_min = link[4][2]
+ i_min = i
+ i += 1
+
+ # Set base at (0, 0, -z_min) so that the lowest foot is at z = 0
+ pyb.resetBasePositionAndOrientation(
+ self.robotId,
+ [0.0, 0.0, -z_min],
+ pin.Quaternion(pin.rpy.rpyToMatrix(p_roll, p_pitch, 0.0)).coeffs(),
+ )
+
# Progressively raise the base to achieve proper contact (take into account radius of the foot)
while (
pyb.getClosestPoints(