fixed explosion bug

Need to preallocate memory

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+
 

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

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):

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

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):

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

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]])