Integration of estimator in the control loop + Small changes for PyBullet...

Integration of estimator in the control loop + Small changes for PyBullet environement + Figures to test the estimator

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
 
 

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:
 

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