diff --git a/source/move.py b/source/move.py index b19bdc0..5f709b1 100644 --- a/source/move.py +++ b/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