working on rising foot

config/walk_parameters.yaml

@@ -7,7 +7,7 @@ robot:
     PLOTTING: true  # Enable/disable automatic plotting at the end of the experiment
     DEMONSTRATION: false  # Enable/disable demonstration functionalities
     SIMULATION: true  # Enable/disable PyBullet simulation or running on real robot
-    enable_pyb_GUI: false  # Enable/disable PyBullet GUI
+    enable_pyb_GUI: true  # Enable/disable PyBullet GUI
     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)

python/quadruped_reactive_walking/Controller.py

@@ -46,7 +46,8 @@ class Interpolator:
             self.beta = self.v1
             self.gamma = self.q0
         elif self.type == 1:  # Quadratic fixed velocity
-            self.alpha = 2 * (self.q1 - self.q0 - self.v0 * self.dt) / self.dt**2
+            self.alpha = 2 * (self.q1 - self.q0 -
+                              self.v0 * self.dt) / self.dt**2
             self.beta = self.v0
             self.gamma = self.q0
         elif self.type == 2:  # Quadratic time variable
@@ -102,7 +103,8 @@ class Interpolator:
             plt.scatter(y=self.q0[i], x=0.0, color="violet", marker="+")
             plt.scatter(y=self.q1[i], x=self.dt, color="violet", marker="+")
             if self.type == 3 and self.q2 is not None:
-                plt.scatter(y=self.q2[i], x=2 * self.dt, color="violet", marker="+")
+                plt.scatter(y=self.q2[i], x=2 * self.dt,
+                            color="violet", marker="+")
 
             plt.subplot(3, 2, (i * 2) + 2)
             plt.title("Velocity interpolation")
@@ -110,7 +112,8 @@ class Interpolator:
             plt.scatter(y=self.v0[i], x=0.0, color="violet", marker="+")
             plt.scatter(y=self.v1[i], x=self.dt, color="violet", marker="+")
             if self.type == 3 and self.v2 is not None:
-                plt.scatter(y=self.v2[i], x=2 * self.dt, color="violet", marker="+")
+                plt.scatter(y=self.v2[i], x=2 * self.dt,
+                            color="violet", marker="+")
 
         plt.show()
 
@@ -122,7 +125,7 @@ class DummyDevice:
         self.base_position = np.zeros(3)
         self.base_position[2] = 0.1944
         self.b_base_velocity = np.zeros(3)
-        self.baseState =  ((0.0, 0.0, 0.2607495), (0.0, 0.0, 0.0, 1.0)) 
+        self.baseState = ((0.0, 0.0, 0.2607495), (0.0, 0.0, 0.0, 1.0))
         self.baseVel = ((0.0, 0.0, 0.0), (0.0, 0.0, 0.0))
 
     class IMU:
@@ -138,7 +141,6 @@ class DummyDevice:
             self.velocities = np.zeros(12)
 
 
