diff --git a/robot_skills/examples/example_move_down_until_force_sensor_edge_up.py b/robot_skills/examples/example_move_down_until_force_sensor_edge_up.py index ac2cd1f5d4..befc12a2db 100755 --- a/robot_skills/examples/example_move_down_until_force_sensor_edge_up.py +++ b/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() diff --git a/robot_skills/src/robot_skills/arm/arms.py b/robot_skills/src/robot_skills/arm/arms.py index b2411d9a92..1fc81c63df 100644 --- a/robot_skills/src/robot_skills/arm/arms.py +++ b/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=None): 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): diff --git a/robot_skills/src/robot_skills/functionalities/arm/guarded_motion.py b/robot_skills/src/robot_skills/functionalities/arm/guarded_motion.py index e59e13d0cb..17a3b4da29 100644 --- a/robot_skills/src/robot_skills/functionalities/arm/guarded_motion.py +++ b/robot_skills/src/robot_skills/functionalities/arm/guarded_motion.py @@ -11,55 +11,76 @@ 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 2 things: + - Force sensor detects an edge up + - Timeout 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: Maximum distance to move down the arm_lift_joint """ - 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): 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 to 0 if distance is not set) + if distance_move_down is None: + current_joint_state['arm_lift_joint'] = 0 # TODO make this function not HERO-specific + else: + current_joint_state['arm_lift_joint'] = max(0, current_joint_state['arm_lift_joint'] - distance_move_down) + + # 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): 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 = [current_joint_state[name] for name in joint_names] + points = [JointTrajectoryPoint( positions=positions, time_from_start=rospy.Duration.from_sec(timeout))