diff --git a/main.py b/main.py
index ebfc03841d7cb233e3fb31b096d5a4d589d5a66e..8e44ad6c1500bd8055abd5dcc7c2de23b387c14e 100644
--- a/main.py
+++ b/main.py
@@ -20,6 +20,7 @@ class SimulatorLoop(Loop):
"""
Class used to call pybullet at a given frequency
"""
+
def __init__(self, period, t_max, envID, velID, dt_mpc, k_mpc, t, n_periods, T_gait, N_SIMULATION, type_MPC, pyb_feedback):
"""
Constructor
@@ -41,14 +42,14 @@ class SimulatorLoop(Loop):
self.ID_deb_lines = []
# Enable/Disable Gepetto viewer
- self.enable_gepetto_viewer = True
+ self.enable_gepetto_viewer = False
# Which MPC solver you want to use
# True to have PA's MPC, to False to have Thomas's MPC
"""type_MPC = True"""
# Create Joystick, FootstepPlanner, Logger and Interface objects
- self.joystick, self.fstep_planner, self.logger, self.interface = utils.init_objects(
+ self.joystick, self.fstep_planner, self.logger, self.interface, self.estimator = utils.init_objects(
dt, dt_mpc, N_SIMULATION, k_mpc, n_periods, T_gait, type_MPC)
# Wrapper that makes the link with the solver that you want to use for the MPC
@@ -135,7 +136,7 @@ class SimulatorLoop(Loop):
# If nothing wrong happened yet in TSID controller
if not self.myController.error:
self.jointTorques = proc.process_invdyn(self.solo, self.k, self.f_applied, self.pyb_sim, self.interface,
- self.fstep_planner, self.myController, self.enable_hybrid_control)
+ self.fstep_planner, self.myController, self.enable_hybrid_control)
# t_list_tsid[k] = time.time() - time_tsid # Logging the time spent to run this iteration of inverse dynamics
# Process PD+ (feedforward torques and feedback torques)
@@ -219,6 +220,7 @@ def run_scenario(envID, velID, dt_mpc, k_mpc, t, n_periods, T_gait, N_SIMULATION
time_error = False
# Lists to log the duration of 1 iteration of the MPC/TSID
+ t_list_filter = [0] * int(N_SIMULATION)
t_list_states = [0] * int(N_SIMULATION)
t_list_fsteps = [0] * int(N_SIMULATION)
t_list_mpc = [0] * int(N_SIMULATION)
@@ -239,7 +241,7 @@ def run_scenario(envID, velID, dt_mpc, k_mpc, t, n_periods, T_gait, N_SIMULATION
"""type_MPC = True"""
# Create Joystick, FootstepPlanner, Logger and Interface objects
- joystick, fstep_planner, logger, interface = utils.init_objects(
+ joystick, fstep_planner, logger, interface, estimator = utils.init_objects(
dt, dt_mpc, N_SIMULATION, k_mpc, n_periods, T_gait, type_MPC, on_solo8,
predefined_vel)
@@ -283,10 +285,19 @@ def run_scenario(envID, velID, dt_mpc, k_mpc, t, n_periods, T_gait, N_SIMULATION
"""if (k % 1000) == 0:
print("Iteration: ", k)"""
- #print("###")
+ # print("###")
+
+ # Process estimator
+ if k == 1:
+ estimator.run_filter(k, fstep_planner.gait[0, 1:], pyb_sim.robotId,
+ myController.invdyn.data(), myController.model)
+ else:
+ estimator.run_filter(k, fstep_planner.gait[0, 1:], pyb_sim.robotId)
+
+ t_filter = time.time()
# Process states update and joystick
- proc.process_states(solo, k, k_mpc, velID, pyb_sim, interface, joystick, myController, pyb_feedback)
+ proc.process_states(solo, k, k_mpc, velID, pyb_sim, interface, joystick, myController, estimator, pyb_feedback)
t_states = time.time()
@@ -315,7 +326,7 @@ def run_scenario(envID, velID, dt_mpc, k_mpc, t, n_periods, T_gait, N_SIMULATION
t_mpc = time.time()
- #print(f_applied.ravel())
+ # print(f_applied.ravel())
# Process Inverse Dynamics
# time_tsid = time.time()
@@ -339,22 +350,27 @@ def run_scenario(envID, velID, dt_mpc, k_mpc, t, n_periods, T_gait, N_SIMULATION
jointTorques[jointTorques > t_max] = t_max
jointTorques[jointTorques < -t_max] = -t_max
- if np.max(np.abs(pyb_sim.vmes12[6:, 0])) < 0.1:
+ """if np.max(np.abs(pyb_sim.vmes12[6:, 0])) < 0.1:
print("Trigger get back to default pos")
# pyb_sim.get_to_default_position(pyb_sim.straight_standing)
pyb_sim.get_to_default_position(np.array([[0.0, np.pi/2, np.pi,
0.0, np.pi/2, np.pi,
0.0, -np.pi/2, np.pi,
- 0.0, -np.pi/2, np.pi]]).transpose())
+ 0.0, -np.pi/2, np.pi]]).transpose())"""
+ """pyb_sim.get_to_default_position(np.array([[0, 0.8, -1.6,
+ 0, 0.8, -1.6,
+ 0, -0.8, 1.6,
+ 0, -0.8, 1.6]]).transpose())"""
- #print(jointTorques.ravel())
+ # print(jointTorques.ravel())
"""if myController.error:
pyb.disconnect()
quit()"""
t_tsid = time.time()
- t_list_states[k] = t_states - time_loop
+ t_list_filter[k] = t_filter - time_loop
+ t_list_states[k] = t_states - t_filter
t_list_fsteps[k] = t_fsteps - t_states
t_list_mpc[k] = t_mpc - t_fsteps
t_list_tsid[k] = t_tsid - t_mpc
@@ -364,9 +380,12 @@ def run_scenario(envID, velID, dt_mpc, k_mpc, t, n_periods, T_gait, N_SIMULATION
proc.process_pybullet(pyb_sim, k, envID, jointTorques)
# Call logger object to log various parameters
- logger.call_log_functions(k, pyb_sim, joystick, fstep_planner, interface, mpc_wrapper, myController,
- False, pyb_sim.robotId, pyb_sim.planeId, solo)
-
+ # logger.call_log_functions(k, pyb_sim, joystick, fstep_planner, interface, mpc_wrapper, myController,
+ # False, pyb_sim.robotId, pyb_sim.planeId, solo)
+ # logger.log_state(k, pyb_sim, joystick, interface, mpc_wrapper, solo)
+ # logger.log_footsteps(k, interface, myController)
+ # logger.log_fstep_planner(k, fstep_planner)
+ # logger.log_tracking_foot(k, myController, solo)
"""if k >= 1000:
time.sleep(0.1)"""
@@ -382,9 +401,15 @@ def run_scenario(envID, velID, dt_mpc, k_mpc, t, n_periods, T_gait, N_SIMULATION
print("Computation duration: ", tac-tic)
print("Simulated duration: ", N_SIMULATION*dt)
print("Max loop time: ", np.max(t_list_loop[10:]))
+
+ if enable_multiprocessing:
+ print("Stopping parallel process")
+ mpc_wrapper.stop_parallel_loop()
+
print("END")
plt.figure()
+ plt.plot(t_list_filter[1:], '+', color="orange")
plt.plot(t_list_states[1:], 'r+')
plt.plot(t_list_fsteps[1:], 'g+')
plt.plot(t_list_mpc[1:], 'b+')
@@ -395,6 +420,37 @@ def run_scenario(envID, velID, dt_mpc, k_mpc, t, n_periods, T_gait, N_SIMULATION
pyb.disconnect()
+ """NN = myController.log_v_est.shape[2]
+ avg = np.zeros((3, NN))
+ for m in range(NN):
+ tmp_cpt = 0
+ tmp_sum = np.zeros((3, 1))
+ for j in range(4):
+ if np.any(np.abs(myController.log_v_est[:, j, m]) > 1e-2):
+ tmp_cpt += 1
+ tmp_sum[:, 0] = tmp_sum[:, 0] + myController.log_v_est[:, j, m].ravel()
+ if tmp_cpt > 0:
+ avg[:, m:(m+1)] = tmp_sum / tmp_cpt
+
+ plt.figure()
+ for i in range(3):
+ plt.subplot(3, 1, i+1)
+ for j in range(4):
+ plt.plot(myController.log_v_est[i, j, :], linewidth=3)
+ # plt.plot(-myController.log_Fv1F[i, j, :], linewidth=3, linestyle="--")
+ plt.plot(myController.log_v_truth[i, :], "k", linewidth=3, linestyle="--")
+ plt.plot(avg[i, :], color="rebeccapurple", linewidth=3, linestyle="--")
+ plt.legend(["FL", "FR", "HL", "HR", "Truth"])
+ plt.suptitle("Estimation of the velocity of the base")
+
+ plt.figure()
+ for i in range(3):
+ plt.subplot(3, 1, i+1)
+ for j in range(4):
+ plt.plot(logger.feet_vel[i, j, :], linewidth=3)
+ plt.suptitle("Velocity of feet over time")
+ plt.show(block=True)"""
+
return logger
diff --git a/processing.py b/processing.py
index f602bc36cd9f1b0c828c86aec3bbd318298fd02c..63c0abca503fd576bf0af2e4165804e0a8a6d586 100644
--- a/processing.py
+++ b/processing.py
@@ -9,7 +9,7 @@ from matplotlib import pyplot as plt
from utils import getQuaternion
-def process_states(solo, k, k_mpc, velID, pyb_sim, interface, joystick, tsid_controller, pyb_feedback):
+def process_states(solo, k, k_mpc, velID, pyb_sim, interface, joystick, tsid_controller, estimator, pyb_feedback):
"""Update states by retrieving information from the simulation and the gamepad
Args:
@@ -21,6 +21,7 @@ def process_states(solo, k, k_mpc, velID, pyb_sim, interface, joystick, tsid_con
interface (object): Interface object of the control loop
joystick (object): Interface with the gamepad
tsid_controller (object): Inverse dynamics controller
+ estimator (object): Estimator object which estimates the state of the robot
pyb_feedback (bool): Whether PyBullet feedback is enabled or not
"""
@@ -28,6 +29,9 @@ def process_states(solo, k, k_mpc, velID, pyb_sim, interface, joystick, tsid_con
# Retrieve data from the simulation (position/orientation/velocity of the robot)
# Stored in pyb_sim.qmes12 and pyb_sim.vmes12 (quantities in PyBullet world frame)
pyb_sim.retrieve_pyb_data()
+ pyb_sim.qmes12[3:7, 0] = estimator.filt_ang_pos
+ pyb_sim.vmes12[0:3, 0] = estimator.filt_lin_vel
+ pyb_sim.vmes12[3:6, 0] = estimator.filt_ang_vel
# Retrieve state desired by TSID (position/orientation/velocity of the robot)
tsid_controller.qtsid[:, 0] = tsid_controller.qdes.copy() # in TSID world frame
@@ -113,6 +117,24 @@ def process_footsteps_planner(k, k_mpc, pyb_sim, interface, joystick, fstep_plan
fstep_planner.update_fsteps(k, k_mpc, interface.l_feet, np.vstack((interface.lV, interface.lW)),
joystick.v_ref, interface.lC[2], interface.oMl, pyb_sim.ftps_Ids, False)
+ """import pybullet as pyb
+ for j in range(4):
+ pos_tmp = np.array([interface.o_feet[:, j]]).transpose()
+ pyb.resetBasePositionAndOrientation(pyb_sim.ftps_Ids[j, 0],
+ posObj=pos_tmp,
+ ornObj=np.array([0.0, 0.0, 0.0, 1.0]))"""
+
+ """for i in range(4):
+ a = interface.o_feet[:, i:(i+1)]
+ b = interface.l_feet[:, i:(i+1)]
+ c = interface.oMl.inverse() * interface.o_feet[:, i:(i+1)]
+ d = interface.oMl * interface.l_feet[:, i:(i+1)]
+ print("### ", i, " ###")
+ print("o_feet: ", a.ravel())
+ print("o_feet: ", b.ravel())
+ print("oMl-1 * o_feet: ", c.ravel())
+ print("oMl * l_feet: ", d.ravel())"""
+
# Update footsteps desired location once every k_mpc iterations of TSID
if True: # (k % k_mpc) == 0:
diff --git a/utils.py b/utils.py
index 7e454a7669cc52d81f03274a107bea2694dd53f3..df3938d2d695657e5c9097d140fcc3949cede63b 100644
--- a/utils.py
+++ b/utils.py
@@ -7,6 +7,7 @@ import Joystick
import FootstepPlanner
import Logger
import Interface
+import Estimator
import pybullet as pyb # Pybullet server
import pybullet_data
@@ -167,7 +168,10 @@ def init_objects(dt_tsid, dt_mpc, k_max_loop, k_mpc, n_periods, T_gait, type_MPC
# Create Interface object
interface = Interface.Interface()
- return joystick, fstep_planner, logger, interface
+ #Â Create Estimator object
+ estimator = Estimator.Estimator(dt_tsid)
+
+ return joystick, fstep_planner, logger, interface, estimator
def display_all(solo, k, sequencer, fstep_planner, ftraj_gen, mpc):
@@ -207,7 +211,7 @@ class pybullet_simulator:
def __init__(self, envID, use_flat_plane, dt=0.001):
# Start the client for PyBullet
- physicsClient = pyb.connect(pyb.GUI)
+ physicsClient = pyb.connect(pyb.DIRECT)
# p.GUI for graphical version
# p.DIRECT for non-graphical version
@@ -218,20 +222,23 @@ class pybullet_simulator:
self.planeId = pyb.loadURDF("plane.urdf") # Flat plane
else:
import random
- random.seed(42)
+ random.seed(41)
# p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,0)
heightPerturbationRange = 0.05
numHeightfieldRows = 256*2
numHeightfieldColumns = 256*2
heightfieldData = [0]*numHeightfieldRows*numHeightfieldColumns
+ height_prev = 0.0
for j in range(int(numHeightfieldColumns/2)):
for i in range(int(numHeightfieldRows/2)):
height = random.uniform(0, heightPerturbationRange) # uniform distribution
- heightfieldData[2*i+2*j*numHeightfieldRows] = height
+ # height = 0.25*np.sin(2*np.pi*(i-128)/46)
+ heightfieldData[2*i+2*j*numHeightfieldRows] = (height + height_prev) * 0.5
heightfieldData[2*i+1+2*j*numHeightfieldRows] = height
- heightfieldData[2*i+(2*j+1)*numHeightfieldRows] = height
+ heightfieldData[2*i+(2*j+1)*numHeightfieldRows] = (height + height_prev) * 0.5
heightfieldData[2*i+1+(2*j+1)*numHeightfieldRows] = height
+ height_prev = height
terrainShape = pyb.createCollisionShape(shapeType=pyb.GEOM_HEIGHTFIELD, meshScale=[.05, .05, 1],
heightfieldTextureScaling=(numHeightfieldRows-1)/2,
@@ -242,14 +249,19 @@ class pybullet_simulator:
pyb.resetBasePositionAndOrientation(self.planeId, [0, 0, 0], [0, 0, 0, 1])
pyb.changeVisualShape(self.planeId, -1, rgbaColor=[1, 1, 1, 1])
- # Add stairs with platform and bridge
- # self.stairsId = pyb.loadURDF("../../../../../Documents/Git-Repositories/mpc-tsid/bauzil_stairs.urdf")#,
- # basePosition=[-1.25, 3.5, -0.1],
- # baseOrientation=pyb.getQuaternionFromEuler([0.0, 0.0, 3.1415]))
- # pyb.changeDynamics(self.stairsId, -1, lateralFriction=1.0)
+ """self.stairsId = pyb.loadURDF("../../../../../Documents/Git-Repositories/mpc-tsid/bauzil_stairs.urdf",
+ basePosition=[0.0, 0.0, -0.3],
+ baseOrientation=pyb.getQuaternionFromEuler([0.0, 0.0, 0.0]))
+ pyb.changeDynamics(self.stairsId, -1, lateralFriction=1.0)"""
if envID == 1:
+ # Add stairs with platform and bridge
+ """self.stairsId = pyb.loadURDF("../../../../../Documents/Git-Repositories/mpc-tsid/bauzil_stairs.urdf") # ,
+ basePosition = [-1.25, 3.5, -0.1],
+ baseOrientation = pyb.getQuaternionFromEuler([0.0, 0.0, 3.1415])
+ pyb.changeDynamics(self.stairsId, -1, lateralFriction=1.0)"""
+
mesh_scale = [1.0, 0.1, 0.02]
visualShapeId = pyb.createVisualShape(shapeType=pyb.GEOM_MESH,
fileName="cube.obj",
@@ -311,7 +323,7 @@ class pybullet_simulator:
useMaximalCoordinates=True)
pyb.changeDynamics(tmpId, -1, lateralFriction=1.0)
- mesh_scale = [0.05, 0.05, 0.05]
+ mesh_scale = [0.1, 0.1, 0.1]
visualShapeId = pyb.createVisualShape(shapeType=pyb.GEOM_MESH,
fileName="sphere_smooth.obj",
halfExtents=[0.5, 0.5, 0.1],
@@ -329,14 +341,14 @@ class pybullet_simulator:
baseInertialFramePosition=[0, 0, 0],
baseCollisionShapeIndex=collisionShapeId,
baseVisualShapeIndex=visualShapeId,
- basePosition=[-0.6, 0.9, 0.05],
+ basePosition=[-0.6, 0.9, 0.1],
useMaximalCoordinates=True)
self.sphereId2 = pyb.createMultiBody(baseMass=0.3,
baseInertialFramePosition=[0, 0, 0],
baseCollisionShapeIndex=collisionShapeId,
baseVisualShapeIndex=visualShapeId,
- basePosition=[0.6, 1.1, 0.05],
+ basePosition=[0.6, 1.1, 0.1],
useMaximalCoordinates=True)
# Flag to launch the two spheres in the environment toward the robot
@@ -430,16 +442,16 @@ class pybullet_simulator:
if envID == 1:
# Check if the robot is in front of the first sphere to trigger it
if self.flag_sphere1 and (qmes12[1, 0] >= 0.9):
- pyb.resetBaseVelocity(self.sphereId1, linearVelocity=[3.0, 0.0, 2.0])
+ pyb.resetBaseVelocity(self.sphereId1, linearVelocity=[2.5, 0.0, 2.0])
self.flag_sphere1 = False
# Check if the robot is in front of the second sphere to trigger it
if self.flag_sphere2 and (qmes12[1, 0] >= 1.1):
- pyb.resetBaseVelocity(self.sphereId2, linearVelocity=[-3.0, 0.0, 2.0])
+ pyb.resetBaseVelocity(self.sphereId2, linearVelocity=[-2.5, 0.0, 2.0])
self.flag_sphere2 = False
# Apply perturbation
- self.apply_external_force(k, 1000, 400, np.array([0.0, 6.0, 0.0]), np.zeros((3,)))
+ # self.apply_external_force(k, 1000, 400, np.array([0.0, 6.0, 0.0]), np.zeros((3,)))
"""self.apply_external_force(k, 4000, 400, np.array([0.0, 2.0, 0.0]), np.zeros((3,)))
self.apply_external_force(k, 8000, 400, np.array([0.0, -2.0, 0.0]), np.zeros((3,)))
self.apply_external_force(k, 12000, 400, np.array([2.0, 0.0, 0.0]), np.zeros((3,)))
@@ -454,7 +466,7 @@ class pybullet_simulator:
# Update the PyBullet camera on the robot position to do as if it was attached to the robot
pyb.resetDebugVisualizerCamera(cameraDistance=0.6, cameraYaw=(0.0*RPY[2]*(180/3.1415)+45), cameraPitch=-39.9,
- cameraTargetPosition=[qmes12[0, 0], qmes12[1, 0] + 0.0, 0.0])
+ cameraTargetPosition=[qmes12[0, 0], qmes12[1, 0] + 0.0, 0.0]) # qmes12[2, 0]-0.2])
return 0
@@ -519,8 +531,8 @@ class pybullet_simulator:
cpt = 0
# PD settings
- P = 0.33 * 3.0
- D = 0.33 * np.array([1.0, 0.3, 0.3, 1.0, 0.3, 0.3, 1.0, 0.3, 0.3, 1.0, 0.3, 0.3])
+ P = 1.0 * 3.0
+ D = 0.05 * np.array([[1.0, 0.3, 0.3, 1.0, 0.3, 0.3, 1.0, 0.3, 0.3, 1.0, 0.3, 0.3]]).transpose()
while True or np.max(np.abs(qtarget - qmes)) > 0.1:
@@ -536,8 +548,9 @@ class pybullet_simulator:
ev = dt_traj * cpt
A3 = 2 * (qmes - qtarget) / t1**3
A2 = (-3/2) * t1 * A3
- qdes = qmes + A2*ev**2 + A3*ev**3
- jointTorques = P * (qdes - qmes) # + D * (self.vdes[6:, 0] - self.vmes[6:, 0])
+ qdes = qmes + A2*(ev**2) + A3*(ev**3)
+ vdes = 2*A2*ev + 3*A3*(ev**2)
+ jointTorques = P * (qdes - qmes) + D * (vdes - vmes)
# Saturation to limit the maximal torque
t_max = 2.5