finish wrist angle control

armlab-regular/kinematics.py

@@ -99,7 +99,7 @@ def check_valid(pose):
 
     return True
 
-def IK(pose, mode):
+def IK(pose, mode, wrist):
     """
     467TODO: implement this function
 
@@ -149,16 +149,18 @@ def IK(pose, mode):
         print(pose, " is out of forearm's reach")
         return None
 
+    wrist_1 = -wrist + theta0_1 + theta1_1
+    wrist_2 = -wrist + theta0_2 + theta1_2    
     if mode == 1:
         if x >= 0.0:
-            return [-math.pi / 2, theta0_1, theta1_1, 0]
+            return [-math.pi / 2, theta0_1, theta1_1, wrist_1]
         else:
-            return [-math.pi / 2, theta0_2, theta1_2, 0]
+            return [-math.pi / 2, theta0_2, theta1_2, wrist_2]
     else:
         if x >= 0.0:
-            return [-math.pi / 2, theta0_2, theta1_2, 0]
+            return [-math.pi / 2, theta0_2, theta1_2, wrist_2]
         else:
-            return [-math.pi / 2, theta0_1, theta1_1, 0]
+            return [-math.pi / 2, theta0_1, theta1_1, wrist_1]
 
 def get_euler_angles_from_T(T):
     """

armlab-regular/state_machine.py

@@ -143,9 +143,9 @@ def move_positions(self):
             target = self.togo.pop(0)
             wrist_angle = self.wrist_angles[id]
             id += 1
-            angles = IK((target[0], target[1], 0), target[2])
+            # wrist_angle is the angle in the frame of arm
+            angles = IK((target[0], target[1], 0), target[2], wrist_angle)
             if angles:
-                angles[len(angles) - 1] = wrist_angle
                 self.rexarm.move_to_target_angles(angles, self.speed_norm, False)
         self.next_state = "idle"
         self.togo_lock.release()