From ca5890e77cb8d5f91330354b9b61d64db4d65149 Mon Sep 17 00:00:00 2001
From: Ale <alessandroassirell98@gmail.com>
Date: Tue, 12 Jul 2022 17:10:04 +0200
Subject: [PATCH] working on walking
it's floating in the air
---
.../WB_MPC/CasadiOCP.py | 28 +++++++++----
.../WB_MPC/ProblemData.py | 18 ++++-----
.../WB_MPC/ShootingNode.py | 30 ++++++++++----
.../WB_MPC/Target.py | 39 ++++++-------------
4 files changed, 62 insertions(+), 53 deletions(-)
diff --git a/python/quadruped_reactive_walking/WB_MPC/CasadiOCP.py b/python/quadruped_reactive_walking/WB_MPC/CasadiOCP.py
index 9107783c..b1b15392 100644
--- a/python/quadruped_reactive_walking/WB_MPC/CasadiOCP.py
+++ b/python/quadruped_reactive_walking/WB_MPC/CasadiOCP.py
@@ -58,10 +58,11 @@ class CasadiOCP:
self.datas += [NodeData()]
self.make_opti_variables(x0)
- self.cost, eq_constraints = self.make_ocp()
+ 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-8,
@@ -104,6 +105,7 @@ class CasadiOCP:
def make_ocp(self):
totalcost = 0
eq = []
+ ineq = []
eq.append(self.dxs[0])
for t in range(self.pd.T):
@@ -111,25 +113,35 @@ class CasadiOCP:
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)) """
+ if (self.target.contactSequence[t] != self.target.contactSequence[t-1] and t >= 1):
+ print('Contact on ', str(self.runningModels[t].contactIds))
+ 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.evaluate_in_t(t))
+ 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))
- totalcost += rcost
- eq_constraints = casadi.vertcat(*eq)
+ 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)
- totalcost += self.terminalModel.calc(x_ref=self.pd.xref)[1]
+ _, 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
+ return totalcost, eq_constraints, ineq_constraints
def warmstart(self, x0, guess=None):
for g in guess:
diff --git a/python/quadruped_reactive_walking/WB_MPC/ProblemData.py b/python/quadruped_reactive_walking/WB_MPC/ProblemData.py
index d31df5c2..91826bcd 100644
--- a/python/quadruped_reactive_walking/WB_MPC/ProblemData.py
+++ b/python/quadruped_reactive_walking/WB_MPC/ProblemData.py
@@ -9,9 +9,9 @@ class problemDataAbstract:
self.dt_bldc = 0.0005
self.r1 = int(self.dt / self.dt_sim)
self.r2 = int(self.dt_sim / self.dt_bldc)
- self.init_steps = 10 # full stand phase
- self.target_steps = 30 # manipulation steps
- self.T = self.init_steps + self.target_steps -1
+ self.t_step = 12 # timestep for each contact phase
+ self.repetitions = 3
+ self.T = (2*self.t_step) * self.repetitions -1
self.robot = erd.load("solo12")
self.q0 = self.robot.q0
@@ -50,6 +50,7 @@ class problemDataAbstract:
self.rfFootId = self.model.getFrameId(self.rfFoot)
self.lhFootId = self.model.getFrameId(self.lhFoot)
self.rhFootId = self.model.getFrameId(self.rhFoot)
+ self.baseId = self.model.getFrameId("base_link")
self.Rsurf = np.eye(3)
@@ -77,15 +78,14 @@ class ProblemData(problemDataAbstract):
self.control_reg_w = 1e1
self.state_reg_w = np.array([0] * 3 \
+ [1e1] * 3 \
- + [1e0] * 3 \
- + [1e-3] * 3\
- + [1e0] * 6
+ + [1e0] * 12 \
+ [0] * 6 \
- + [1e1] * 3 \
- + [1e-1] * 3\
- + [1e1] * 6 )
+ + [1e1] * 12 )
self.terminal_velocity_w = np.array([0] * 18 + [1e3] * 18 )
self.control_bound_w = 1e3
+ self.k = np.array([2,2])
+ self.linear_velocity_weight = np.array([10, 10, 10])
+ self.angular_velocity_weight = np.array([10, 10, 10])
self.x0 = np.array([ 0.0, 0.0, 0.2607495, 0, 0, 0, 1,
0, 0.7, -1.4,
diff --git a/python/quadruped_reactive_walking/WB_MPC/ShootingNode.py b/python/quadruped_reactive_walking/WB_MPC/ShootingNode.py
index 3cf0d8da..61c72f54 100644
--- a/python/quadruped_reactive_walking/WB_MPC/ShootingNode.py
+++ b/python/quadruped_reactive_walking/WB_MPC/ShootingNode.py
@@ -164,6 +164,21 @@ class ShootingNode():
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] - conf.mu*fw[2] )
+ ineq.append( -fw[0] - conf.mu*fw[2] )
+ ineq.append( fw[1] - conf.mu*fw[2] )
+ ineq.append( -fw[1] - conf.mu*fw[2] ) """
+
+ return (casadi.vertcat(*ineq))
+
def constraint_standing_feet_cost(self):
# Friction cone
for i, stFoot in enumerate(self.contactIds):
@@ -188,11 +203,11 @@ class ShootingNode():
eq.append(self.acc(self.x, self.tau, *self.fs) - self.a)
return(casadi.vertcat(*eq))
- def constraint_swing_feet_ineq(self, x_ref):
+ 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.feet[sw_foot](self.x)[2] + self.feet[sw_foot](x_ref)[2])
+ ineq.append(self.vfeet[sw_foot](self.x)[0:2] - k[0]* self.feet[sw_foot](self.x)[2])
return(casadi.vertcat(*ineq))
def force_reg_cost(self):
@@ -222,16 +237,15 @@ class ShootingNode():
def target_cost(self, target):
# I am Assuming just FR FOOt to be free
- for sw_foot in self.freeIds:
-
- self.cost += 1/2 * self.pd.foot_tracking_w * casadi.sumsqr(self.feet[sw_foot](self.x) - target[sw_foot]) * self.pd.dt
+ for obj in target:
+ self.cost += casadi.sumsqr(self.pd.linear_velocity_weight*(self.baseVelocityLin(self.x) - target[obj]["linear_velocity"])) * self.pd.dt
+ self.cost += casadi.sumsqr(self.pd.angular_velocity_weight*(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.constraint_standing_feet_cost()
self.control_cost(u_ref)
- self.constraint_control_cost()
+ #self.constraint_control_cost()
self.target_cost(target)
self.body_reg_cost(x_ref=x_ref)
diff --git a/python/quadruped_reactive_walking/WB_MPC/Target.py b/python/quadruped_reactive_walking/WB_MPC/Target.py
index cc7ce001..ca1d639c 100644
--- a/python/quadruped_reactive_walking/WB_MPC/Target.py
+++ b/python/quadruped_reactive_walking/WB_MPC/Target.py
@@ -7,45 +7,28 @@ class Target:
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
- elif pd.useFixedBase == 1:
- 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.gait = [] \
+ + [ [ 0,1,1,0 ] ] * pd.t_step \
+ + [ [ 1,0,0,1 ] ] * pd.t_step
+
+ self.gait = self.gait * pd.repetitions
self.T = pd.T
self.contactSequence = [ self.patternToId(p) for p in self.gait]
- self.target = {pd.rfFootId: []}
- q = pd.x0[: pd.nq]
- v = pd.x0[pd.nq :]
- pin.forwardKinematics(pd.model, pd.rdata, q, v)
- 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.05, 0., 0.06])
- self.freq = np.array([0, 2.5, 2.5])
- self.phase = np.array([0, 0, np.pi/2])
+ self.target = {pd.baseId: {"linear_velocity" :[1,0,0], "angular_velocity": [0,0,0]}}
+
def patternToId(self, gait):
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]]
+ g0 = self.gait.pop(0)
+ self.gait += [g0]
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)]
- self.target[self.pd.rfFootId] = np.array(target)
+ pass
def evaluate_in_t(self, t):
- return {e: self.target[e][t] for e in self.target}
\ No newline at end of file
+ pass
\ No newline at end of file
--
GitLab