Add new Result structure to store control quantities that are sent to the control board

Controller.py

@@ -10,6 +10,17 @@ import MPC_Wrapper
 import pybullet as pyb
 
 
+class Result:
+
+    def __init__(self):
+
+        self.P = 0.0
+        self.D = 0.0
+        self.q_des = np.zeros(12)
+        self.v_des = np.zeros(12)
+        self.tau_ff = np.zeros(12)
+
+
 class dummyDevice:
 
     def __init__(self):
@@ -124,11 +135,7 @@ class Controller:
         self.vmes12 = np.zeros((18, 1))
 
         # Interface with the PD+ on the control board
-        self.result.P = 0.0
-        self.result.D = 0.0
-        self.result.q_des = np.zeros(12)
-        self.result.v_des = np.zeros(12)
-        self.result.tau_ff = np.zeros(12)
+        self.result = Result()
 
         # Run the control loop once with a dummy device for initialization
         dDevice = dummyDevice()
@@ -198,8 +205,8 @@ class Controller:
                 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])
             self.result.q_des[:] = self.myController.qdes[7:]
-            self.result.v_des[:] = self.myController.vdes[6:]
-            self.result.tau_ff[:] = self.myController.tau_ff
+            self.result.v_des[:] = self.myController.vdes[6:, 0]
+            # self.result.tau_ff[:] = self.myController.tau_ff
 
             # Process PD+ (feedforward torques and feedback torques)
             self.jointTorques[:, 0] = proc.process_pdp(self.myController, self.estimator)
@@ -214,15 +221,6 @@ class Controller:
             self.result.v_des[:] = np.zeros(12)
             self.result.tau_ff[:] = np.zeros(12)
 
-            # D controller to slow down the legs
-            D = 0.05
-            self.jointTorques[:, 0] = D * (- self.estimator.v_filt[6:, 0])
-
-            # Saturation to limit the maximal torque
-            t_max = 1.0
-            self.jointTorques[self.jointTorques > t_max] = t_max
-            self.jointTorques[self.jointTorques < -t_max] = -t_max
-
         # t_tsid = time.time()  # To analyze the time taken by each step
 
         # Compute processing duration of each step
@@ -268,7 +266,12 @@ class Controller:
 
             tau = self.compute(device)
 
-            device.SetDesiredJointTorque(tau)
+            # device.SetDesiredJointTorque(tau)
+            device.SetKp(self.result.P)
+            device.SetKd(self.result.D)
+            device.SetQdes(self.result.q_des)
+            device.SetVdes(self.result.v_des)
+            device.SetTauFF(self.result.tau_ff)
 
             device.SendCommand(WaitEndOfCycle=True)
 

TSID_Debug_controller_four_legs_fb_vel.py

@@ -694,54 +694,43 @@ class controller:
     def run_PDplus(self):
 
         # Check for NaN value in the output torques (means error during solving process)
-        """if np.any(np.isnan(self.tau_ff)):
-            self.tau_ff[np.isnan(self.tau_ff)] = 0.0"""
-
         if np.any(np.isnan(self.tau_ff)):
             self.error = True
             print('NaN value in feedforward torque. Switching to safety controller.')
-            return np.zeros((12, ))
         elif np.any(np.abs(self.qdes[7:]) > np.tile(np.array([1.0, 2.0, 3.0]), 4)) \
                 or np.any(np.abs(self.qdes[9::3]) < 0.1):
-            print('Abnormal angular values. Switching to safety controller.')
             self.error = True
-            return np.zeros((12, ))
+            print('Abnormal angular values. Switching to safety controller.')
         else:
             # Torque PD controller
             if self.enable_hybrid_control:
