experiment preparations (number 3)

experiments/design_C.1/donothing.py

@@ -34,5 +34,5 @@
     motor_ids=[3, 1],
     tau_limit=torque_limit,
     save_dir=os.path.join("data", design, "double-pendulum/donothing"),
-    record_video=False,
+    record_video=True,
 )

experiments/design_C.1/tvlqr_acrobot.py

@@ -109,9 +109,9 @@ def condition2(t, x):
 controller.init()
 
 # np.random.seed(2)
-# perturbation_array, _, _, _ = get_random_gauss_perturbation_array(
-#     t_final, dt, 3, 1.0, [0.05, 0.1], [0.5, 0.6]
-# )
+perturbation_array, _, _, _ = get_random_gauss_perturbation_array(
+    t_final, dt, 2, 1.0, [0.05, 0.1], [0.4, 0.6]
+)
 
 run_experiment(
     controller=controller,
@@ -122,6 +122,7 @@ def condition2(t, x):
     motor_directions=[1.0, -1.0],
     tau_limit=torque_limit,
     save_dir=os.path.join("data", design, robot, "tvlqr"),
-    record_video=False,
+    record_video=True,
+    safety_velocity_limit=30.0,
     # perturbation_array=perturbation_array,
 )

experiments/design_C.1/tvlqr_pendubot.py

@@ -31,7 +31,7 @@
 
 ## trajectory parameters
 csv_path = "../../data/trajectories/design_C.1/model_1.1/pendubot/ilqr_1/trajectory.csv"
-dt = 0.0025
+dt = 0.002
 t_final = 10.0
 
 # swingup parameters
@@ -46,13 +46,13 @@
 lqr_path = "../../data/controller_parameters/design_C.1/model_1.1/pendubot/lqr"
 lqr_pars = np.loadtxt(os.path.join(lqr_path, "controller_par.csv"))
 Q_lqr = np.diag(lqr_pars[:4])
-R_lqr = np.diag([lqr_pars[4], lqr_pars[4]])
+R_lqr = 8.0*np.diag([lqr_pars[4], lqr_pars[4]])
 
 S = np.loadtxt(os.path.join(lqr_path, "Smatrix"))
 rho = np.loadtxt(os.path.join(lqr_path, "rho"))
 
 Q = np.diag([1.0, 1.0, 1.0, 1.0])
-R = np.eye(2)
+R = np.eye(2)*2.0
 Qf = np.loadtxt(os.path.join(lqr_path, "Smatrix"))
 
 
@@ -106,9 +106,9 @@ def condition2(t, x):
 controller.init()
 
 # np.random.seed(2)
-# perturbation_array, _, _, _ = get_random_gauss_perturbation_array(
-#     t_final, dt, 3, 1.0, [0.05, 0.1], [0.5, 0.6]
-# )
+perturbation_array, _, _, _ = get_random_gauss_perturbation_array(
+    t_final, dt, 2, 1.0, [0.05, 0.1], [0.4, 0.6]
+)
 
 run_experiment(
     controller=controller,
@@ -119,6 +119,7 @@ def condition2(t, x):
     motor_directions=[1.0, -1.0],
     tau_limit=torque_limit,
     save_dir=os.path.join("data", design, robot, "tvlqr"),
-    record_video=False,
-    # perturbation_array=perturbation_array,
+    record_video=True,
+    safety_velocity_limit=30.0,
+    #perturbation_array=perturbation_array,
 )

src/python/double_pendulum/experiments/experimental_utils.py

@@ -113,7 +113,7 @@ def go_to_zero(motor1, motor2, motor_directions=[1.0, 1.0]):
     print("Moving back to initial configuration...")
     try:
         counter = 0
-        pos_epsilon = 0.02
+        pos_epsilon = 0.05
         vel_epsilon = 0.1
         pos1, vel1, tau1 = motor1.send_rad_command(0.0, 0.0, 0.0, 0.0, 0.0)
         pos2, vel2, tau2 = motor2.send_rad_command(0.0, 0.0, 0.0, 0.0, 0.0)
@@ -154,12 +154,12 @@ def go_to_zero(motor1, motor2, motor_directions=[1.0, 1.0]):
                 pos1,
                 vel1,
                 shoulder_tau,
-            ) = motor1.send_rad_command(next_pos1, 0.0, 5.0, 0.1, 0.0)
+            ) = motor1.send_rad_command(next_pos1, 0.0, 5.0, 1.0, 0.0)
             (
                 pos2,
                 vel2,
                 elbow_tau,
-            ) = motor2.send_rad_command(next_pos2, 0.0, 5.0, 0.1, 0.0)
+            ) = motor2.send_rad_command(next_pos2, 0.0, 5.0, 1.0, 0.0)
 
             # correction for motor axis directions
             pos1 *= motor_directions[0]

src/python/double_pendulum/experiments/hardware_control_loop_tmotors.py

@@ -86,15 +86,15 @@ def run_experiment(
     if perturbation_array is None:
         perturbation_array = np.zeros((2, int(t_final / dt)))
 
-    n = int(t_final / dt) + 2
+    n = int(t_final / dt)
 
-    meas_time = np.zeros(n + 1)
-    pos_meas1 = np.zeros(n + 1)
-    vel_meas1 = np.zeros(n + 1)
-    tau_meas1 = np.zeros(n + 1)
-    pos_meas2 = np.zeros(n + 1)
-    vel_meas2 = np.zeros(n + 1)
-    tau_meas2 = np.zeros(n + 1)
+    meas_time = np.zeros(n + 2)
+    pos_meas1 = np.zeros(n + 2)
+    vel_meas1 = np.zeros(n + 2)
+    tau_meas1 = np.zeros(n + 2)
+    pos_meas2 = np.zeros(n + 2)
+    vel_meas2 = np.zeros(n + 2)
+    tau_meas2 = np.zeros(n + 2)
 
     tau_cmd = np.zeros(2)
 
@@ -151,6 +151,8 @@ def run_experiment(
 
         index += 1
 
+        time.sleep(1.0)
+
         print("Starting Experiment...")
         try:
             while t < t_final:
@@ -166,11 +168,11 @@ def run_experiment(
                 tau_cmd[1] = np.clip(tau_con[1], -tau_limit[1], tau_limit[1])
 
                 # perturbations
-                tau_cmd[0] += perturbation_array[0, index]
-                tau_cmd[1] += perturbation_array[1, index]
+                tau_cmd[0] += perturbation_array[0, min(index, n-1)]
+                tau_cmd[1] += perturbation_array[1, min(index, n-1)]
 
                 # play terminal bell sound when perturbation is active
-                if np.max(np.abs(perturbation_array[:, index])) > 0.1:
+                if np.max(np.abs(perturbation_array[:, min(index, n-1)])) > 0.1:
                     print("\a", end="\r")
 
                 # correction for motor axis directions

src/python/double_pendulum/experiments/video_recording.py

@@ -18,8 +18,9 @@ def __init__(self, video_file_name, src=0):
         self.frame_height = int(self.capture.get(4))
 
         # Set up codec and output video settings
-        self.codec = cv2.VideoWriter_fourcc("M", "J", "P", "G")
-        self.fps = 30
+        # self.codec = cv2.VideoWriter_fourcc("M", "J", "P", "G")
+        self.codec = cv2.VideoWriter_fourcc("m", "p", "4", "v")
+        self.fps = 24
         self.output_video = cv2.VideoWriter(
             self.video_file_name,
             self.codec,