Feature/guarded motion added distance until edge up

robot_skills/examples/example_move_down_until_force_sensor_edge_up.py

@@ -1,10 +1,10 @@
 #!/usr/bin/env python
 import rospy
 
-from robot_skills.util.robot_constructor import robot_constructor
+from robot_skills.get_robot import get_robot
 
 rospy.init_node('test_mode_down_until_force_sensor_edge_up', anonymous=True)
-robot = robot_constructor("hero")
+robot = get_robot("hero")
 arm = robot.get_arm(force_sensor_required=True)
 
 arm.move_down_until_force_sensor_edge_up()

robot_skills/src/robot_skills/arm/arms.py

@@ -174,10 +174,11 @@ def has_force_sensor(self):
                        "Specify get_arm(..., force_sensor_required=True)")
         return hasattr(self._arm, "force_sensor")
 
-    def move_down_until_force_sensor_edge_up(self, timeout=10, retract_distance=0.01):
+    def move_down_until_force_sensor_edge_up(self, timeout=10, retract_distance=0.01, distance_move_down=2.0):
         self._test_die(self.has_force_sensor, 'has_force_sensor=' + str(self.has_force_sensor),
                        "Specify get_arm(..., force_sensor_required=True)")
-        return self._arm.move_down_until_force_sensor_edge_up(timeout=timeout, retract_distance=retract_distance)
+        return self._arm.move_down_until_force_sensor_edge_up(timeout=timeout, retract_distance=retract_distance,
+                                                              distance_move_down=distance_move_down)
 
     # salvaged deprecated functionality
     def send_gripper_goal(self, state, timeout=5.0, gripper_type=None, max_torque=0.1):

robot_skills/src/robot_skills/functionalities/arm/guarded_motion.py

@@ -11,55 +11,81 @@ class GuardedMotionFunc(RobotFunc):
     def __init__(self):
         super(GuardedMotionFunc, self).__init__("guarded_motion",
                                                 Arm,
-                                                {"move_down_until_force_sensor_edge_up": move_down_until_force_sensor_edge_up,
-                                                 "_create_lower_force_sensing_goal": _create_lower_force_sensing_goal,
-                                                 "_create_retract_force_sensing_goal": _create_retract_force_sensing_goal})
+                                                {
+                                                    "move_down_until_force_sensor_edge_up": move_down_until_force_sensor_edge_up,
+                                                    "_create_lower_force_sensing_goal": _create_lower_force_sensing_goal,
+                                                    "_create_retract_force_sensing_goal": _create_retract_force_sensing_goal
+                                                })
 
     def check_requirements(self, arm):
-        "Check that the arm has at least one force sensor"
+        """
+        Check that the arm has at least one force sensor
+        """
         for part in arm.parts:
             if isinstance(arm.parts[part], ForceSensor):
                 return True
         return False
 
 
-def move_down_until_force_sensor_edge_up(self, force_sensor=None, timeout=10, retract_distance=0.01):
+def move_down_until_force_sensor_edge_up(self, force_sensor=None, timeout=10, retract_distance=0.01,
+                                         distance_move_down=None):
     """
-    Move down the arm (hero specific, only joint arm_lift_joint) until the force sensor detects an edge up
+    Move down the arm (hero specific, only joint arm_lift_joint) until one of 3 things:
+        - Force sensor detects an edge up
+        - Timeout
+        - Force sensor has moved down the distance specified
 
     A 'force_sensor.TimeOutException' will be raised if no edge up is detected within timeout
 
+    :param force_sensor: ForceSensor of the arm
     :param timeout: Max duration for edge up detection
     :param retract_distance: How much to retract if we have reached a surface
+    :param distance_move_down: How much to move down until we reach a surface
     """
-    if not force_sensor:
+    if force_sensor is None:
         force_sensor = self.parts["force_sensor"]
 
     # Fill with required joint names (desired in hardware / gazebo impl)
-    goal = self._create_lower_force_sensing_goal(timeout)
+    goal = self._create_lower_force_sensing_goal(distance_move_down, timeout)
     self._ac_joint_traj.send_goal(goal)
 
     force_sensor.wait_for_edge_up(timeout)
     self.cancel_goals()
 
-    goal = self._create_retract_force_sensing_goal(0.5, timeout)
+    goal = self._create_retract_force_sensing_goal(retract_distance, timeout)
     self._ac_joint_traj.send_goal(goal)
 
 
-def _create_lower_force_sensing_goal(self, timeout):
+def _create_lower_force_sensing_goal(self, distance_move_down, timeout):
+    # Gets current joint state
     current_joint_state = self.get_joint_states()
-    current_joint_state['arm_lift_joint'] = 0   # TODO make this function not HERO-specific
+
+    # Sets the joint state to move down a certain distance (or 0 if distance is not set)
+    if distance_move_down is not None:
+        current_joint_state['arm_lift_joint'] = max(0, current_joint_state['arm_lift_joint'] - distance_move_down)
+    else:
+        current_joint_state['arm_lift_joint'] = 0  # TODO make this function not HERO-specific
+
+    # Creates goal for force_sensor
     return create_force_sensing_goal(self.joint_names, current_joint_state, timeout)
 
 
 def _create_retract_force_sensing_goal(self, retract_distance, timeout):
+    # Gets current joint states
     current_joint_state = self.get_joint_states()
-    current_joint_state['arm_lift_joint'] += retract_distance # TODO make this function not HERO-specific
+
+    # Changes state to retract arm after force sensor edge up
+    current_joint_state['arm_lift_joint'] += retract_distance  # TODO make this function not HERO-specific
+
+    # Creates joint states goal
     return create_force_sensing_goal(self.joint_names, current_joint_state, timeout)
 
 
 def create_force_sensing_goal(joint_names, current_joint_state, timeout):
-    positions = [current_joint_state[n] for n in joint_names]
+    positions = []
+    for name in joint_names:
+        positions.append(current_joint_state[name])
+
     points = [JointTrajectoryPoint(
         positions=positions,
         time_from_start=rospy.Duration.from_sec(timeout))