-                self.tau_pd = self.P * (self.qdes[7:] - self.qmes[7:, 0]) + \
-                    self.D * (self.vdes[6:, 0] - self.vmes[6:, 0])
-                self.torques12 = self.tau_pd  # + self.tau_ff
+
+                # Saturation to limit the maximal torque
+                t_max = 2.5
+                self.tau_ff[self.tau_ff > t_max] = t_max
+                self.tau_ff[self.tau_ff < -t_max] = -t_max
+
+                # If several torques are at saturation, switch to safety control
+                cpt = 0
+                for i in (np.abs(self.tau_ff[:]) > (t_max-0.02)):
+                    if i:
+                        cpt += 1
+                if cpt >= 4:
+                    self.error = True
+                    print('Several torques at saturation. Switching to safety controller.')
+
+                # If one of the torques is at saturation during several time steps, switch to safety controller
+                self.reached_tau_max += 1
+                self.reached_tau_max *= (np.abs(self.tau_ff[:]) > (t_max-0.02))
+                if np.any(self.reached_tau_max >= 50):
+                    self.error = True
+                    print('A torque reached saturation during several time steps. Switching to safety controller.')
+
             else:
-                # self.torques12 = self.tau_ff
                 print("Control with enable_hybrid_control disabled for security. Lots of things have changed \n",
                       "and we need to be sure nothing is broken.")
-                return np.zeros((12, ))
-
-            # Saturation to limit the maximal torque
-            t_max = 2.5
-            self.torques12[self.torques12 > t_max] = t_max
-            self.torques12[self.torques12 < -t_max] = -t_max
-
-            # If several torques are at saturation, switch to safety control
-            cpt = 0
-            for i in (np.abs(self.torques12[:]) > (t_max-0.02)):
-                if i:
-                    cpt += 1
-            if cpt >= 4:
-                self.error = True
-                print('Several torques at saturation. Switching to safety controller.')
-                return np.zeros((12, ))
-
-            # If one of the torques is at saturation during several time steps, switch to safety controller
-            self.reached_tau_max += 1
-            self.reached_tau_max *= (np.abs(self.torques12[:]) > (t_max-0.02))
-            if np.any(self.reached_tau_max >= 50):
-                self.error = True
-                print('A torque reached saturation during several time steps. Switching to safety controller.')
-                return np.zeros((12, ))
-
-            return self.torques12
+
+            return np.zeros((12, ))
 
     def display(self, t, solo, k_simu, sequencer):
         """ To display debug spheres in Gepetto Viewer

utils_mpc.py

@@ -715,6 +715,13 @@ class PyBulletSimulator():
         self.baseLinearAcceleration = np.zeros(3)
         self.prev_b_baseVel = np.zeros(3)
 
+        # PD+ quantities
+        self.P = 0.0
+        self.D = 0.0
+        self.q_des = np.zeros(12)
+        self.v_des = np.zeros(12)
+        self.tau_ff = np.zeros(12)
+
     def Init(self, calibrateEncoders=False, q_init=None, envID=0, use_flat_plane=True, enable_pyb_GUI=False, dt=0.002):
 
         # Initialisation of the PyBullet simulator
@@ -765,8 +772,29 @@ class PyBulletSimulator():
 
         return
 
+    def SetKp(self, P):
+        self.P = P
+
+    def SetKd(self, D):
+        self.D = D
+
+    def SetQdes(self, q_des):
+        self.q_des = q_des
+
+    def SetVdes(self, v_des):
+        self.v_des = v_des
+
+    def SetTauFF(self, tau_ff):
+        self.tau_ff = tau_ff
+
     def SendCommand(self, WaitEndOfCycle=True):
 
+        # Compute PD torques
+        tau_pd = self.P * (self.q_des - self.q_mes) + self.D * (self.v_des - self.v_mes)
+
+        # Save desired torques in a storage array
+        self.jointTorques = tau_pd + self.tau_ff
+
         # Set control torque for all joints
         pyb.setJointMotorControlArray(self.pyb_sim.robotId, self.pyb_sim.revoluteJointIndices,
                                       controlMode=pyb.TORQUE_CONTROL, forces=self.jointTorques)