adding load conversion function. Also added back the Brutal trajectory

bam/feetech/record.py

@@ -55,7 +55,7 @@
         # motor.disable_torque()
     # control.set_kps(ids, [32])
     io.set_P_coefficient({1: args.kp})
-    io.set_D_coefficient({1:0})
+    io.set_D_coefficient({1: 0})
 
     # motor.kp = args.kp
 
@@ -72,6 +72,14 @@
 }
 
 
+def convert_load(raw_load):
+    sign = -1
+    if raw_load > 1023:
+        raw_load -= 1024
+        sign = 1
+    return sign * raw_load * 0.001
+
+
 def read_data():
 
     # position = control.read_present_position(ids)[0]
@@ -83,8 +91,7 @@ def read_data():
     # speed = control.read_present_velocity(ids)[0]
     speed = np.deg2rad(io.get_present_speed([1])[0])
 
-
-    load = 0  # TMP
+    load = convert_load(io.get_present_load([1])[0])
 
     volts = io.get_present_voltage([1])[0] * 0.1
     # volts = 0

bam/trajectory.py

@@ -78,6 +78,17 @@ def __call__(self, t: float):
         return angle, True
 
 
+class Brutal(Trajectory):
+    duration = 6.0
+
+    def __call__(self, t: float):
+
+        if t > self.duration / 4 and t < self.duration / 1.5:
+            return np.pi / 2, True
+        else:
+            return 0.0, True
+
+
 class Nothing(Trajectory):
     duration = 6.0
 
@@ -90,6 +101,7 @@ def __call__(self, t: float):
     "sin_time_square": SinusTimeSquare(),
     "up_and_down": UpAndDown(),
     "sin_sin": SinSin(),
+    "brutal": Brutal(),
     "nothing": Nothing(),
 }