circular motion linear interpolation (no match derivatives)

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: 5000  # Number of simulated wbc time steps
+    N_SIMULATION: 10000  # 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
@@ -27,7 +27,7 @@ robot:
 #     Kp_main: [0.0, 0.0, 0.0]  # Proportional gains for the PD+
     Kp_main: [3,3,3]  # Proportional gains for the PD+
 #     Kd_main: [0., 0., 0.]  # Derivative gains for the PD+
-    Kd_main: [0.1, 0.1, 0.1]  # Derivative gains for the PD+
+    Kd_main: [0.3, 0.3, 0.3]  # 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

@@ -47,23 +47,30 @@ class Interpolation:
         # q_t = gamma + beta * t + alpha * t**2
 
         # Linear
+        beta = self.v1
+        gamma = self.q0
+
+        v_t = beta
+        q_t = gamma + beta * t
+
+        # Linear Wrong
         # beta = self.v1
         # gamma = self.q0
 
-        # v_t = beta
-        # q_t = gamma + beta * t
+        # v_t = self.v0 + self.v1*(self.v1 - self.v0)/(self.q1 - self.q0) * t
+        # q_t = self.q0 + self.v1 * t
 
         # Quadratic
-        if (self.q1-self.q0 == 0).any():
-            alpha = np.zeros(len(self.q0))
-        else:
-            alpha = self.v1*(self.v1 - self.v0)/(self.q1 - self.q0)
+        # if (self.q1-self.q0 == 0).any():
+        #     alpha = np.zeros(len(self.q0))
+        # else:
+        #     alpha = self.v1*(self.v1 - self.v0)/(self.q1 - self.q0)
 
-        beta = self.v0
-        gamma = self.q0
+        # beta = self.v0
+        # gamma = self.q0
 
-        v_t = beta + alpha * t
-        q_t = gamma + beta * t + 1/2 * alpha * t**2
+        # v_t = beta + alpha * t
+        # q_t = gamma + beta * t + 1/2 * alpha * t**2
 
         return q_t, v_t
 
@@ -225,7 +232,7 @@ class Controller:
             actuated_tau_ff = self.mpc_result.us[0] + np.dot(self.mpc_result.K[0],
                                                              np.concatenate([pin.difference(self.pd.model, 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:]]))
+                                                                             m["x_m"][self.pd.nq:] - self.mpc_result.xs[0][self.pd.nq:]]))
             self.result.tau_ff = np.array(
                 [0] * 3 + list(actuated_tau_ff) + [0] * 6)
 

python/quadruped_reactive_walking/WB_MPC/ProblemData.py

@@ -114,11 +114,11 @@ class ProblemDataFull(problemDataAbstract):
         # Cost function weights
         # Cost function weights
         self.mu = 0.7
-        self.foot_tracking_w = 1e4
+        self.foot_tracking_w = 1e3
         #self.friction_cone_w = 1e3 * 0
         self.control_bound_w = 1e3
         self.control_reg_w = 1e0
-        self.state_reg_w = np.array([1e0] * 3 + [1e0]*3)
+        self.state_reg_w = np.array([1e-2] * 3 + [1e0]*3)
         self.terminal_velocity_w = np.array([0] * 3 + [1e3] * 3 )
 
         self.q0_reduced = self.q0[10 : 13]