From ee95a1c65a796d60a32b98f8cd166ae9b082864f Mon Sep 17 00:00:00 2001 From: P-ict0 Date: Tue, 21 Jun 2022 21:50:00 +0200 Subject: [PATCH 1/6] fixed robot construction --- .../examples/example_move_down_until_force_sensor_edge_up.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) 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() From 6736e955429b669c7c50215be5967964c782371d Mon Sep 17 00:00:00 2001 From: P-ict0 Date: Tue, 21 Jun 2022 21:50:30 +0200 Subject: [PATCH 2/6] Added parameter distance_move_down and added comments --- .../functionalities/arm/guarded_motion.py | 52 ++++++++++++++----- 1 file changed, 39 insertions(+), 13 deletions(-) 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..5dc4d0631f 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,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)) From d399630482a8de70165509f67aacb701c5fd835a Mon Sep 17 00:00:00 2001 From: P-ict0 Date: Tue, 21 Jun 2022 21:51:59 +0200 Subject: [PATCH 3/6] added distance_move_down parameter --- robot_skills/src/robot_skills/arm/arms.py | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/robot_skills/src/robot_skills/arm/arms.py b/robot_skills/src/robot_skills/arm/arms.py index b2411d9a92..b042472476 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=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): From b3e92aec54fe405a13fceba8ed1e8949a52b2a0f Mon Sep 17 00:00:00 2001 From: P-ict0 Date: Tue, 21 Jun 2022 22:07:05 +0200 Subject: [PATCH 4/6] Quick fix to not alter old behaviour --- robot_skills/src/robot_skills/arm/arms.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/robot_skills/src/robot_skills/arm/arms.py b/robot_skills/src/robot_skills/arm/arms.py index b042472476..1fc81c63df 100644 --- a/robot_skills/src/robot_skills/arm/arms.py +++ b/robot_skills/src/robot_skills/arm/arms.py @@ -174,7 +174,7 @@ 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, distance_move_down=2.0): + 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, From f9cb9537f3d472e797055eb80bf172555020e9d4 Mon Sep 17 00:00:00 2001 From: P-ict0 Date: Wed, 22 Jun 2022 09:32:19 +0200 Subject: [PATCH 5/6] Small fixes and improved code --- .../functionalities/arm/guarded_motion.py | 14 +++++--------- 1 file changed, 5 insertions(+), 9 deletions(-) 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 5dc4d0631f..24452d6669 100644 --- a/robot_skills/src/robot_skills/functionalities/arm/guarded_motion.py +++ b/robot_skills/src/robot_skills/functionalities/arm/guarded_motion.py @@ -57,21 +57,19 @@ def move_down_until_force_sensor_edge_up(self, force_sensor=None, timeout=10, re def _create_lower_force_sensing_goal(self, distance_move_down, timeout): - # Gets current joint state current_joint_state = self.get_joint_states() - # 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: + # 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): - # Gets current joint states current_joint_state = self.get_joint_states() # Changes state to retract arm after force sensor edge up @@ -82,9 +80,7 @@ def _create_retract_force_sensing_goal(self, retract_distance, timeout): def create_force_sensing_goal(joint_names, current_joint_state, timeout): - positions = [] - for name in joint_names: - positions.append(current_joint_state[name]) + positions = [current_joint_state[name] for name in joint_names] points = [JointTrajectoryPoint( positions=positions, From 1faf1152c99e937821afa151cea4cb3677d624a8 Mon Sep 17 00:00:00 2001 From: Rodrigo Date: Wed, 29 Jun 2022 13:54:12 +0200 Subject: [PATCH 6/6] Adjusted docstring to better explain functionality --- .../src/robot_skills/functionalities/arm/guarded_motion.py | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) 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 24452d6669..17a3b4da29 100644 --- a/robot_skills/src/robot_skills/functionalities/arm/guarded_motion.py +++ b/robot_skills/src/robot_skills/functionalities/arm/guarded_motion.py @@ -30,17 +30,16 @@ def check_requirements(self, arm): 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 one of 3 things: + Move down the arm (hero specific, only joint arm_lift_joint) until one of 2 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 + :param distance_move_down: Maximum distance to move down the arm_lift_joint """ if force_sensor is None: force_sensor = self.parts["force_sensor"]