diff --git a/config/walk_parameters.yaml b/config/walk_parameters.yaml
index df3b5ffbee65beb42aeaf271c44f78dd65c72194..1309d6fef4d4ce470acd74e4ad83b84dbb7f1979 100644
--- a/config/walk_parameters.yaml
+++ b/config/walk_parameters.yaml
@@ -11,7 +11,7 @@ robot:
envID: 0 # Identifier of the environment to choose in which one the simulation will happen
use_flat_plane: true # If True the ground is flat, otherwise it has bumps
predefined_vel: true # If we are using a predefined reference velocity (True) or a joystick (False)
- N_SIMULATION: 2000 # Number of simulated wbc time steps
+ N_SIMULATION: 20000 # Number of simulated wbc time steps
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
perfect_estimator: true # Enable/disable perfect estimator by using data directly from PyBullet
@@ -22,12 +22,12 @@ robot:
# q_init: [0.0, 0.764, -1.407, 0.0, 0.76407, -1.4, 0.0, 0.76407, -1.407, 0.0, 0.764, -1.407] #Â h_com = 0.218
q_init: [0.0, 0.7, -1.4, 0.0, 0.7, -1.4, 0.0, -0.7, 1.4, 0.0, -0.7, 1.4] # Initial articular positions
dt_wbc: 0.001 # Time step of the whole body control
- dt_mpc: 0.02 # Time step of the model predictive control
+ dt_mpc: 0.002 # Time step of the model predictive control
type_MPC: 3 # Which MPC solver you want to use: 0 for OSQP MPC, 1, 2, 3 for Crocoddyl MPCs
# Kp_main: [0.0, 0.0, 0.0] # Proportional gains for the PD+
- Kp_main: [100.0, 100.0, 100.0] # Proportional gains for the PD+
+ Kp_main: [1, 1, 1] # Proportional gains for the PD+
# Kd_main: [0., 0., 0.] # Derivative gains for the PD+
- Kd_main: [0.3, 0.3, 0.3] # Derivative gains for the PD+
+ Kd_main: [1, 1, 1] # Derivative gains for the PD+
# Kff_main: 0.0 # Feedforward torques multiplier for the PD+
Kff_main: 1.0 # Feedforward torques multiplier for the PD+
diff --git a/python/quadruped_reactive_walking/Controller.py b/python/quadruped_reactive_walking/Controller.py
index b9966a78f6dddbbb52d32a36fe41791084bacbd0..a1a3451167c85b39ad86c4c5f5fefcc59714439a 100644
--- a/python/quadruped_reactive_walking/Controller.py
+++ b/python/quadruped_reactive_walking/Controller.py
@@ -93,16 +93,25 @@ class Controller:
if not self.error:
self.mpc_result, self.mpc_cost = self.mpc.get_latest_result()
- self.result.P = np.array(self.params.Kp_main.tolist() * 4)
- self.result.D = np.array(self.params.Kd_main.tolist() * 4)
+ #self.result.P = np.array(self.params.Kp_main.tolist() * 4)
+ self.result.P = np.array([5] * 3 + [5] * 3 + [5]*6)
+ #self.result.D = np.array(self.params.Kd_main.tolist() * 4)
+ self.result.D = np.array([0.3] * 3 + [0.1] * 3 + [0.3]*6)
self.result.FF = np.zeros(12)
# self.result.FF = self.params.Kff_main * np.ones(12)
+
+ # Keep only the actuated joints and set the other to default values
+ self.mpc_result.q = np.array([self.pd.q0] * (self.pd.T + 1))[:, 7: 19]
+ self.mpc_result.v = np.array([self.pd.v0] * (self.pd.T +1 ))[:, 6: ]
+ self.mpc_result.q[:, 3:6] = np.array(self.mpc_result.x)[:, : self.pd.nq]
+ self.mpc_result.v[:, 3:6] = np.array(self.mpc_result.x)[:, self.pd.nq :]
+
self.result.q_des = self.mpc_result.q[1]
self.result.v_des = self.mpc_result.v[1]
self.result.tau_ff = np.zeros(12)
- self.guess["xs"] = self.mpc_result.x
- self.guess["us"] = self.mpc_result.u
+ self.guess["xs"] = self.mpc_result.x[1:] + [self.mpc_result.x[-1]*0]
+ self.guess["us"] = self.mpc_result.u[1:] + [self.mpc_result.u[-1]*0]
self.t_wbc = time.time() - t_start
@@ -220,6 +229,7 @@ class Controller:
self.result.tau_ff[:] = np.zeros(12)
def read_state(self, device):
+ device.parse_sensor_data()
qj_m = device.joints.positions
vj_m = device.joints.velocities
bp_m = self.tuple_to_array(device.baseState)
@@ -231,6 +241,13 @@ class Controller:
return {'qj_m': qj_m, 'vj_m': vj_m, 'x_m': x_m}
+ def interpolate_traj(self, device, q_des, v_des, ratio):
+ measures = self.read_state(device)
+ qj_des_i = np.linspace(measures['qj_m'], q_des, ratio)
+ vj_des_i = np.linspace(measures['vj_m'], v_des, ratio)
+
+ return qj_des_i, vj_des_i
+
def tuple_to_array(self, tup):
a = np.array([element for tupl in tup for element in tupl])
return a
diff --git a/python/quadruped_reactive_walking/WB_MPC/CrocoddylOCP.py b/python/quadruped_reactive_walking/WB_MPC/CrocoddylOCP.py
index b4d98589367201b171f5db49109b912f0b9eab7a..41c4f2ea755f817f8bc5c6146e3ae6323df930fa 100644
--- a/python/quadruped_reactive_walking/WB_MPC/CrocoddylOCP.py
+++ b/python/quadruped_reactive_walking/WB_MPC/CrocoddylOCP.py
@@ -145,7 +145,7 @@ class OCP:
us = guess['us']
print("Using warmstart")
start_time = time()
- self.ddp.solve(xs, us, 30, False)
+ self.ddp.solve(xs, us, 1, False)
print("Solver time: ", time()- start_time, "\n")
@@ -154,17 +154,6 @@ class OCP:
self.results.a = self.get_croco_acc()
self.results.u = self.ddp.us.tolist()
self.results.K = self.ddp.K
-
- if self.pd.useFixedBase == 0:
- self.results.q = np.array(self.results.x)[:, 7: self.pd.nq]
- self.results.v = np.array(self.results.x)[:, self.pd.nq + 6: ]
- else:
- self.results.q = self.pd.q0
- self.results.v = self.pd.q0
- self.results.q[3:6] = np.array(self.results.x)[:, : self.pd.nq]
- self.results.v[3:6] = np.array(self.results.x)[:, self.pd.nq :]
-
-
return self.results
def get_croco_forces(self):
diff --git a/python/quadruped_reactive_walking/WB_MPC/ProblemData.py b/python/quadruped_reactive_walking/WB_MPC/ProblemData.py
index 1e47aab4c0ec4a0b21de75b63e3d6923822ebd79..30d62f4eacaf7ac913be6ec83d93fd9e991e71cf 100644
--- a/python/quadruped_reactive_walking/WB_MPC/ProblemData.py
+++ b/python/quadruped_reactive_walking/WB_MPC/ProblemData.py
@@ -87,9 +87,12 @@ class ProblemData(problemDataAbstract):
self.terminal_velocity_w = np.array([0] * 18 + [1e3] * 18 )
self.control_bound_w = 1e3
- self.x0 = np.array([ 0.0, 0.0, 0.2607495, 0, 0, 0, 1, 0.1, 0.8, -1.6,
- -0.1, 0.8, -1.6, 0.1, -0.8, 1.6, -0.1, -0.8, 1.6,
- 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]) # x0 got from PyBullet
+ self.x0 = np.array([ 0.0, 0.0, 0.2607495, 0, 0, 0, 1,
+ 0, 0.7, -1.4,
+ 0. , 0.7, -1.4,
+ 0. , -0.7, 1.4,
+ 0. , -0.7, 1.4,
+ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]) # x0 got from PyBullet
self.u0 = np.array([-0.02615051, -0.25848605, 0.51696646,
0.0285894 , -0.25720605, 0.51441775,
@@ -120,8 +123,8 @@ class ProblemDataFull(problemDataAbstract):
self.q0_reduced = self.q0[10:13]
self.v0_reduced = np.zeros(self.nq)
- self.x0 = np.concatenate([self.q0_reduced, self.v0_reduced])
+ self.x0_reduced = np.concatenate([self.q0_reduced, self.v0_reduced])
- self.xref = self.x0
+ self.xref = self.x0_reduced
self.uref = self.u0
\ No newline at end of file
diff --git a/python/quadruped_reactive_walking/WB_MPC/Target.py b/python/quadruped_reactive_walking/WB_MPC/Target.py
index 4452df207a10ecec50ea46f6f343b2d7334a5bcd..fdc0825be3a9297a5ad22534e6ef20e401ec6a81 100644
--- a/python/quadruped_reactive_walking/WB_MPC/Target.py
+++ b/python/quadruped_reactive_walking/WB_MPC/Target.py
@@ -22,14 +22,14 @@ class Target:
self.contactSequence = [ self.patternToId(p) for p in self.gait]
self.target = {pd.rfFootId: []}
- q = pd.x0[: pd.nq]
- v = pd.x0[pd.nq :]
+ q = pd.q0_reduced
+ v = pd.v0_reduced
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.08, 0., 0.06])
- self.freq = np.array([0, 2.5, 2.5])
+ self.freq = np.array([0, 2.5 *0, 2.5*0])
self.phase = np.array([0, 0, np.pi/2])
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 23b6accba828c6198b3bba32373e917a4c7c2a76..5e25b6b2f12a0f76160c0c8d97169c5daaa19abd 100644
--- a/python/quadruped_reactive_walking/main_solo12_control.py
+++ b/python/quadruped_reactive_walking/main_solo12_control.py
@@ -170,7 +170,6 @@ def control_loop():
while (not device.is_timeout) and (t < t_max) and (not controller.error):
t_start_whole = time.time()
- device.parse_sensor_data()
target.update(cnt)
if controller.compute(device, qc):
break
@@ -179,20 +178,20 @@ def control_loop():
break
# Set desired quantities for the actuators
- device.joints.set_position_gains(controller.result.P)
- device.joints.set_velocity_gains(controller.result.D)
- device.joints.set_desired_positions(controller.result.q_des)
- device.joints.set_desired_velocities(controller.result.v_des)
- device.joints.set_torques(
- controller.result.FF * controller.result.tau_ff.ravel()
- )
+ q_des, v_des = controller.interpolate_traj(device, controller.result.q_des, controller.result.v_des, pd.r1)
+ for k in range(controller.pd.r1):
+ device.joints.set_position_gains(controller.result.P)
+ device.joints.set_velocity_gains(controller.result.D)
+ device.joints.set_desired_positions(q_des[k])
+ device.joints.set_desired_velocities(v_des[k])
+ device.send_command_and_wait_end_of_cycle(params.dt_wbc)
if params.LOGGING or params.PLOTTING:
loggerControl.sample(device, qc, controller)
t_end_whole = time.time()
- device.send_command_and_wait_end_of_cycle(params.dt_wbc)
+
t += params.dt_wbc
dT_whole = T_whole
diff --git a/python/quadruped_reactive_walking/tools/PyBulletSimulator.py b/python/quadruped_reactive_walking/tools/PyBulletSimulator.py
index c72e242516cd66c9ebc1ab896f07ccea7893f101..07f59c9c4179422f80ac321d353cabcfd5252027 100644
--- a/python/quadruped_reactive_walking/tools/PyBulletSimulator.py
+++ b/python/quadruped_reactive_walking/tools/PyBulletSimulator.py
@@ -42,7 +42,7 @@ class pybullet_simulator:
# Either use a flat ground or a rough terrain
if use_flat_plane:
- self.planeId = pyb.loadURDF("plane.urdf") # Flat plane
+ """ self.planeId = pyb.loadURDF("plane.urdf") # Flat plane
self.planeIdbis = pyb.loadURDF("plane.urdf") # Flat plane
# Tune position and orientation of plane
@@ -55,7 +55,7 @@ class pybullet_simulator:
self.planeIdbis,
[200.0, 0, -100.0 * np.sin(p_pitch)],
pin.Quaternion(pin.rpy.rpyToMatrix(p_roll, p_pitch, 0.0)).coeffs(),
- )
+ ) """
else:
import random
@@ -269,12 +269,12 @@ class pybullet_simulator:
pyb.setGravity(0, 0, -9.81)
# Load Quadruped robot
- robotStartPos = [0, 0, 0.0]
+ robotStartPos = [0, 0, 0.2607495]
robotStartOrientation = pyb.getQuaternionFromEuler([0.0, 0.0, 0.0])
pyb.setAdditionalSearchPath(
EXAMPLE_ROBOT_DATA_MODEL_DIR + "/solo_description/robots"
)
- self.robotId = pyb.loadURDF("solo12.urdf", robotStartPos, robotStartOrientation)
+ self.robotId = pyb.loadURDF("solo12.urdf", robotStartPos, robotStartOrientation, useFixedBase = 1)
# Disable default motor control for revolute joints
self.revoluteJointIndices = [0, 1, 2, 4, 5, 6, 8, 9, 10, 12, 13, 14]
@@ -301,42 +301,6 @@ class pybullet_simulator:
forces=jointTorques,
)
- # 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(
- 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(),
- )
# Fix the base in the world frame
# pyb.createConstraint(self.robotId, -1, -1, -1, pyb.JOINT_FIXED, [0, 0, 0], [0, 0, 0], [0, 0, robotStartPos[2]])