Commit controller for same reason

scripts/Controller.py

@@ -255,7 +255,7 @@ class Controller:
                                                                 int(self.k_mpc - self.k % self.k_mpc),
                                                                 self.q[:, 0],
                                                                 self.h_v_windowed[0:6, 0:1].copy(),
-                                                                self.v_ref[0:6, 0])
+                                                                self.v_ref[0:6, 0:1])
 
         # Run state planner (outputs the reference trajectory of the base)
         self.statePlanner.computeReferenceStates(self.q_filt_mpc[0:6, 0:1], self.h_v_filt_mpc[0:6, 0:1].copy(),
@@ -311,10 +311,12 @@ class Controller:
         if (not self.error) and (not self.joystick.stop):
 
             # Update configuration vector for wbc
-            self.q_wbc[2, 0] = self.h_ref  # Height
+            """self.q_wbc[2, 0] = self.h_ref  # Height
             self.q_wbc[3, 0] = self.q_filt_mpc[3, 0]  # Roll
             self.q_wbc[4, 0] = self.q_filt_mpc[4, 0]  # Pitch
-            self.q_wbc[6:, 0] = self.wbcWrapper.qdes[:]  # with reference angular positions of previous loop
+            self.q_wbc[6:, 0] = self.wbcWrapper.qdes[:]  # with reference angular positions of previous loop"""
+            self.q_wbc[:, 0] = self.q_filt_mpc[:, 0].copy()
+            self.q_wbc[2, 0] = self.h_ref  # Height
 
             # Update velocity vector for wbc
             self.dq_wbc[:6, 0] = self.estimator.getVFilt()[:6]  #  Velocities in base frame (not horizontal frame!)
@@ -327,8 +329,9 @@ class Controller:
                 oRh.transpose(), np.zeros((3, 1)), np.zeros((3, 1)))
             self.feet_p_cmd = self.footTrajectoryGenerator.getFootPositionBaseFrame(oRh.transpose(), oTh)
 
-            # Desired position, orientation and velocities of the base
+            # Desired position, orientation and velocities of the base
             if not self.gait.getIsStatic():
+                self.xgoals[[0, 1, 5], 0] = self.q_filt_mpc[[0, 1, 5], 0]
                 self.xgoals[2:5, 0] = [self.h_ref, 0.0, 0.0]  #  Height (in horizontal frame!)
             else:
                 self.xgoals[2:5, 0] += self.vref_filt_mpc[2:5, 0] * self.dt_wbc
@@ -338,9 +341,9 @@ class Controller:
             # Run InvKin + WBC QP
             self.wbcWrapper.compute(self.q_wbc, self.dq_wbc,
                                     (self.x_f_mpc[12:24, 0:1]).copy(), np.array([cgait[0, :]]),
-                                    self.feet_p_cmd,
-                                    self.feet_v_cmd,
-                                    self.feet_a_cmd,
+                                    self.footTrajectoryGenerator.getFootPosition(),
+                                    self.footTrajectoryGenerator.getFootVelocity(),
+                                    self.footTrajectoryGenerator.getFootAcceleration(),
                                     self.xgoals)
 
             # Quantities sent to the control board