diff --git a/scripts/Controller.py b/scripts/Controller.py
index ff75422c8772111c875de3f4e400b65d229d7b96..c4029b4970a10dcab4052c8ece956fe49ac75c01 100644
--- a/scripts/Controller.py
+++ b/scripts/Controller.py
@@ -133,6 +133,7 @@ class Controller:
# Wrapper that makes the link with the solver that you want to use for the MPC
self.mpc_wrapper = MPC_Wrapper.MPC_Wrapper(params, self.q)
+ self.o_targetFootstep = np.zeros((3,4)) # Store result for MPC_planner
# ForceMonitor to display contact forces in PyBullet with red lines
# import ForceMonitor
@@ -270,8 +271,8 @@ class Controller:
try:
if self.type_MPC == 3 :
# Compute the target foostep in local frame, to stop the optimisation around it when t_lock overpass
- l_targetFootstep = self.footstepPlanner.getRz().transpose() @ self.footTrajectoryGenerator.getFootPosition() - self.q[0:3,0:1]
- self.mpc_wrapper.solve(self.k, xref, fsteps, cgait, l_targetFootstep)
+ l_targetFootstep = oRh.transpose() @ (self.o_targetFootstep - oTh)
+ self.mpc_wrapper.solve(self.k, xref, fsteps, cgait, l_targetFootstep, oRh, oTh)
else :
self.mpc_wrapper.solve(self.k, xref, fsteps, cgait, np.zeros((3,4)))
@@ -281,15 +282,19 @@ class Controller:
# Retrieve reference contact forces in horizontal frame
self.x_f_mpc = self.mpc_wrapper.get_latest_result()
+ # Store o_targetFootstep, used with MPC_planner
+ self.o_targetFootstep = o_targetFootstep.copy()
+
t_mpc = time.time()
# If the MPC optimizes footsteps positions then we use them
if self.k > 100 and self.type_MPC == 3 :
- for foot in range(4):
- id = 0
- while cgait[id,foot] == 0 :
- id += 1
- o_targetFootstep[:2,foot] = np.array(self.footstepPlanner.getRz()[:2, :2]) @ self.x_f_mpc[24 + 2*foot:24+2*foot+2, id] + np.array([self.q[0, 0] , self.q[1,0] ])
+ for foot in range(4):
+ if cgait[0,foot] == 0 :
+ id = 0
+ while cgait[id,foot] == 0 :
+ id += 1
+ self.o_targetFootstep[:2,foot] = self.x_f_mpc[24 + 2*foot:24+2*foot+2, id]
# Update pos, vel and acc references for feet
self.footTrajectoryGenerator.update(self.k, o_targetFootstep)
diff --git a/scripts/MPC_Wrapper.py b/scripts/MPC_Wrapper.py
index beb7db3920a116c4cf697a55c9ada2467d5e6c05..3061c7c79cd91f8961b4351d1a80ce6710a713c7 100644
--- a/scripts/MPC_Wrapper.py
+++ b/scripts/MPC_Wrapper.py
@@ -7,6 +7,14 @@ import crocoddyl_class.MPC_crocoddyl as MPC_crocoddyl
import crocoddyl_class.MPC_crocoddyl_planner as MPC_crocoddyl_planner
import pinocchio as pin
from time import time
+from enum import Enum
+
+class MPC_type(Enum):
+ OSQP = 0
+ CROCODDYL_LINEAR = 1
+ CROCODDYL_NON_LINEAR = 2
+ CROCODDYL_PLANNER = 3
+ CROCODDYL_PLANNER_TIME = 4
class Dummy:
"""Dummy class to store variables"""
@@ -54,13 +62,13 @@ class MPC_Wrapper:
self.gait_next = np.zeros(4)
self.mass = params.mass
- self.mpc_type = params.type_MPC
+ self.mpc_type = MPC_type(params.type_MPC)
self.multiprocessing = params.enable_multiprocessing
if self.multiprocessing: # Setup variables in the shared memory
self.newData = Value('b', False)
self.newResult = Value('b', False)
- if self.mpc_type == 3: # Need more space to store optimized footsteps and l_fsteps to stop the optimization around it
- self.dataIn = Array('d', [0.0] * (1 + (np.int(self.n_steps)+1) * 12 + 12*self.N_gait + 12) )
+ if self.mpc_type == MPC_type.CROCODDYL_PLANNER: # Need more space to store optimized footsteps and l_fsteps to stop the optimization around it (+ oTh and oRh)
+ self.dataIn = Array('d', [0.0] * (1 + (np.int(self.n_steps)+1) * 12 + 12*self.N_gait + 12 + 9 + 3) )
self.dataOut = Array('d', [0] * 32 * (np.int(self.n_steps)))
else:
self.dataIn = Array('d', [0.0] * (1 + (np.int(self.n_steps)+1) * 12 + 12*self.N_gait))
@@ -69,25 +77,29 @@ class MPC_Wrapper:
self.running = Value('b', True)
else:
# Create the new version of the MPC solver object
- if self.mpc_type == 0: # OSQP MPC
+ if self.mpc_type == MPC_type.OSQP: # OSQP MPC
self.mpc = MPC.MPC(params) # self.dt, self.n_steps, self.T_gait, self.N_gait)
- elif self.mpc_type == 1: # Crocoddyl MPC Linear
+ elif self.mpc_type == MPC_type.CROCODDYL_LINEAR: # Crocoddyl MPC Linear
self.mpc = MPC_crocoddyl.MPC_crocoddyl(params, mu=0.9, inner=False, linearModel=True)
- elif self.mpc_type == 2: # Crocoddyl MPC Non-Linear
+ elif self.mpc_type == MPC_type.CROCODDYL_NON_LINEAR: # Crocoddyl MPC Non-Linear
self.mpc = MPC_crocoddyl.MPC_crocoddyl(params, mu=0.9, inner=False, linearModel=False)
- else: # Crocoddyl MPC Non-Linear with footsteps optimization
+ elif self.mpc_type == MPC_type.CROCODDYL_PLANNER: # Crocoddyl MPC Non-Linear with footsteps optimization
self.mpc = MPC_crocoddyl_planner.MPC_crocoddyl_planner(params, mu=0.9, inner=False)
+ else:
+ print("Unknown MPC type, using crocoddyl linear")
+ self.type = MPC_type.CROCODDYL_LINEAR
+ self.mpc = MPC_crocoddyl.MPC_crocoddyl(params, mu=0.9, inner=False, linearModel=True)
# Setup initial result for the first iteration of the main control loop
x_init = np.zeros(12)
x_init[0:6] = q_init[0:6, 0].copy()
- if self.mpc_type == 3: # Need more space to store optimized footsteps
+ if self.mpc_type == MPC_type.CROCODDYL_PLANNER: # Need more space to store optimized footsteps
self.last_available_result = np.zeros((32, (np.int(self.n_steps))))
else:
self.last_available_result = np.zeros((24, (np.int(self.n_steps))))
self.last_available_result[:24, 0] = np.hstack((x_init, np.array([0.0, 0.0, 8.0] * 4)))
- def solve(self, k, xref, fsteps, gait, l_targetFootstep):
+ def solve(self, k, xref, fsteps, gait, l_targetFootstep, oRh = np.eye(3), oTh = np.zeros((3,1)) ):
"""Call either the asynchronous MPC or the synchronous MPC depending on the value of multiprocessing during
the creation of the wrapper
@@ -102,9 +114,9 @@ class MPC_Wrapper:
self.t_mpc_solving_start = time()
if self.multiprocessing: # Run in parallel process
- self.run_MPC_asynchronous(k, xref, fsteps, l_targetFootstep)
+ self.run_MPC_asynchronous(k, xref, fsteps, l_targetFootstep, oRh, oTh)
else: # Run in the same process than main loop
- self.run_MPC_synchronous(k, xref, fsteps, l_targetFootstep)
+ self.run_MPC_synchronous(k, xref, fsteps, l_targetFootstep, oRh, oTh)
if not np.allclose(gait[0, :], self.gait_past): # If gait status has changed
if np.allclose(gait[0, :], self.gait_next): # If we're still doing what was planned the last time MPC was solved
@@ -145,7 +157,7 @@ class MPC_Wrapper:
self.not_first_iter = True
return self.last_available_result
- def run_MPC_synchronous(self, k, xref, fsteps, l_targetFootstep):
+ def run_MPC_synchronous(self, k, xref, fsteps, l_targetFootstep, oRh, oTh):
"""Run the MPC (synchronous version) to get the desired contact forces for the feet currently in stance phase
Args:
@@ -158,10 +170,10 @@ class MPC_Wrapper:
# Run the MPC to get the reference forces and the next predicted state
# Result is stored in mpc.f_applied, mpc.q_next, mpc.v_next
- if self.mpc_type == 0:
+ if self.mpc_type == MPC_type.OSQP:
# OSQP MPC
self.mpc.run(np.int(k), xref.copy(), fsteps.copy())
- elif self.mpc_type == 3: # Add goal position to stop the optimisation
+ elif self.mpc_type == MPC_type.CROCODDYL_PLANNER: # Add goal position to stop the optimisation
# Crocoddyl MPC
self.mpc.solve(k, xref.copy(), fsteps.copy(), l_targetFootstep)
else:
@@ -169,9 +181,12 @@ class MPC_Wrapper:
self.mpc.solve(k, xref.copy(), fsteps.copy())
# Output of the MPC
- self.f_applied = self.mpc.get_latest_result()
+ if self.mpc_type == MPC_type.CROCODDYL_PLANNER:
+ self.f_applied = self.mpc.get_latest_result(oRh,oTh )
+ else:
+ self.f_applied = self.mpc.get_latest_result()
- def run_MPC_asynchronous(self, k, xref, fsteps, l_targetFootstep):
+ def run_MPC_asynchronous(self, k, xref, fsteps, l_targetFootstep, oRh, oTh):
"""Run the MPC (asynchronous version) to get the desired contact forces for the feet currently in stance phase
Args:
@@ -189,7 +204,7 @@ class MPC_Wrapper:
p.start()
# Stacking data to send them to the parallel process
- self.compress_dataIn(k, xref, fsteps, l_targetFootstep)
+ self.compress_dataIn(k, xref, fsteps, l_targetFootstep, oRh, oTh)
self.newData.value = True
return 0
@@ -215,11 +230,13 @@ class MPC_Wrapper:
# print("New data detected")
# Retrieve data thanks to the decompression function and reshape it
- if self.mpc_type != 3:
+ if self.mpc_type != MPC_type.CROCODDYL_PLANNER:
kf, xref_1dim, fsteps_1dim = self.decompress_dataIn(dataIn)
else:
- kf, xref_1dim, fsteps_1dim, l_target_1dim = self.decompress_dataIn(dataIn)
+ kf, xref_1dim, fsteps_1dim, l_target_1dim, oRh, oTh = self.decompress_dataIn(dataIn)
l_target = np.reshape(l_target_1dim, (3,4))
+ oRh = np.reshape(oRh, (3,3))
+ oTh = np.reshape(oTh, (3,1))
# Reshaping 1-dimensional data
k = int(kf[0])
@@ -229,20 +246,22 @@ class MPC_Wrapper:
# Create the MPC object of the parallel process during the first iteration
if k == 0:
# loop_mpc = MPC.MPC(self.dt, self.n_steps, self.T_gait)
- if self.mpc_type == 0:
+ if self.mpc_type == MPC_type.OSQP:
loop_mpc = MPC.MPC(self.params)
- elif self.mpc_type == 1: # Crocoddyl MPC Linear
+ elif self.mpc_type == MPC_type.CROCODDYL_LINEAR: # Crocoddyl MPC Linear
loop_mpc = MPC_crocoddyl.MPC_crocoddyl(self.params, mu=0.9, inner=False, linearModel=True)
- elif self.mpc_type == 2: # Crocoddyl MPC Non-Linear
+ elif self.mpc_type == MPC_type.CROCODDYL_NON_LINEAR: # Crocoddyl MPC Non-Linear
loop_mpc = MPC_crocoddyl.MPC_crocoddyl(self.params, mu=0.9, inner=False, linearModel=False)
- else: # Crocoddyl MPC Non-Linear with footsteps optimization
+ elif self.mpc_type == MPC_type.CROCODDYL_PLANNER: # Crocoddyl MPC Non-Linear with footsteps optimization
loop_mpc = MPC_crocoddyl_planner.MPC_crocoddyl_planner(self.params, mu=0.9, inner=False)
+ else: # Using linear model
+ loop_mpc = MPC_crocoddyl.MPC_crocoddyl(self.params, mu=0.9, inner=False, linearModel=True)
# Run the asynchronous MPC with the data that as been retrieved
- if self.mpc_type == 0:
+ if self.mpc_type == MPC_type.OSQP:
fsteps[np.isnan(fsteps)] = 0.0
loop_mpc.run(np.int(k), xref, fsteps)
- elif self.mpc_type == 3:
+ elif self.mpc_type == MPC_type.CROCODDYL_PLANNER:
loop_mpc.solve(k, xref.copy(), fsteps.copy(), l_target.copy())
else:
loop_mpc.solve(k, xref.copy(), fsteps.copy())
@@ -251,14 +270,17 @@ class MPC_Wrapper:
# print(len(self.dataOut))
# print((loop_mpc.get_latest_result()).shape)
- self.dataOut[:] = loop_mpc.get_latest_result().ravel(order='F')
+ if self.mpc_type == MPC_type.CROCODDYL_PLANNER:
+ self.dataOut[:] = loop_mpc.get_latest_result(oRh, oTh).ravel(order='F')
+ else:
+ self.dataOut[:] = loop_mpc.get_latest_result().ravel(order='F')
# Set shared variable to true to signal that a new result is available
newResult.value = True
return 0
- def compress_dataIn(self, k, xref, fsteps, l_targetFootstep):
+ def compress_dataIn(self, k, xref, fsteps, l_targetFootstep, oRh, oTh):
"""Compress data in a single C-type array that belongs to the shared memory to send data from the main control
loop to the asynchronous MPC
@@ -272,9 +294,9 @@ class MPC_Wrapper:
fsteps[np.isnan(fsteps)] = 0.0
# Compress data in the shared input array
- if self.mpc_type == 3:
+ if self.mpc_type == MPC_type.CROCODDYL_PLANNER:
self.dataIn[:] = np.concatenate([[(k/self.k_mpc)], xref.ravel(),
- fsteps.ravel(), l_targetFootstep.ravel()], axis=0)
+ fsteps.ravel(), l_targetFootstep.ravel(), oRh.ravel(), oTh.ravel()], axis=0)
else:
self.dataIn[:] = np.concatenate([[(k/self.k_mpc)], xref.ravel(),
fsteps.ravel()], axis=0)
@@ -290,8 +312,8 @@ class MPC_Wrapper:
"""
# Sizes of the different variables that are stored in the C-type array
- if self.mpc_type == 3:
- sizes = [0, 1, (np.int(self.n_steps)+1) * 12, 12*self.N_gait, 12]
+ if self.mpc_type == MPC_type.CROCODDYL_PLANNER:
+ sizes = [0, 1, (np.int(self.n_steps)+1) * 12, 12*self.N_gait, 12, 9, 3]
else:
sizes = [0, 1, (np.int(self.n_steps)+1) * 12, 12*self.N_gait]
csizes = np.cumsum(sizes)
@@ -303,7 +325,7 @@ class MPC_Wrapper:
"""Return the result of the asynchronous MPC (desired contact forces) that is stored in the shared memory
"""
- if self.mpc_type == 3: # Need more space to store optimized footsteps
+ if self.mpc_type == MPC_type.CROCODDYL_PLANNER: # Need more space to store optimized footsteps
return np.array(self.dataOut[:]).reshape((32, -1), order='F')
else:
return np.array(self.dataOut[:]).reshape((24, -1), order='F')
diff --git a/scripts/crocoddyl_class/MPC_crocoddyl.py b/scripts/crocoddyl_class/MPC_crocoddyl.py
index fc255f44fff4c72c3de3ef569f506cd7e3f5a73f..346a2f49fbd94f8722f0642c215afda485c1d80f 100644
--- a/scripts/crocoddyl_class/MPC_crocoddyl.py
+++ b/scripts/crocoddyl_class/MPC_crocoddyl.py
@@ -10,26 +10,19 @@ class MPC_crocoddyl:
retrieve the results.
Args:
- dt (float): time step of the MPC
- T_mpc (float): Duration of the prediction horizon
+ params (obj): Params object containing the simulation parameters
mu (float): Friction coefficient
- inner(bool): Inside or outside approximation of the friction cone
+ inner (float): Inside or outside approximation of the friction cone (mu * 1/sqrt(2))
linearModel(bool) : Approximation in the cross product by using desired state
"""
def __init__(self, params, mu=1, inner=True, linearModel=True):
-
- # Time step of the solver
- self.dt = params.dt_mpc
-
- # Period of the MPC
- self.T_mpc = params.T_mpc
-
- # Mass of the robot
- self.mass = params.mass
-
- # Inertia matrix of the robot in body frame
- self.gI = np.array(params.I_mat.tolist()).reshape((3, 3))
+
+ self.dt = params.dt_mpc # Time step of the solver
+ self.T_mpc = params.T_mpc # Period of the MPC
+ self.n_nodes = int(self.T_mpc/self.dt) # Number of nodes
+ self.mass = params.mass # Mass of the robot
+ self.gI = np.array(params.I_mat.tolist()).reshape((3, 3)) # Inertia matrix in ody frame
# Friction coefficient
if inner:
@@ -37,25 +30,9 @@ class MPC_crocoddyl:
else:
self.mu = mu
- # Gain from OSQP MPC
-
- # self.w_x = np.sqrt(0.5)
- # self.w_y = np.sqrt(0.5)
- # self.w_z = np.sqrt(2.)
- # self.w_roll = np.sqrt(0.11)
- # self.w_pitch = np.sqrt(0.11)
- # self.w_yaw = np.sqrt(0.11)
- # self.w_vx = np.sqrt(2.*np.sqrt(0.5))
- # self.w_vy = np.sqrt(2.*np.sqrt(0.5))
- # self.w_vz = np.sqrt(2.*np.sqrt(2.))
- # self.w_vroll = np.sqrt(0.05*np.sqrt(0.11))
- # self.w_vpitch = np.sqrt(0.05*np.sqrt(0.11))
- # self.w_vyaw = np.sqrt(0.05*np.sqrt(0.11))
-
- # from osqp, config
- # self.stateWeight = np.sqrt([2.0, 2.0, 20.0, 0.25, 0.25, 0.25, 0.2, 0.2, 5., 0.0, 0.0, 0.3])
-
- # Set of gains to get a better behaviour with mpc height used in WBC
+ # self.stateWeight = np.sqrt([2.0, 2.0, 20.0, 0.25, 0.25, 0.25, 0.2, 0.2, 5., 0.0, 0.0, 0.3])
+
+ # Weights on the state vector
self.w_x = 0.3
self.w_y = 0.3
self.w_z = 2
@@ -69,93 +46,39 @@ class MPC_crocoddyl:
self.w_vpitch = 0.07*np.sqrt(self.w_pitch)
self.w_vyaw = 0.05*np.sqrt(self.w_yaw)
- self.stateWeight = np.array([self.w_x, self.w_y, self.w_z, self.w_roll, self.w_pitch, self.w_yaw,
+ self.stateWeights = np.array([self.w_x, self.w_y, self.w_z, self.w_roll, self.w_pitch, self.w_yaw,
self.w_vx, self.w_vy, self.w_vz, self.w_vroll, self.w_vpitch, self.w_vyaw])
- # Weight Vector : Force Norm
- self.forceWeights = np.array(4*[0.007, 0.007, 0.007])
-
- # Weight Vector : Friction cone cost
- self.frictionWeights = 1.0
-
- # Max iteration ddp solver
- self.max_iteration = 10
-
- # Warm Start for the solver
- self.warm_start = True
-
- # Minimum normal force (N)
- self.min_fz = 0.2
- self.max_fz = 25
+
+ self.forceWeights = np.array(4*[0.007, 0.007, 0.007]) # Weight Vector : Force Norm
+ self.frictionWeights = 1.0 # Weight Vector : Friction cone cost
- # Gait matrix
- self.gait = np.zeros((params.N_gait, 4))
- self.index = 0
+ self.min_fz = 0.2 # Minimum normal force (N)
+ self.max_fz = 25 # Maximum normal force (N)
- # Weight on the shoulder term :
- self.shoulderWeights = 5.
- self.shoulder_hlim = 0.23
+ self.shoulderWeights = 5. # Weight on the shoulder term :
+ self.shoulder_hlim = 0.23 # shoulder maximum height
# Integration scheme
self.implicit_integration = True
+ self.max_iteration = 10 # Max iteration ddp solver
+ self.warm_start = True # Warm Start for the solver
+
# Position of the feet
self.fsteps = np.full((params.N_gait, 12), np.nan)
+ self.gait = np.zeros((params.N_gait, 4))
+ self.index = 0
- # List of the actionModel
+ # Action models
self.ListAction = []
-
- # Initialisation of the List model using ActionQuadrupedModel()
- # The same model cannot be used [model]*(T_mpc/dt) because the dynamic
- # model changes for each nodes.
- for i in range(int(self.T_mpc/self.dt)):
- if linearModel:
- model = quadruped_walkgen.ActionModelQuadruped()
- else:
- model = quadruped_walkgen.ActionModelQuadrupedNonLinear()
-
- # Model parameters
- model.dt = self.dt
- model.mass = self.mass
- model.gI = self.gI
- model.mu = self.mu
- model.min_fz = self.min_fz
- model.max_fz = self.max_fz
-
- # Weights vectors
- model.stateWeights = self.stateWeight
- model.forceWeights = self.forceWeights
- model.frictionWeights = self.frictionWeights
- # shoulder term :
- model.shoulderWeights = self.shoulderWeights
- model.shoulder_hlim = self.shoulder_hlim
-
- # integration scheme
- model.implicit_integration = self.implicit_integration
-
- # Add model to the list of model
- self.ListAction.append(model)
-
- # Terminal Node
if linearModel:
+ self.ListAction = [quadruped_walkgen.ActionModelQuadruped() for _ in range(self.n_nodes)]
self.terminalModel = quadruped_walkgen.ActionModelQuadruped()
else:
+ self.ListAction = [quadruped_walkgen.ActionModelQuadrupedNonLinear() for _ in range(self.n_nodes)]
self.terminalModel = quadruped_walkgen.ActionModelQuadrupedNonLinear()
-
- # Model parameters of terminal node
- self.terminalModel.dt = self.dt
- self.terminalModel.mass = self.mass
- self.terminalModel.gI = self.gI
- self.terminalModel.mu = self.mu
- self.terminalModel.min_fz = self.min_fz
- self.terminalModel.shoulderWeights = self.shoulderWeights
- self.terminalModel.shoulder_hlim = self.shoulder_hlim
- self.terminalModel.implicit_integration = self.implicit_integration
-
- # Weights vectors of terminal node
- self.terminalModel.stateWeights = 10*self.stateWeight
- self.terminalModel.forceWeights = np.zeros(12)
- self.terminalModel.frictionWeights = 0.
+ self.updateActionModels()
# Shooting problem
self.problem = crocoddyl.ShootingProblem(np.zeros(12), self.ListAction, self.terminalModel)
@@ -226,27 +149,9 @@ class MPC_crocoddyl:
self.x_init = self.ddp.xs[2:]
self.x_init.insert(0, xref[:, 0])
self.x_init.append(self.ddp.xs[-1])
-
- else :
-
- self.x_init.append(xref[:, 0] )
-
- for i in range(len(self.ListAction)) :
- self.x_init.append(np.zeros(12) )
- self.u_init.append(np.zeros(12) )
-
-
-
- """print("1")
- from IPython import embed
- embed()"""
self.ddp.solve(self.x_init, self.u_init, self.max_iteration)
- """print("3")
- from IPython import embed
- embed()"""
-
return 0
def get_latest_result(self):
@@ -275,42 +180,38 @@ class MPC_crocoddyl:
return np.array(self.ddp.us)[:, :].transpose()[:, :]
- def updateActionModel(self):
+ def initializeActionModel(self, model, terminal=False):
+ """ Initialize an action model with the parameters"""
+ # Model parameters
+ model.dt = self.dt
+ model.mass = self.mass
+ model.gI = self.gI
+ model.mu = self.mu
+ model.min_fz = self.min_fz
+ model.max_fz = self.max_fz
+
+ # Weights vectors
+ model.stateWeights = self.stateWeights
+ if terminal:
+ model.forceWeights = np.zeros(12)
+ model.frictionWeights = 0.
+ else:
+ model.max_fz = self.max_fz
+ model.forceWeights = self.forceWeights
+ model.frictionWeights = self.frictionWeights
+
+ # shoulder term :
+ model.shoulderWeights = self.shoulderWeights
+ model.shoulder_hlim = self.shoulder_hlim
+
+ # integration scheme
+ model.implicit_integration = self.implicit_integration
+
+ def updateActionModels(self):
"""Update the quadruped model with the new weights or model parameters.
Useful to try new weights without modify this class
"""
+ for model in self.ListAction:
+ self.initializeActionModel(model)
- for elt in self.ListAction:
- elt.dt = self.dt
- elt.mass = self.mass
- elt.gI = self.gI
- elt.mu = self.mu
- elt.min_fz = self.min_fz
- elt.max_fz = self.max_fz
-
- # Weights vectors
- elt.stateWeights = self.stateWeight
- elt.forceWeights = self.forceWeights
- elt.frictionWeights = self.frictionWeights
-
- # shoulder term :
- elt.shoulderWeights = self.shoulderWeights
- elt.shoulder_hlim = self.shoulder_hlim
-
- # Model parameters of terminal node
- self.terminalModel.dt = self.dt
- self.terminalModel.mass = self.mass
- self.terminalModel.gI = self.gI
- self.terminalModel.mu = self.mu
- self.terminalModel.min_fz = self.min_fz
-
- # Weights vectors of terminal node
- self.terminalModel.stateWeights = self.stateWeight
- self.terminalModel.forceWeights = np.zeros(12)
- self.terminalModel.frictionWeights = 0.
-
- # shoulder term :
- self.terminalModel.shoulderWeights = self.shoulderWeights
- self.terminalModel.shoulder_hlim = self.shoulder_hlim
-
- return 0
+ self.initializeActionModel(self.terminalModel, terminal=True)
diff --git a/scripts/crocoddyl_class/MPC_crocoddyl_planner.py b/scripts/crocoddyl_class/MPC_crocoddyl_planner.py
index b3f46193c0bcfb64fad42c2bfbf632fc642a5c5a..e870b50e1a4765ef752e952e6f548267daae9e3c 100644
--- a/scripts/crocoddyl_class/MPC_crocoddyl_planner.py
+++ b/scripts/crocoddyl_class/MPC_crocoddyl_planner.py
@@ -10,27 +10,20 @@ class MPC_crocoddyl_planner():
retrieve the results.
Args:
- dt (float): time step of the MPC
- T_mpc (float): Duration of the prediction horizon
+ params (obj): Params object containing the simulation parameters
mu (float): Friction coefficient
- inner(bool): Inside or outside approximation of the friction cone
+ inner (float): Inside or outside approximation of the friction cone (mu * 1/sqrt(2))
+ linearModel(bool) : Approximation in the cross product by using desired state
"""
def __init__(self, params, mu=1, inner=True, warm_start=True, min_fz=0.0):
- # Time step of the solver
- self.dt = params.dt_mpc
-
- # Period of the MPC
- self.T_mpc = params.T_mpc
-
- # Mass of the robot
- self.mass = 2.50000279
-
- # Inertia matrix of the robot in body frame
- self.gI = np.array([[3.09249e-2, -8.00101e-7, 1.865287e-5],
- [-8.00101e-7, 5.106100e-2, 1.245813e-4],
- [1.865287e-5, 1.245813e-4, 6.939757e-2]])
+ self.dt = params.dt_mpc # Time step of the solver
+ self.T_mpc = params.T_mpc # Period of the MPC
+ self.n_nodes = int(self.T_mpc/self.dt) # Number of nodes
+ self.mass = params.mass # Mass of the robot
+ self.max_iteration = 20 # Max iteration ddp solver
+ self.gI = np.array(params.I_mat.tolist()).reshape((3, 3)) # Inertia matrix in ody frame
# Friction coefficient
if inner:
@@ -38,6 +31,8 @@ class MPC_crocoddyl_planner():
else:
self.mu = mu
+ # self.stateWeight = np.sqrt([2.0, 2.0, 20.0, 0.25, 0.25, 0.25, 0.2, 0.2, 5., 0.0, 0.0, 0.3])
+
# Weights Vector : States
self.w_x = 0.3
self.w_y = 0.3
@@ -54,72 +49,53 @@ class MPC_crocoddyl_planner():
self.stateWeights = np.array([self.w_x, self.w_y, self.w_z, self.w_roll, self.w_pitch, self.w_yaw,
self.w_vx, self.w_vy, self.w_vz, self.w_vroll, self.w_vpitch, self.w_vyaw])
- # Weight Vector : Force Norm
- self.forceWeights = np.array(4*[0.01, 0.01, 0.01])
-
- # Weight Vector : Friction cone cost
- self.frictionWeights = 0.5
-
- # Weights on the heuristic term
- self.heuristicWeights = np.array(4*[0.3, 0.4])
+ self.forceWeights = 2*np.array(4*[0.01, 0.01, 0.01]) # Weight Vector : Force Norm
+ self.frictionWeights = 1. # Weight Vector : Friction cone cost
+ self.heuristicWeights = np.array(4*[0.3, 0.4]) # Weights on the heuristic term
+ self.stepWeights = np.full(8, 0.005) # Weight on the step command (distance between steps)
+ self.stopWeights = 2.5*np.ones(8) # Weights to stop the optimisation at the end of the flying phase
+ self.shoulderContactWeight = 5. # Weight for shoulder-to-contact penalty
+ self.shoulder_hlim = 0.235
- # Weight on the step command (distance between steps)
- self.stepWeights = np.full(8, 0.05)
-
- # Weights to stop the optimisation at the end of the flying phase
- self.stopWeights = np.full(8, 1.)
-
- # Weight for shoulder-to-contact penalty
- self.shoulderContactWeight = 5
- self.shoulder_hlim = 0.225
-
- # Max iteration ddp solver
- self.max_iteration = 10
-
- # Warm-start for the solver
- # Always on, just an approximation, not the previous result
# TODO : create a proper warm-start with the previous optimisation
- self.warm_start = warm_start
+ self.warm_start = True
# Minimum normal force(N) and reference force vector bool
- self.min_fz = min_fz
+ self.min_fz = 1.
self.relative_forces = True
# Gait matrix
self.gait = np.zeros((params.N_gait, 4))
- self.gait_old = np.zeros((1, 4))
+ self.gait_old = np.zeros(4)
# Position of the feet in local frame
- self.fsteps = np.full((params.N_gait, 12), 0.0)
+ self.fsteps = np.zeros((params.N_gait, 12))
# List to generate the problem
- self.ListAction = []
+ self.action_models = []
self.x_init = []
- self.u_init = []
-
- self.l_fsteps = np.zeros((3, 4))
- self.o_fsteps = np.zeros((3, 4))
+ self.u_init = []
- # Shooting problem
- self.problem = None
-
- # ddp solver
- self.ddp = None
-
- # Xs results without the actionStepModel
- self.Xs = np.zeros((20, int(self.T_mpc/self.dt)))
- # self.Us = np.zeros((12,int(T_mpc/dt)))
+ self.problem = None # Shooting problem
+ self.ddp = None # ddp solver
+ self.Xs = np.zeros((20, int(self.T_mpc/self.dt))) # Xs results without the actionStepModel
# Initial foot location (local frame, X,Y plan)
- self.p0 = [0.1946, 0.14695, 0.1946, -0.14695, -0.1946, 0.14695, -0.1946, -0.14695]
+ self.shoulders = [0.1946, 0.14695, 0.1946, -0.14695, -0.1946, 0.14695, -0.1946, -0.14695]
+
+ # Index to stop the feet optimisation
+ self.index_lock_time = int(params.lock_time / params.dt_mpc) # Row index in the gait matrix when the optimisation of the feet should be stopped
+ self.index_stop_optimisation = [] # List of index to reset the stopWeights to 0 after optimisation
- # Index to stop the feet optimisation
- self.index_lock_time = int(params.lock_time / params.dt_mpc ) # Row index in the gait matrix when the optimisation of the feet should be stopped
- self.index_stop_optimisation = [] # List of index to reset the stopWeights to 0 after optimisation
+ # USefull to optimise around the previous optimisation
+ self.flying_foot = 4*[False] # Bool corresponding to the current flying foot (gait[0,foot_id] == 0)
+ self.flying_foot_nodes = np.zeros(4) # The number of nodes in the next phase of flight
+ self.flying_max_nodes = int(params.T_mpc / (2 * params.dt_mpc)) # TODO : get the maximum number of nodes from the gait_planner
- self.initializeModels(params)
+ # Initialize the lists of models
+ self.initialize_models(params)
- def initializeModels(self, params):
+ def initialize_models(self, params):
''' Reset the two lists of augmented and step-by-step models, to avoid recreating them at each loop.
Not all models here will necessarily be used.
@@ -129,226 +105,228 @@ class MPC_crocoddyl_planner():
self.models_augmented = []
self.models_step = []
- for j in range(params.N_gait) :
- model = quadruped_walkgen.ActionModelQuadrupedAugmented()
-
+ for _ in range(params.N_gait):
+ model = quadruped_walkgen.ActionModelQuadrupedAugmented()
self.update_model_augmented(model)
self.models_augmented.append(model)
- for j in range(4 * int(params.T_gait / params.T_mpc) ) :
- model = quadruped_walkgen.ActionModelQuadrupedStep()
-
+ for _ in range(4 * int(params.T_gait / params.T_mpc)):
+ model = quadruped_walkgen.ActionModelQuadrupedStep()
self.update_model_step(model)
self.models_step.append(model)
# Terminal node
- self.terminalModel = quadruped_walkgen.ActionModelQuadrupedAugmented()
- self.update_model_augmented(self.terminalModel)
- # Weights vectors of terminal node (no command cost)
- self.terminalModel.forceWeights = np.zeros(12)
- self.terminalModel.frictionWeights = 0.
- self.terminalModel.heuristicWeights = np.full(8, 0.0)
- self.terminalModel.stopWeights = np.full(8, 0.0)
+ self.terminal_model = quadruped_walkgen.ActionModelQuadrupedAugmented()
+ self.update_model_augmented(self.terminal_model, terminal=True)
- return 0
-
- def solve(self, k, xref, l_feet, l_stop):
+ def solve(self, k, xref, footsteps, l_stop):
""" Solve the MPC problem
Args:
k : Iteration
xref : the desired state vector
- l_feet : current position of the feet (given by planner)
+ footsteps : current position of the feet (given by planner)
l_stop : current and target position of the feet (given by footstepTragectory generator)
"""
-
- # Update the dynamic depending on the predicted feet position
- self.updateProblem(k, xref, l_feet, l_stop)
-
- # Solve problem
+ self.updateProblem(k, xref, footsteps, l_stop)
self.ddp.solve(self.x_init, self.u_init, self.max_iteration)
# Reset to 0 the stopWeights for next optimisation
- for index_stopped in self.index_stop_optimisation :
+ for index_stopped in self.index_stop_optimisation:
self.models_augmented[index_stopped].stopWeights = np.zeros(8)
- return 0
-
- def updateProblem(self, k, xref, l_feet, l_stop):
- """Update the dynamic of the model list according to the predicted position of the feet,
+ def updateProblem(self, k, xref, footsteps, l_stop):
+ """
+ Update the dynamic of the model list according to the predicted position of the feet,
and the desired state.
Args:
"""
- # Save previous gait state before updating the gait
- self.gait_old[0, :] = self.gait[0, :].copy()
-
- # Recontruct the gait based on the computed footsteps
- a = 0
- while np.any(l_feet[a, :]):
- self.gait[a, :] = (l_feet[a, ::3] != 0.0).astype(int)
- a += 1
- self.gait[a:, :] = 0.0
-
- # On swing phase before --> initialised below shoulder
- p0 = (1.0 - np.repeat(self.gait[0, :], 2)) * self.p0
- # On the ground before --> initialised with the current feet position
- p0 += np.repeat(self.gait[0, :], 2) * l_feet[0, [0, 1, 3, 4, 6, 7, 9, 10]] # (x, y) of each foot
-
+ self.compute_gait_matrix(footsteps)
+
+ p0 = (1.0 - np.repeat(self.gait[0, :], 2)) * self.shoulders \
+ + np.repeat(self.gait[0, :], 2) * footsteps[0, [0, 1, 3, 4, 6, 7, 9, 10]]
+
self.x_init.clear()
self.u_init.clear()
- self.ListAction.clear()
+ self.action_models.clear()
self.index_stop_optimisation.clear()
index_step = 0
- index_augmented = 0
- j = 0
-
- # Iterate over all phases of the gait
- while np.any(self.gait[j, :]):
- if j == 0 : # First row, need to take into account previous gait
- if np.any(self.gait[0,:] - self.gait_old[0,:]) :
- # Step model
- self.models_step[index_step].updateModel(np.reshape(l_feet[j, :], (3, 4), order='F'),
- xref[:, j+1], self.gait[0, :] - self.gait_old[0, :])
- self.ListAction.append(self.models_step[index_step])
-
- # Augmented model
- self.models_augmented[index_augmented].updateModel(np.reshape(l_feet[j, :], (3, 4), order='F'),
- l_stop, xref[:, j+1], self.gait[j, :])
-
- # Activation of the cost to stop the optimisation around l_stop (position locked by the footstepGenerator)
- if j < self.index_lock_time :
- self.models_augmented[index_augmented].stopWeights = self.stopWeights
- self.index_stop_optimisation.append(index_augmented)
-
- self.ListAction.append(self.models_augmented[index_augmented])
-
-
- index_step += 1
- index_augmented += 1
- # Warm-start
- self.x_init.append(np.concatenate([xref[:, j+1], p0]))
- self.u_init.append(np.zeros(8))
- self.x_init.append(np.concatenate([xref[:, j+1], p0]))
- self.u_init.append(np.repeat(self.gait[j, :], 3) * np.array(4*[0., 0., 2.5*9.81/np.sum(self.gait[j, :])] ))
+ index_augmented = 0
+ j = 1
+
+ stopping_needed = 4*[False]
+ if k > 110 :
+ for index_foot, is_flying in enumerate(self.flying_foot):
+ if is_flying:
+ stopping_needed[index_foot] = self.flying_foot_nodes[index_foot] != self.flying_max_nodes # Position optimized at the previous control cycle
+
+ # Augmented model, first node, j = 0
+ self.models_augmented[index_augmented].updateModel(np.reshape(footsteps[0, :], (3, 4), order='F'),
+ l_stop, xref[:, 1], self.gait[0, :])
+ self.action_models.append(self.models_augmented[index_augmented])
+
+ index_augmented += 1
+ # Warm-start
+ self.x_init.append(np.concatenate([xref[:, 1], p0]))
+ self.u_init.append(np.repeat(self.gait[0, :], 3) * np.array(4*[0., 0., 2.5*9.81/np.sum(self.gait[0, :])]))
+
+ while np.any(self.gait[j, :]):
+ if np.any(self.gait[j, :] - self.gait[j-1, :]):
+ # Step model
+ self.models_step[index_step].updateModel(np.reshape(footsteps[j, :], (3, 4), order='F'),
+ xref[:, j+1], self.gait[j, :] - self.gait[j-1, :])
+ self.action_models.append(self.models_step[index_step])
+
+ # Augmented model
+ self.models_augmented[index_augmented].updateModel(np.reshape(footsteps[j, :], (3, 4), order='F'),
+ l_stop, xref[:, j+1], self.gait[j, :])
+
+ # Activation of the cost to stop the optimisation around l_stop (position locked by the footstepGenerator)
+ # if j < self.index_lock_time:
+ # self.models_augmented[index_augmented].stopWeights = self.stopWeights
+ # self.index_stop_optimisation.append(index_augmented)
+
+ feet_ground = np.where(self.gait[j,:] == 1)[0]
+ activation_cost = False
+ for foot in range(4):
+ if (stopping_needed[foot]) and (self.flying_foot[foot]) and (foot in feet_ground) and (j < int(self.flying_foot_nodes[foot] + self.flying_max_nodes) ) :
+ coeff_activated = np.zeros(8)
+ coeff_activated[2*foot: 2*foot + 2] = np.array([1,1])
+ self.models_augmented[index_augmented].stopWeights = self.models_augmented[index_augmented].stopWeights + coeff_activated*self.stopWeights
+ activation_cost = True
- else :
- # Augmented model
- self.models_augmented[index_augmented].updateModel(np.reshape(l_feet[j, :], (3, 4), order='F'),
- l_stop, xref[:, j+1], self.gait[j, :])
- self.ListAction.append(self.models_augmented[index_augmented])
-
- index_augmented += 1
- # Warm-start
- self.x_init.append(np.concatenate([xref[:, j+1], p0]))
- self.u_init.append(np.repeat(self.gait[j, :], 3) * np.array(4*[0., 0., 2.5*9.81/np.sum(self.gait[j, :])] ))
-
- else :
- if np.any(self.gait[j,:] - self.gait[j-1,:]) :
- # Step model
- self.models_step[index_step].updateModel(np.reshape(l_feet[j, :], (3, 4), order='F'),
- xref[:, j+1], self.gait[j, :] - self.gait[j-1, :])
- self.ListAction.append(self.models_step[index_step])
-
- # Augmented model
- self.models_augmented[index_augmented].updateModel(np.reshape(l_feet[j, :], (3, 4), order='F'),
- l_stop, xref[:, j+1], self.gait[j, :])
-
- # Activation of the cost to stop the optimisation around l_stop (position locked by the footstepGenerator)
- if j < self.index_lock_time :
- self.models_augmented[index_augmented].stopWeights = self.stopWeights
- self.index_stop_optimisation.append(index_augmented)
-
- self.ListAction.append(self.models_augmented[index_augmented])
-
- index_step += 1
- index_augmented += 1
- # Warm-start
- self.x_init.append(np.concatenate([xref[:, j+1], p0]))
- self.u_init.append(np.zeros(8))
- self.x_init.append(np.concatenate([xref[:, j+1], p0]))
- self.u_init.append(np.repeat(self.gait[j, :], 3) * np.array(4*[0., 0., 2.5*9.81/np.sum(self.gait[j, :])] ))
-
+ if activation_cost :
+ self.index_stop_optimisation.append(index_augmented)
+
+
+ self.action_models.append(self.models_augmented[index_augmented])
+
+ index_step += 1
+ index_augmented += 1
+ # Warm-start
+ self.x_init.append(np.concatenate([xref[:, j+1], p0]))
+ self.u_init.append(np.zeros(8))
+ self.x_init.append(np.concatenate([xref[:, j+1], p0]))
+ self.u_init.append(np.repeat(self.gait[j, :], 3) * np.array(4*[0., 0., 2.5*9.81/np.sum(self.gait[j, :])]))
+
+ else:
+ self.models_augmented[index_augmented].updateModel(np.reshape(footsteps[j, :], (3, 4), order='F'),
+ l_stop, xref[:, j+1], self.gait[j, :])
+ self.action_models.append(self.models_augmented[index_augmented])
+
+ feet_ground = np.where(self.gait[j,:] == 1)[0]
+ activation_cost = False
+ for foot in range(4):
+ if (stopping_needed[foot]) and (self.flying_foot[foot]) and (foot in feet_ground) and (j < int(self.flying_foot_nodes[foot] + self.flying_max_nodes) ) :
+ coeff_activated = np.zeros(8)
+ coeff_activated[2*foot: 2*foot + 2] = np.array([1,1])
+ self.models_augmented[index_augmented].stopWeights = self.models_augmented[index_augmented].stopWeights + coeff_activated*self.stopWeights
+ activation_cost = True
- else :
- self.models_augmented[index_augmented].updateModel(np.reshape(l_feet[j, :], (3, 4), order='F'),
- l_stop, xref[:, j+1], self.gait[j, :])
- self.ListAction.append(self.models_augmented[index_augmented])
-
- index_augmented += 1
- # Warm-start
- self.x_init.append(np.concatenate([xref[:, j+1], p0]))
- self.u_init.append(np.repeat(self.gait[j, :], 3) * np.array(4*[0., 0., 2.5*9.81/np.sum(self.gait[j, :])] ))
+ if activation_cost :
+ self.index_stop_optimisation.append(index_augmented)
+
+ index_augmented += 1
+ # Warm-start
+ self.x_init.append(np.concatenate([xref[:, j+1], p0]))
+ self.u_init.append(np.repeat(self.gait[j, :], 3) * np.array(4*[0., 0., 2.5*9.81/np.sum(self.gait[j, :])]))
# Update row matrix
j += 1
- # Update terminal model
- self.terminalModel.updateModel(np.reshape(l_feet[j-1, :], (3, 4), order='F'), l_stop, xref[:, -1], self.gait[j-1, :])
+ # Update terminal model
+ self.terminal_model.updateModel(np.reshape(footsteps[j-1, :], (3, 4), order='F'), l_stop, xref[:, -1], self.gait[j-1, :])
# Warm-start
- self.x_init.append(np.concatenate([xref[:, j-1], p0]))
-
- # Shooting problem
- self.problem = crocoddyl.ShootingProblem(np.zeros(20), self.ListAction, self.terminalModel)
+ self.x_init.append(np.concatenate([xref[:, j-1], p0]))
+ self.problem = crocoddyl.ShootingProblem(np.zeros(20), self.action_models, self.terminal_model)
self.problem.x0 = np.concatenate([xref[:, 0], p0])
- # DDP Solver
self.ddp = crocoddyl.SolverDDP(self.problem)
- return 0
-
- def get_latest_result(self):
- """Return the desired contact forces that have been computed by the last iteration of the MPC
- Args:
+ def get_latest_result(self, oRh, oTh):
+ """
+ Return the desired contact forces that have been computed by the last iteration of the MPC
+ Args :
+ - q ( Array 7x1 ) : pos, quaternion orientation
"""
-
- cpt = 0
+ index = 0
N = int(self.T_mpc / self.dt)
result = np.zeros((32, N))
- for i in range(len(self.ListAction)):
- if self.ListAction[i].__class__.__name__ != "ActionModelQuadrupedStep":
- if cpt >= N:
+ for i in range(len(self.action_models)):
+ if self.action_models[i].__class__.__name__ != "ActionModelQuadrupedStep":
+ if index >= N:
raise ValueError("Too many action model considering the current MPC prediction horizon")
- result[:12, cpt] = self.ddp.xs[i][:12]
- result[12:24, cpt] = self.ddp.us[i] # * np.repeat(self.gait[cpt, :] , 3)
- result[24:, cpt] = self.ddp.xs[i][12:]
- cpt += 1
- if i > 0 and self.ListAction[i-1].__class__.__name__ == "ActionModelQuadrupedStep":
- # print(self.ddp.xs[i][12:])
+ result[:12, index] = self.ddp.xs[i+1][:12] # First node correspond to current state
+ result[12:24, index] = self.ddp.us[i]
+ result[24:, index] = ( oRh[:2,:2] @ (self.ddp.xs[i+1][12:].reshape((2,4) , order = "F") ) + oTh[:2]).reshape((8), order = "F")
+ if i > 0 and self.action_models[i-1].__class__.__name__ == "ActionModelQuadrupedStep":
pass
+ index += 1
return result
- def update_model_augmented(self, model):
- '''Set intern parameters for augmented model type
- '''
+ def update_model_augmented(self, model, terminal=False):
+ """
+ Set intern parameters for augmented model type
+ """
# Model parameters
model.dt = self.dt
model.mass = self.mass
model.gI = self.gI
model.mu = self.mu
model.min_fz = self.min_fz
- model.relative_forces = self.relative_forces
+ model.relative_forces = True
model.shoulderContactWeight = self.shoulderContactWeight
+ model.shoulder_hlim = self.shoulder_hlim
# Weights vectors
model.stateWeights = self.stateWeights
- model.forceWeights = self.forceWeights
- model.frictionWeights = self.frictionWeights
+ model.stopWeights = np.zeros(8)
- # Weight on feet position
- model.stopWeights = np.full(8, 0.0) # Will be updated when necessary
- model.heuristicWeights = self.heuristicWeights
-
- return 0
+ if terminal:
+ self.terminal_model.forceWeights = np.zeros(12)
+ self.terminal_model.frictionWeights = 0.
+ self.terminal_model.heuristicWeights = np.zeros(8)
+ else:
+ model.frictionWeights = self.frictionWeights
+ model.forceWeights = self.forceWeights
+ model.heuristicWeights = self.heuristicWeights
def update_model_step(self, model):
- """Set intern parameters for step model type
+ """
+ Set intern parameters for step model type
"""
model.heuristicWeights = np.zeros(8)
model.stateWeights = self.stateWeights
model.stepWeights = self.stepWeights
- return 0
\ No newline at end of file
+ def compute_gait_matrix(self, footsteps):
+ """
+ Recontruct the gait based on the computed footstepsC
+ Args:
+ footsteps : current and predicted position of the feet
+ """
+
+ self.gait_old = self.gait[0, :].copy()
+
+ j = 0
+ self.gait = np.zeros(np.shape(self.gait))
+ while np.any(footsteps[j, :]):
+ self.gait[j, :] = (footsteps[j, ::3] != 0.0).astype(int)
+ j += 1
+
+ # Get the current flying feet and the number of nodes
+ for foot in range(4):
+ row = 0
+ if self.gait[0, foot] == 0:
+ self.flying_foot[foot] = True
+ while self.gait[row, foot] == 0:
+ row += 1
+ self.flying_foot_nodes[foot] = int(row)
+ else:
+ self.flying_foot[foot] = False
+
+
diff --git a/scripts/solo3D/tools/vizu_help.py b/scripts/solo3D/tools/vizu_help.py
new file mode 100644
index 0000000000000000000000000000000000000000..70da222f983acc6e7d5a6e53bdc9032246616241
--- /dev/null
+++ b/scripts/solo3D/tools/vizu_help.py
@@ -0,0 +1,508 @@
+import numpy as np
+import pybullet as pyb
+import pybullet_data
+from time import perf_counter as clock
+import pinocchio as pin
+# from solo3D.tools.geometry import EulerToQuaternion
+import os
+
+class PybVisualizationTraj():
+ ''' Class used to vizualise the feet trajectory on the pybullet simulation
+ '''
+
+ def __init__(self, footTrajectoryGenerator, enable_pyb_GUI):
+
+ # Pybullet enabled
+ self.enable_pyb_GUI = enable_pyb_GUI
+
+ self.footTrajectoryGenerator = footTrajectoryGenerator
+
+
+ # self.ftps_Ids_target = [0] * 4
+ # self.n_points = 15
+
+ # n_points for the traj , 4 feet , 3 target max in futur
+ # self.trajectory_Ids = np.zeros((3, 4, self.n_points))
+ self.ftps_Ids_target = np.zeros((3, 4))
+
+
+
+ def update(self, k , device , o_targetFootstep_mpc, o_targetFootstep_planner) :
+ ''' Update position of the objects in pybullet environment.
+ Args :
+ - k (int) : step of the simulation
+ - device (object) : Class used to set up the pybullet env
+ '''
+
+ if k == 1: # k ==0, device is still a dummy object, pyb env did not started
+ self.initializeEnv()
+
+
+ if self.enable_pyb_GUI and k > 100:
+
+ self.updateTarget(o_targetFootstep_mpc, o_targetFootstep_planner)
+
+ # # Update position of PyBullet camera
+ # # self.updateCamera(k , device)
+
+ # if k % self.refresh == 0:
+
+ # t_init = clock()
+
+ # # # Update target trajectory, current and next phase
+ # self.updateTargetTrajectory()
+
+ # # # Update refrence trajectory
+ # # self.updateRefTrajectory()
+
+ # # update constraints
+ # # self.updateConstraints()
+
+ # #update Sl1M :
+ # # self.updateSl1M_target(all_feet_pos)
+
+ # t_end = clock() - t_init
+ # print("Time for update pybullet = ", t_end)
+
+ return 0
+
+ def updateTarget(self, o_targetFootstep_mpc, o_targetFootstep_planner ):
+
+ for foot in range(4) :
+ pyb.resetBasePositionAndOrientation(int(self.ftps_Ids_target[0, foot]),
+ posObj=o_targetFootstep_mpc[:, foot],
+ ornObj=np.array([0.0, 0.0, 0.0, 1.0]))
+
+ target_footstep_curve = self.footTrajectoryGenerator.getFootPosition()
+ for foot in range(4) :
+ pyb.resetBasePositionAndOrientation(int(self.ftps_Ids_target[1, foot]),
+ posObj=target_footstep_curve[:, foot],
+ ornObj=np.array([0.0, 0.0, 0.0, 1.0]))
+
+ # for foot in range(4) :
+ # pyb.resetBasePositionAndOrientation(int(self.ftps_Ids_target[2, foot]),
+ # posObj=o_targetFootstep_planner[:, foot],
+ # ornObj=np.array([0.0, 0.0, 0.0, 1.0]))
+
+
+ return 0
+
+ def initializeEnv(self):
+ ''' Load object in pybullet environment.
+ '''
+
+ print("Loading pybullet object ...")
+
+ pyb.setAdditionalSearchPath(pybullet_data.getDataPath())
+ for i in range(self.ftps_Ids_target.shape[0]): # nb of feet target in futur
+
+ # rgbaColor : [R , B , G , alpha opacity]
+ if i == 0:
+ rgba = [0., 1., 0., 1.]
+
+ elif i == 1 :
+ rgba = [1., 0., 0., 1.]
+ else :
+ rgba = [0., 0., 1., 1.]
+
+
+ mesh_scale = [0.008, 0.008, 0.008]
+ visualShapeId = pyb.createVisualShape(shapeType=pyb.GEOM_MESH,
+ fileName="sphere_smooth.obj",
+ halfExtents=[0.5, 0.5, 0.1],
+ rgbaColor=rgba,
+ specularColor=[0.4, .4, 0],
+ visualFramePosition=[0.0, 0.0, 0.0],
+ meshScale=mesh_scale)
+ for j in range(4):
+ self.ftps_Ids_target[i, j] = pyb.createMultiBody(baseMass=0.0,
+ baseInertialFramePosition=[0, 0, 0],
+ baseVisualShapeIndex=visualShapeId,
+ basePosition=[0.0, 0.0, -0.1],
+ useMaximalCoordinates=True)
+ return 0
+
+
+
+ # def updateConstraints(self):
+
+ # gait = self.gait.getCurrentGait()
+ # fsteps = self.footStepPlannerQP.getFootsteps()
+ # footsteps = fsteps[0].reshape((3,4) , order ="F")
+
+ # feet = np.where(gait[0,:] == 1)[0]
+
+ # for i in range(4):
+ # if i in feet :
+ # pyb.resetBasePositionAndOrientation(int( self.constraints_objects[i]),
+ # posObj=footsteps[:,i],
+ # ornObj=np.array([0.0, 0.0, 0.0, 1.0]))
+
+ # else :
+ # pyb.resetBasePositionAndOrientation(int( self.constraints_objects[i]),
+ # posObj=np.array([0.,0.,-1]),
+ # ornObj=np.array([0.0, 0.0, 0.0, 1.0]))
+
+ # return 0
+
+ # def updateCamera(self, k, device):
+ # # Update position of PyBullet camera on the robot position to do as if it was attached to the robot
+ # if k > 10 and self.enable_pyb_GUI:
+ # pyb.resetDebugVisualizerCamera(cameraDistance=0.95, cameraYaw=357, cameraPitch=-29,
+ # cameraTargetPosition=[0.6, 0.14, -0.22])
+ # # pyb.resetDebugVisualizerCamera(cameraDistance=1., cameraYaw=357, cameraPitch=-28,
+ # # cameraTargetPosition=[device.dummyHeight[0], device.dummyHeight[1], 0.0])
+
+ # return 0
+
+ # def updateSl1M_target(self,all_feet_pos) :
+ # ''' Update position of the SL1M target
+ # Args :
+ # - all_feet_pos : list of optimized position such as : [[Foot 1 next_pos, None , Foot1 next_pos] , [Foot 2 next_pos, None , Foot2 next_pos] ]
+ # '''
+
+ # for i in range(len(all_feet_pos[0])) :
+ # for j in range(len(all_feet_pos)) :
+ # if all_feet_pos[j][i] is None :
+ # pyb.resetBasePositionAndOrientation(int(self.sl1m_Ids_target[i, j]),
+ # posObj=np.array([0., 0., -0.5]),
+ # ornObj=np.array([0.0, 0.0, 0.0, 1.0]))
+
+ # else :
+ # pyb.resetBasePositionAndOrientation(int(self.sl1m_Ids_target[i, j]),
+ # posObj=all_feet_pos[j][i],
+ # ornObj=np.array([0.0, 0.0, 0.0, 1.0]))
+
+
+
+
+ # return 0
+
+ # def updateRefTrajectory(self):
+ # ''' Update the reference state trajectory given by StatePlanner Class.
+ # '''
+
+ # referenceState = self.statePlanner.getReferenceStates()
+
+ # for k in range(self.state_Ids.shape[0]):
+
+ # quat = EulerToQuaternion(referenceState[3:6, k])
+ # pyb.resetBasePositionAndOrientation(int(self.state_Ids[k]),
+ # posObj=referenceState[:3, k] + np.array([0., 0., 0.04]),
+ # ornObj=quat)
+
+ # # Update surface :
+
+ # a, b, c = self.statePlanner.surface_equation
+ # pos = self.statePlanner.surface_point
+
+ # RPY = np.array([-np.arctan2(b, 1.), -np.arctan2(a, 1.), 0.])
+ # quat = EulerToQuaternion(RPY)
+ # pyb.resetBasePositionAndOrientation(int(self.surface_Id),
+ # posObj=pos,
+ # ornObj=quat)
+
+ # return 0
+
+ # def updateTargetTrajectory(self):
+ # ''' Update the target trajectory for current and next phases. Hide the unnecessary spheres.
+ # '''
+
+ # gait = self.gait.getCurrentGait()
+ # fsteps = self.footStepPlannerQP.getFootsteps()
+
+ # for j in range(4):
+
+ # # Count the position of the plotted trajectory in the temporal horizon
+ # # c = 0 --> Current trajectory/foot pos
+ # # c = 1 --> Next trajectory/foot pos
+ # c = 0
+ # i = 0
+
+ # init_pos = np.zeros(3)
+
+ # while gait[i, :].any():
+ # footsteps = fsteps[i].reshape((3,4) , order ="F")
+ # if i > 0:
+ # if (1 - gait[i-1, j]) * gait[i, j] > 0: # from flying phase to stance
+
+
+
+
+ # # In any case plot target
+ # pyb.resetBasePositionAndOrientation(int(self.ftps_Ids_target[c, j]),
+ # posObj=footsteps[:, j],
+ # ornObj=np.array([0.0, 0.0, 0.0, 1.0]))
+ # if c == 0:
+ # # Current flying phase, using coeff store in Bezier curve class
+ # t0 = self.footTrajectoryGenerator.t0s[j]
+ # t1 = self.footTrajectoryGenerator.t_swing[j]
+ # t_vector = np.linspace(t0, t1, self.n_points)
+
+ # for id_t, t in enumerate(t_vector):
+ # # Bezier trajectory
+ # pos = self.footTrajectoryGenerator.evaluateBezier(j, 0, t)
+ # # Polynomial Curve 5th order
+ # # pos = self.footTrajectoryGenerator.evaluatePoly(j, 0, t)
+ # pyb.resetBasePositionAndOrientation(int(self.trajectory_Ids[c, j, id_t]),
+ # posObj=pos,
+ # ornObj=np.array([0.0, 0.0, 0.0, 1.0]))
+
+ # else:
+ # # Next phase, using a simple polynomial curve to approximate the next trajectory
+ # t0 = 0.
+ # t1 = self.gait.getPhaseDuration(i, j, 1.0)
+ # t_vector = np.linspace(t0, t1, self.n_points)
+
+ # self.footTrajectoryGenerator.updatePolyCoeff_simple(j, init_pos, footsteps[:, j], t1)
+ # for id_t, t in enumerate(t_vector):
+ # pos = self.footTrajectoryGenerator.evaluatePoly_simple(j, 0, t)
+ # pyb.resetBasePositionAndOrientation(int(self.trajectory_Ids[c, j, id_t]),
+ # posObj=pos,
+ # ornObj=np.array([0.0, 0.0, 0.0, 1.0]))
+
+ # c += 1
+
+ # if gait[i-1, j] * (1 - gait[i, j]) > 0:
+ # # Starting a flying phase
+ # footsteps_previous = fsteps[i-1].reshape((3,4) , order ="F")
+ # init_pos[:] = footsteps_previous[:, j]
+
+ # else:
+ # if gait[i, j] == 1:
+ # # foot already on the ground
+ # pyb.resetBasePositionAndOrientation(int(self.ftps_Ids_target[c, j]),
+ # posObj=footsteps[:, j],
+ # ornObj=np.array([0.0, 0.0, 0.0, 1.0]))
+
+ # # not hidden in the floor, traj
+ # if not pyb.getBasePositionAndOrientation(int(self.trajectory_Ids[0, j, 0]))[0][2] == -0.1:
+ # for t in range(self.n_points):
+ # pyb.resetBasePositionAndOrientation(int(self.trajectory_Ids[0, j, t]),
+ # posObj=np.array([0., 0., -0.1]),
+ # ornObj=np.array([0.0, 0.0, 0.0, 1.0]))
+
+ # c += 1
+
+ # i += 1
+
+ # # Hide the sphere objects not used
+ # while c < self.ftps_Ids_target.shape[0]:
+
+ # # not hidden in the floor, target
+ # if not pyb.getBasePositionAndOrientation(int(self.ftps_Ids_target[c, j]))[0][2] == -0.1:
+ # pyb.resetBasePositionAndOrientation(int(self.ftps_Ids_target[c, j]),
+ # posObj=np.array([0., 0., -0.1]),
+ # ornObj=np.array([0.0, 0.0, 0.0, 1.0]))
+
+ # # not hidden in the floor, traj
+ # if not pyb.getBasePositionAndOrientation(int(self.trajectory_Ids[c, j, 0]))[0][2] == -0.1:
+ # for t in range(self.n_points):
+ # pyb.resetBasePositionAndOrientation(int(self.trajectory_Ids[c, j, t]),
+ # posObj=np.array([0., 0., -0.1]),
+ # ornObj=np.array([0.0, 0.0, 0.0, 1.0]))
+
+ # c += 1
+
+ # return 0
+
+ # def initializeEnv(self):
+ # ''' Load object in pybullet environment.
+ # '''
+
+ # print("Loading pybullet object ...")
+
+ # pyb.setAdditionalSearchPath(pybullet_data.getDataPath())
+
+ # Stairs object
+ # Add stairs with platform and bridge
+ # self.stairsId = pyb.loadURDF(self.ENV_URDF)
+ # pyb.changeDynamics(self.stairsId, -1, lateralFriction=1.0)
+ #pyb.setAdditionalSearchPath("/home/corberes/Bureau/edin/my_quad_reactive/quadruped-reactive-walking/scripts/solo3D/objects/")
+ # path_mesh = "solo3D/objects/object_" + str(self.object_stair) + "/meshes/"
+ # for elt in os.listdir(path_mesh) :
+ # name_object = path_mesh + elt
+
+ # mesh_scale = [1.0, 1., 1.]
+ # # Add stairs with platform and bridge
+ # visualShapeId = pyb.createVisualShape(shapeType=pyb.GEOM_MESH,
+ # fileName=name_object,
+ # rgbaColor=[.3, 0.3, 0.3, 1.0],
+ # specularColor=[0.4, .4, 0],
+ # visualFramePosition=[0.0, 0.0, 0.0],
+ # meshScale=mesh_scale)
+
+ # collisionShapeId = pyb.createCollisionShape(shapeType=pyb.GEOM_MESH,
+ # fileName=name_object,
+ # collisionFramePosition=[0.0, 0.0, 0.0],
+ # meshScale=mesh_scale)
+
+ # tmpId = pyb.createMultiBody(baseMass=0.0,
+ # baseInertialFramePosition=[0, 0, 0],
+ # baseCollisionShapeIndex=collisionShapeId,
+ # baseVisualShapeIndex=visualShapeId,
+ # basePosition=[0.0, 0., 0.0],
+ # useMaximalCoordinates=True)
+ # pyb.changeDynamics(tmpId, -1, lateralFriction=1.0)
+
+ # Sphere Object for target footsteps :
+ # for i in range(self.ftps_Ids_target.shape[0]): # nb of feet target in futur
+
+ # # rgbaColor : [R , B , G , alpha opacity]
+ # if i == 0:
+ # rgba = [0.41, 1., 0., 1.]
+
+ # else:
+ # rgba = [0.41, 1., 0., 0.5]
+
+ # mesh_scale = [0.008, 0.008, 0.008]
+ # visualShapeId = pyb.createVisualShape(shapeType=pyb.GEOM_MESH,
+ # fileName="sphere_smooth.obj",
+ # halfExtents=[0.5, 0.5, 0.1],
+ # rgbaColor=rgba,
+ # specularColor=[0.4, .4, 0],
+ # visualFramePosition=[0.0, 0.0, 0.0],
+ # meshScale=mesh_scale)
+ # for j in range(4):
+ # self.ftps_Ids_target[i, j] = pyb.createMultiBody(baseMass=0.0,
+ # baseInertialFramePosition=[0, 0, 0],
+ # baseVisualShapeIndex=visualShapeId,
+ # basePosition=[0.0, 0.0, -0.1],
+ # useMaximalCoordinates=True)
+
+ # Sphere Object for trajcetories :
+ # for i in range(self.trajectory_Ids.shape[0]):
+
+ # # rgbaColor : [R , B , G , alpha opacity]
+ # if i == 0:
+ # rgba = [0.41, 1., 0., 1.]
+ # else:
+ # rgba = [0.41, 1., 0., 0.5]
+
+ # mesh_scale = [0.0035, 0.0035, 0.0035]
+ # visualShapeId = pyb.createVisualShape(shapeType=pyb.GEOM_MESH,
+ # fileName="sphere_smooth.obj",
+ # halfExtents=[0.5, 0.5, 0.1],
+ # rgbaColor=rgba,
+ # specularColor=[0.4, .4, 0],
+ # visualFramePosition=[0.0, 0.0, 0.0],
+ # meshScale=mesh_scale)
+ # for j in range(4):
+ # for id_t in range(self.n_points):
+ # self.trajectory_Ids[i, j, id_t] = pyb.createMultiBody(baseMass=0.0,
+ # baseInertialFramePosition=[0, 0, 0],
+ # baseVisualShapeIndex=visualShapeId,
+ # basePosition=[0.0, 0.0, -0.1],
+ # useMaximalCoordinates=True)
+
+ # # Cube for planner trajectory
+ # for i in range(self.state_Ids.shape[0]):
+ # # rgbaColor : [R , B , G , alpha opacity]
+ # if i == 0:
+ # rgba = [0., 0., 1., 1.]
+ # else:
+ # rgba = [0., 0., 1., 1.]
+
+ # mesh_scale = [0.005, 0.005, 0.005]
+ # visualShapeId = pyb.createVisualShape(shapeType=pyb.GEOM_MESH,
+ # fileName="sphere_smooth.obj",
+ # halfExtents=[0.5, 0.5, 0.1],
+ # rgbaColor=rgba,
+ # specularColor=[0.4, .4, 0],
+ # visualFramePosition=[0.0, 0.0, 0.0],
+ # meshScale=mesh_scale)
+
+ # self.state_Ids[i] = pyb.createMultiBody(baseMass=0.0,
+ # baseInertialFramePosition=[0, 0, 0],
+ # baseVisualShapeIndex=visualShapeId,
+ # basePosition=[0.0, 0.0, -0.1],
+ # useMaximalCoordinates=True)
+
+ # # Cube for surface Planner
+
+ # rgba = [0., 0., 1., 0.2]
+
+ # mesh_scale = [2*self.statePlanner.FIT_SIZE_X, 2*self.statePlanner.FIT_SIZE_Y, 0.001]
+ # visualShapeId = pyb.createVisualShape(shapeType=pyb.GEOM_MESH,
+ # fileName="cube.obj",
+ # halfExtents=[0.5, 0.5, 0.1],
+ # rgbaColor=rgba,
+ # specularColor=[0.4, .4, 0],
+ # visualFramePosition=[0.0, 0.0, 0.0],
+ # meshScale=mesh_scale)
+
+ # self.surface_Id = pyb.createMultiBody(baseMass=0.0,
+ # baseInertialFramePosition=[0, 0, 0],
+ # baseVisualShapeIndex=visualShapeId,
+ # basePosition=[0.0, 0.0, -0.1],
+ # useMaximalCoordinates=True)
+
+ # Object for constraints :
+ # n_effectors = 4
+ # # kinematic_constraints_path = "/home/thomas_cbrs/Library/solo-rbprm/data/com_inequalities/feet_quasi_flat/"
+ # # limbs_names = ["FLleg" , "FRleg" , "HLleg" , "HRleg"]
+ # kinematic_constraints_path = os.getcwd() + "/solo3D/objects/constraints_sphere/"
+ # limbs_names = ["FL" , "FR" , "HL" , "HR"]
+ # for foot in range(n_effectors):
+ # foot_name = limbs_names[foot]
+ # # filekin = kinematic_constraints_path + "COM_constraints_in_" + \
+ # # foot_name + "_effector_frame_quasi_static_reduced.obj"
+ # filekin = kinematic_constraints_path + "COM_constraints_" + \
+ # foot_name + "3.obj"
+
+ # mesh_scale = [1.,1.,1.]
+ # rgba = [0.,0.,1.,0.2]
+ # visualShapeId = pyb.createVisualShape(shapeType=pyb.GEOM_MESH,
+ # fileName=filekin,
+ # rgbaColor=rgba,
+ # specularColor=[0.4, .4, 0],
+ # visualFramePosition=[0.0, 0.0, 0.0],
+ # meshScale=mesh_scale)
+
+ # self.constraints_objects[foot] = pyb.createMultiBody(baseMass=0.0,
+ # baseInertialFramePosition=[0, 0, 0],
+ # baseVisualShapeIndex=visualShapeId,
+ # basePosition=[0.0, 0.0, -0.1],
+ # useMaximalCoordinates=True)
+
+ # Sphere Object for trajcetories :
+ # 5 phases + init pos
+ # for i in range(6):
+
+ # # rgbaColor : [R , B , G , alpha opacity]
+ # # if i == 0 :
+ # # rgba = [1., 0., 1., 1.]
+ # # else :
+ # # rgba = [1., (1/7)*i, 0., 1.]
+
+ # rgba_list = [[1.,0.,0.,1.],
+ # [1.,0.,1.,1.],
+ # [1.,1.,0.,1.],
+ # [0.,0.,1.,1.]]
+
+
+ # mesh_scale = [0.01, 0.01, 0.01]
+
+ # for j in range(4):
+ # rgba = rgba_list[j]
+ # rgba[-1] = 1 - (1/9)*i
+ # visualShapeId = pyb.createVisualShape(shapeType=pyb.GEOM_MESH,
+ # fileName="sphere_smooth.obj",
+ # halfExtents=[0.5, 0.5, 0.1],
+ # rgbaColor=rgba,
+ # specularColor=[0.4, .4, 0],
+ # visualFramePosition=[0.0, 0.0, 0.0],
+ # meshScale=mesh_scale)
+
+
+ # self.sl1m_Ids_target[i, j] = pyb.createMultiBody(baseMass=0.0,
+ # baseInertialFramePosition=[0, 0, 0],
+ # baseVisualShapeIndex=visualShapeId,
+ # basePosition=[0.0, 0.0, -0.1],
+ # useMaximalCoordinates=True)
+
+
+
+ print("pybullet object loaded")
+
+ return 0
\ No newline at end of file
diff --git a/src/config_solo12.yaml b/src/config_solo12.yaml
index ca0cef1a7fde5144a13b9420897d3a9839a63f35..01e8c291d8fd442e090e772d63ebb8dd2c023c47 100644
--- a/src/config_solo12.yaml
+++ b/src/config_solo12.yaml
@@ -11,7 +11,7 @@ robot:
N_SIMULATION: 30000 # Number of simulated wbc time steps
enable_pyb_GUI: true # Enable/disable PyBullet GUI
enable_corba_viewer: false # Enable/disable Corba Viewer
- enable_multiprocessing: false # Enable/disable running the MPC in another process in parallel of the main loop
+ enable_multiprocessing: true # Enable/disable running the MPC in another process in parallel of the main loop
perfect_estimator: false # Enable/disable perfect estimator by using data directly from PyBullet
# General control parameters
@@ -22,7 +22,7 @@ robot:
dt_mpc: 0.02 # Time step of the model predictive control
T_gait: 0.48 # Duration of one gait period
T_mpc: 0.48 # Duration of the prediction horizon
- type_MPC: 1 # Which MPC solver you want to use: 0 for OSQP MPC, 1, 2, 3 for Crocoddyl MPCs
+ type_MPC: 3 # Which MPC solver you want to use: 0 for OSQP MPC, 1, 2, 3 for Crocoddyl MPCs
kf_enabled: false # Use complementary filter (False) or kalman filter (True) for the estimator
Kp_main: 6.0 # Proportional gains for the PD+
Kd_main: 0.3 # Derivative gains for the PD+