add far grasps

source/move.py

@@ -74,6 +74,9 @@ class ArmGraspController:
     def distance_from_shoulder(self, pos):
         return self.distance(self.vec_diff(pos, self.get_shoulder_pos()))
 
+    def distance_from_shoulder_start(self, pos):
+        return self.distance(self.vec_diff(pos, self.start_pos_shoulder))
+
     def move_wrist(self, new_pos: Tuple[float]):
         """Moves wrist to new position.
 
@@ -106,6 +109,23 @@ class ArmGraspController:
 
             self.move_wrist(new_pos)
 
+    # Moves the arm, with the shoulder always moving back to its original position.
+    def move_arm_rel(self, new_pos: Tuple[float]):
+        if(self.distance_from_shoulder_start(new_pos) <= self.arm_range):
+            # Do standard move
+            self.move_wrist(new_pos)
+            # Move shoulder back to start location
+            self.move_shoulder(self.start_pos_shoulder)
+        else:
+            # Combine both shoulder and arm move
+            shoulder_pos = self.start_pos_shoulder
+            diff = self.vec_diff(new_pos, shoulder_pos)
+            diff_2 = self.vec_diff(diff, [i*self.arm_range for i in self.normalize_vec(diff)])
+            move = self.vec_add(shoulder_pos, diff_2)
+            self.move_shoulder(move)
+
+            self.move_wrist(new_pos)
+
     def move_back(self):
         self.move_shoulder(self.start_pos_shoulder)
         self.move_wrist(self.start_pos_wrist)
@@ -175,5 +195,5 @@ z_rand = RandomBounds(-3, 2)
 
 for i in range(10):
     bpy.context.scene.frame_set(frame_number)
-    controller.move_arm([x_rand.random(), y_rand.random(), z_rand.random()])
+    controller.move_arm_rel([x_rand.random(), y_rand.random(), z_rand.random()])
     frame_number += 20