-
 class Controller:
     def __init__(self, pd, params, q_init, t):
         """
@@ -153,7 +155,8 @@ class Controller:
 
         self.pd = pd
         self.params = params
-        self.gait = np.repeat(np.array([0, 0, 0, 0]).reshape((1, 4)), self.pd.T, axis=0)
+        self.gait = np.concatenate([np.repeat(np.array([1, 1, 1, 1]).reshape((1, 4)), self.pd.init_steps, axis=0),
+                                    np.repeat(np.array([1, 0, 1, 1]).reshape((1, 4)), self.pd.target_steps - 1, axis=0)])
         self.q_init = pd.q0
 
         self.k = 0
@@ -261,8 +264,8 @@ class Controller:
             #     q, v = self.interpolator.interpolate(t)
             # else:
             #     q, v = self.integrate_x(m)
-            q = xs[1][: self.pd.nq]
-            v = xs[1][self.pd.nq :]
+            q = xs[1][7: self.pd.nq]
+            v = xs[1][6 + self.pd.nq:]
 
             self.result.q_des = q[:]
             self.result.v_des = v[:]
@@ -426,9 +429,10 @@ class Controller:
                 pin.difference(
                     self.pd.model,
                     m["x_m"][: self.pd.nq],
-                    self.mpc_result.xs[0][: self.pd.nq],
+                    self.mpc_result.xs[0][: self.pd.nq]
+
                 ),
-                m["x_m"][self.pd.nq :] - self.mpc_result.xs[0][self.pd.nq :],
+                m["x_m"][self.pd.nq:] - self.mpc_result.xs[0][self.pd.nq:]
             ]
         )
         # x_diff = self.mpc_result.xs[0] - m["x_m"]
@@ -459,13 +463,16 @@ class Controller:
 
         legend = ["Hip", "Shoulder", "Knee"]
         fig, axs = plt.subplots(3)
-        [axs[0].plot(np.array(self.mpc_result.xs)[:, joint]) for joint in range(3)]
+        [axs[0].plot(np.array(self.mpc_result.xs)[:, joint])
+         for joint in range(3)]
         axs[0].legend(legend)
 
-        [axs[1].plot(np.array(self.mpc_result.xs)[:, 3 + joint]) for joint in range(3)]
+        [axs[1].plot(np.array(self.mpc_result.xs)[:, 3 + joint])
+         for joint in range(3)]
         axs[1].legend(legend)
 
-        [axs[2].plot(np.array(self.mpc_result.us)[:, joint]) for joint in range(3)]
+        [axs[2].plot(np.array(self.mpc_result.us)[:, joint])
+         for joint in range(3)]
         axs[2].legend(legend)
 
         return axs

python/quadruped_reactive_walking/WB_MPC/ProblemData.py

@@ -8,8 +8,8 @@ class problemDataAbstract:
         self.dt = param.dt_mpc  # OCP dt
         self.dt_wbc = param.dt_wbc
         self.mpc_wbc_ratio = int(self.dt / self.dt_wbc)
-        self.init_steps = 0
-        self.target_steps = 150
+        self.init_steps = 10
+        self.target_steps = 90
         self.T = self.init_steps + self.target_steps - 1
 
         self.robot = erd.load("solo12")
@@ -89,25 +89,25 @@ class ProblemData(problemDataAbstract):
         # Cost function weights
         # Cost function weights
         self.mu = 0.7
-        self.foot_tracking_w = 1e3
-        # self.friction_cone_w = 1e3 * 0
+        self.foot_tracking_w = 1e2
+        self.friction_cone_w = 1e3 
         self.control_bound_w = 1e3
-        self.control_reg_w = 1e0
+        self.control_reg_w = 1e1
         self.state_reg_w = np.array([0] * 3
                                     + [1e1] * 3
                                     + [1e0] * 3
-                                    + [1e-5] * 3
+                                    + [1e-3] * 3
                                     + [1e0] * 6
                                     + [0] * 6
                                     + [1e1] * 3
-                                    + [1e0] * 3
+                                    + [1e-1] * 3
                                     + [1e1] * 6
                                     )
         self.terminal_velocity_w = np.array(
-            [0] * self.nq + [1e3] * self.nv)
+            [0] * self.nv + [1e3] * self.nv)
 
         self.xref = self.x0
-        self.uref = self.u0
+        self.uref =self.u0
 
 
 class ProblemDataFull(problemDataAbstract):

python/quadruped_reactive_walking/WB_MPC/Target.py

@@ -9,10 +9,10 @@ class Target:
         pin.forwardKinematics(pd.model, pd.rdata, pd.q0, pd.v0)
         pin.updateFramePlacements(pd.model, pd.rdata)
         self.foot_pose = pd.rdata.oMf[pd.rfFootId].translation.copy()
-        self.A = np.array([0, 0.03, 0.03])
-        self.offset = np.array([0.05, -0.02, 0.06])
-        self.freq = np.array([0, 0.5, 0.5])
-        self.phase = np.array([0, np.pi / 2, 0])
+        self.A = np.array([0, 0.05, 0.05])
+        self.offset = np.array([0.05, 0, 0.06])
+        self.freq = np.array([0, 0.5 * 0, 0.5*0])
+        self.phase = np.array([0, 0, np.pi / 2])
         self.t_offset = 0
 
     def shift(self):

python/quadruped_reactive_walking/tools/PyBulletSimulator.py

@@ -42,7 +42,8 @@ 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 +56,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 +270,12 @@ class pybullet_simulator:
         pyb.setGravity(0, 0, -9.81)
 
         # Load Quadruped robot
-        robotStartPos = [0, 0, 0.2607495]
+        robotStartPos = [0, 0, 0]
         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, useFixedBase = 1)
+        self.robotId = pyb.loadURDF("solo12.urdf", robotStartPos, robotStartOrientation)
 
         # Disable default motor control for revolute joints
         self.revoluteJointIndices = [0, 1, 2, 4, 5, 6, 8, 9, 10, 12, 13, 14]
@@ -301,6 +302,42 @@ 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]])