diff --git a/python/quadruped_reactive_walking/WB_MPC/CasadiOCP.py b/python/quadruped_reactive_walking/WB_MPC/CasadiOCP.py
index b1b1539202628605f997c5c7c25de946254f327e..c0eddbeea558e81392fcb3bd51819802da3d9692 100644
--- a/python/quadruped_reactive_walking/WB_MPC/CasadiOCP.py
+++ b/python/quadruped_reactive_walking/WB_MPC/CasadiOCP.py
@@ -65,8 +65,8 @@ class CasadiOCP:
self.opti.subject_to(ineq_constraints <= 0)
p_opts = {"expand": False}
- s_opts = {"tol": 1e-8,
- "acceptable_tol": 1e-8,
+ s_opts = {"tol": 1e-4,
+ "acceptable_tol": 1e-4,
# "max_iter": 21,
# "compl_inf_tol": 1e-2,
# "constr_viol_tol": 1e-2
@@ -115,7 +115,7 @@ class CasadiOCP:
# 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))
- self.runningModels[t].constraint_landing_feet_eq(self.pd.xref)
+ 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,
@@ -126,7 +126,7 @@ class CasadiOCP:
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_swing_feet_ineq(self.pd.xref, k = self.pd.k))
ineq.append(self.runningModels[t].constraint_standing_feet_ineq())
totalcost += rcost
diff --git a/python/quadruped_reactive_walking/WB_MPC/ProblemData.py b/python/quadruped_reactive_walking/WB_MPC/ProblemData.py
index 91826bcdef6afee2a7edcde739fee627b8834864..b60e4fc0dc949c07ec38a8f23815c3a91f045681 100644
--- a/python/quadruped_reactive_walking/WB_MPC/ProblemData.py
+++ b/python/quadruped_reactive_walking/WB_MPC/ProblemData.py
@@ -72,20 +72,19 @@ class ProblemData(problemDataAbstract):
self.useFixedBase = 0
# Cost function weights
self.mu = 0.7
- self.foot_tracking_w = 1e2
self.friction_cone_w = 1e3
- self.control_bound_w = 1e3
- self.control_reg_w = 1e1
+ #self.control_bound_w = 1e3
+ self.control_reg_w = 1e0 * 0
self.state_reg_w = np.array([0] * 3 \
- + [1e1] * 3 \
- + [1e0] * 12 \
+ + [1e0] * 3 \
+ + [1e1] * 12 \
+ [0] * 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])
+ + [1e0] * 12 )
+ #self.terminal_velocity_w = np.array([0] * 18 + [1e3] * 18 )
+ #self.control_bound_w = 1e3
+ self.k = np.array([10,10])
+ self.linear_velocity_w = np.array([10, 10, 10])
+ self.angular_velocity_w = 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 61c72f545c918ff480d48aa247c082deef206f39..be1b27200607c8a4040909f96d820f10d82a5d27 100644
--- a/python/quadruped_reactive_walking/WB_MPC/ShootingNode.py
+++ b/python/quadruped_reactive_walking/WB_MPC/ShootingNode.py
@@ -172,10 +172,10 @@ class ShootingNode():
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] ) """
+ """ 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))
@@ -207,7 +207,9 @@ class ShootingNode():
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[0]* self.feet[sw_foot](self.x)[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):
@@ -227,9 +229,9 @@ class ShootingNode():
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.terminal_velocity_w *
+ # self.difference(self.x, x_ref))
else:
self.cost += 1/2 * \
casadi.sumsqr(self.pd.state_reg_w *
@@ -238,8 +240,8 @@ class ShootingNode():
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_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
+ 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
diff --git a/python/quadruped_reactive_walking/WB_MPC/Target.py b/python/quadruped_reactive_walking/WB_MPC/Target.py
index ca1d639c96f920b37f5e450772bece3f7e49570b..175704b2068df42f9ffb575e9f6180d8c5bb8f93 100644
--- a/python/quadruped_reactive_walking/WB_MPC/Target.py
+++ b/python/quadruped_reactive_walking/WB_MPC/Target.py
@@ -8,15 +8,15 @@ class Target:
self.dt = pd.dt
self.gait = [] \
- + [ [ 0,1,1,0 ] ] * pd.t_step \
- + [ [ 1,0,0,1 ] ] * pd.t_step
+ + [ [ 1,0,0,1 ] ] * pd.t_step \
+ + [ [ 0,1,1,0 ] ] * 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.baseId: {"linear_velocity" :[1,0,0], "angular_velocity": [0,0,0]}}
+ self.target = {pd.baseId: {"linear_velocity" :[2,0,0], "angular_velocity": [0,0,0]}}
def patternToId(self, gait):
diff --git a/python/quadruped_reactive_walking/main_solo12_control.py b/python/quadruped_reactive_walking/main_solo12_control.py
index a8c0b89d30e24fb81548b78f1f52d769ce2d2030..e8d82d01acf9a4301cfd810c19c3210cf74ea3b0 100644
--- a/python/quadruped_reactive_walking/main_solo12_control.py
+++ b/python/quadruped_reactive_walking/main_solo12_control.py
@@ -7,6 +7,7 @@ import quadruped_reactive_walking as qrw
from .Controller import Controller
from .tools.LoggerControl import LoggerControl
from .WB_MPC.ProblemData import ProblemData, ProblemDataFull
+from pinocchio.visualize import GepettoVisualizer
from .WB_MPC.Target import Target
params = qrw.Params() # Object that holds all controller parameters
@@ -21,6 +22,15 @@ else:
from .tools.qualisysClient import QualisysClient
+try:
+ viz = GepettoVisualizer(pd.model,pd.collision_model, pd.visual_model)
+ viz.initViewer()
+ viz.loadViewerModel()
+ gv = viz.viewer.gui
+except:
+ print("No viewer" )
+
+
def get_input():
"""