diff --git a/Controller.py b/Controller.py
index 14e50aa54dfc6072ca1666e8000784067e3c87ad..f4c89fd5d7b44ccb60f8288fc395fa70d3522f89 100644
--- a/Controller.py
+++ b/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)
diff --git a/TSID_Debug_controller_four_legs_fb_vel.py b/TSID_Debug_controller_four_legs_fb_vel.py
index cf074356cfba5c2a442b2cb6b119fa7f95689d33..a0a35f08020560f93cc6a0caf72ec85fdf446577 100644
--- a/TSID_Debug_controller_four_legs_fb_vel.py
+++ b/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
diff --git a/utils_mpc.py b/utils_mpc.py
index cd59ac6800eac6991c171b1dea1df4a9ca25bd34..ccac06e05866aa4c4f771b4ddfeb21c040f114e0 100644
--- a/utils_mpc.py
+++ b/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)