From 8deaceae08495719a632053991c90e485dd93b6a Mon Sep 17 00:00:00 2001 From: Janno Lunenburg Date: Sat, 7 Mar 2020 16:26:01 +0100 Subject: [PATCH 1/9] example: grasping height test --- .../manipulation/grasping_height_test.py | 77 +++++++++++++++++++ 1 file changed, 77 insertions(+) create mode 100644 robot_smach_states/examples/manipulation/grasping_height_test.py diff --git a/robot_smach_states/examples/manipulation/grasping_height_test.py b/robot_smach_states/examples/manipulation/grasping_height_test.py new file mode 100644 index 0000000000..40088a28a0 --- /dev/null +++ b/robot_smach_states/examples/manipulation/grasping_height_test.py @@ -0,0 +1,77 @@ +#! /usr/bin/env python + +# System +from collections import namedtuple + +# ROS +import PyKDL as kdl +import rospy + +# TU/e Robotics +from robot_smach_states.manipulation import Grab +from robot_smach_states.navigation import NavigateToSymbolic +from robot_smach_states.util.designators import EntityByIdDesignator, UnoccupiedArmDesignator + +# Robot Skills +from robot_skills import arms +from robot_skills.hero_parts.hero_arm import HeroArm +from robot_skills.get_robot import get_robot_from_argv +from robot_skills.util.kdl_conversions import FrameStamped + +DZ = 0.1 +Z_MAX = 1.2 + +X_ENTITY = 3.2 +Y_ENTITY = 1.55 + +Result = namedtuple("Result", ["height", "result", "duration"]) + +if __name__ == "__main__": + + rospy.init_node("test_grasping") + + robot = get_robot_from_argv(1) + + # Navigate to the couch table + nav_designator = EntityByIdDesignator(robot=robot, id="couch_table") + nav_state = NavigateToSymbolic(robot, {nav_designator: "in_front_of"}, nav_designator) + nav_state.execute() + + # Setup the test + z_values = [DZ] + while z_values[-1] < Z_MAX: + z_values.append(z_values[-1] + DZ) + + results = [] + for z in z_values: + + # Add an entity to ed + grasp_entity_id = "test_entity" + entity_pose = FrameStamped(kdl.Frame(kdl.Vector(X_ENTITY, Y_ENTITY, z)), "map") + robot.ed.update_entity(id=grasp_entity_id, frame_stamped=entity_pose) + + # Try to grasp the entity + item_designator = EntityByIdDesignator(robot=robot, id=grasp_entity_id) + t_start = rospy.Time.now() + grasp_state = Grab( + robot, + item_designator, + UnoccupiedArmDesignator(robot, { + "required_goals": ["reset", "carrying_pose"], + "required_trajectories": ["prepare_grasp"], + "required_gripper_types": [arms.GripperTypes.GRASPING] + })) + grasp_result = grasp_state.execute() + grasp_duration = (rospy.Time.now() - t_start).to_sec() + results.append(Result(z, grasp_result, grasp_duration)) + + # Lose the entity + # noinspection PyProtectedMember + arm = robot._arms.values()[0] # type: HeroArm + arm.send_gripper_goal(state=arms.GripperState.OPEN) + + # Print the results + print("\n ---------- \n") + for result in results: + print "Height: {} --> {} (duration: {})".format(result.height, result.result, result.duration) + print("\n ---------- \n") From 952fcfefe16a161304a0ae17377388f99a990e3d Mon Sep 17 00:00:00 2001 From: Janno Lunenburg Date: Sat, 7 Mar 2020 16:27:29 +0100 Subject: [PATCH 2/9] refactor(benchmarks)Moved all grasp benchmarks together in examples --- .../manipulation/grasp_benchmark}/analyse_grasping.py | 0 .../manipulation/grasp_benchmark}/benchmark_grasping.py | 0 .../manipulation/grasp_benchmark}/grasp_benchmark.csv | 0 .../manipulation/grasp_benchmark}/grasp_benchmark_config.csv | 0 .../manipulation/{ => grasp_benchmark}/grasping_height_test.py | 0 5 files changed, 0 insertions(+), 0 deletions(-) rename robot_smach_states/{benchmark => examples/manipulation/grasp_benchmark}/analyse_grasping.py (100%) rename robot_smach_states/{benchmark => examples/manipulation/grasp_benchmark}/benchmark_grasping.py (100%) rename robot_smach_states/{benchmark => examples/manipulation/grasp_benchmark}/grasp_benchmark.csv (100%) rename robot_smach_states/{benchmark => examples/manipulation/grasp_benchmark}/grasp_benchmark_config.csv (100%) rename robot_smach_states/examples/manipulation/{ => grasp_benchmark}/grasping_height_test.py (100%) diff --git a/robot_smach_states/benchmark/analyse_grasping.py b/robot_smach_states/examples/manipulation/grasp_benchmark/analyse_grasping.py similarity index 100% rename from robot_smach_states/benchmark/analyse_grasping.py rename to robot_smach_states/examples/manipulation/grasp_benchmark/analyse_grasping.py diff --git a/robot_smach_states/benchmark/benchmark_grasping.py b/robot_smach_states/examples/manipulation/grasp_benchmark/benchmark_grasping.py similarity index 100% rename from robot_smach_states/benchmark/benchmark_grasping.py rename to robot_smach_states/examples/manipulation/grasp_benchmark/benchmark_grasping.py diff --git a/robot_smach_states/benchmark/grasp_benchmark.csv b/robot_smach_states/examples/manipulation/grasp_benchmark/grasp_benchmark.csv similarity index 100% rename from robot_smach_states/benchmark/grasp_benchmark.csv rename to robot_smach_states/examples/manipulation/grasp_benchmark/grasp_benchmark.csv diff --git a/robot_smach_states/benchmark/grasp_benchmark_config.csv b/robot_smach_states/examples/manipulation/grasp_benchmark/grasp_benchmark_config.csv similarity index 100% rename from robot_smach_states/benchmark/grasp_benchmark_config.csv rename to robot_smach_states/examples/manipulation/grasp_benchmark/grasp_benchmark_config.csv diff --git a/robot_smach_states/examples/manipulation/grasping_height_test.py b/robot_smach_states/examples/manipulation/grasp_benchmark/grasping_height_test.py similarity index 100% rename from robot_smach_states/examples/manipulation/grasping_height_test.py rename to robot_smach_states/examples/manipulation/grasp_benchmark/grasping_height_test.py From 0645e51f7f36c4c86ae1fa6ab43aa852324fc133 Mon Sep 17 00:00:00 2001 From: Janno Lunenburg Date: Sat, 7 Mar 2020 16:29:04 +0100 Subject: [PATCH 3/9] fix(smach)Grasp point determination also works without convex hull In that case, it defaults to the current robot orienation --- .../manipulation/grasp_point_determination.py | 18 ++++++++++++++++++ 1 file changed, 18 insertions(+) diff --git a/robot_smach_states/src/robot_smach_states/manipulation/grasp_point_determination.py b/robot_smach_states/src/robot_smach_states/manipulation/grasp_point_determination.py index 31727fd81f..cde3827f3a 100644 --- a/robot_smach_states/src/robot_smach_states/manipulation/grasp_point_determination.py +++ b/robot_smach_states/src/robot_smach_states/manipulation/grasp_point_determination.py @@ -34,6 +34,24 @@ def get_grasp_pose(self, entity, arm): :param arm: arm to use :return: FrameStamped with grasp pose in map frame """ + try: + # Try to compute the grasp pose based on the entity + return self._get_grasp_pose(entity, arm) + # noinspection PyBroadException + except Exception as e: + rospy.logwarn("Cannot compute grasp pose, defaulting to entity position and " + "orientation the same as the robot") + robot_pose_map = self._robot.base.get_location() # type: FrameStamped + return FrameStamped(kdl.Frame(robot_pose_map.frame.M, entity.pose.frame.p), entity.frame_id) + + def _get_grasp_pose(self, entity, arm): + """ Computes the most suitable grasp pose to grasp the specified entity with the specified arm + + :param entity: entity to grasp + :param arm: arm to use + :return: FrameStamped with grasp pose in map frame + """ + candidates = [] starttime = rospy.Time.now() # ToDo: divide into functions From 3b6ee2de8aaa4948a84d1db49c308fa6b672bb0e Mon Sep 17 00:00:00 2001 From: Janno Lunenburg Date: Tue, 10 Mar 2020 20:51:11 +0100 Subject: [PATCH 4/9] test(smach)Improved info print and preempting of grasp height testscript --- .../manipulation/grasp_benchmark/grasping_height_test.py | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/robot_smach_states/examples/manipulation/grasp_benchmark/grasping_height_test.py b/robot_smach_states/examples/manipulation/grasp_benchmark/grasping_height_test.py index 40088a28a0..017b6f194f 100644 --- a/robot_smach_states/examples/manipulation/grasp_benchmark/grasping_height_test.py +++ b/robot_smach_states/examples/manipulation/grasp_benchmark/grasping_height_test.py @@ -43,7 +43,9 @@ z_values.append(z_values[-1] + DZ) results = [] - for z in z_values: + for test_nr, z in enumerate(z_values): + + print("\nTest nr {} of {} at height {}".format(test_nr + 1, len(z_values), z)) # Add an entity to ed grasp_entity_id = "test_entity" @@ -70,6 +72,9 @@ arm = robot._arms.values()[0] # type: HeroArm arm.send_gripper_goal(state=arms.GripperState.OPEN) + if rospy.is_shutdown(): + break + # Print the results print("\n ---------- \n") for result in results: From 7702914abf384c911ce846bb5598e7949468e2b2 Mon Sep 17 00:00:00 2001 From: Janno Lunenburg Date: Tue, 10 Mar 2020 21:45:05 +0100 Subject: [PATCH 5/9] doc(smach)Added docstring to grasping height test ` --- .../grasp_benchmark/grasping_height_test.py | 15 +++++++++++++-- 1 file changed, 13 insertions(+), 2 deletions(-) diff --git a/robot_smach_states/examples/manipulation/grasp_benchmark/grasping_height_test.py b/robot_smach_states/examples/manipulation/grasp_benchmark/grasping_height_test.py index 017b6f194f..db622417cb 100644 --- a/robot_smach_states/examples/manipulation/grasp_benchmark/grasping_height_test.py +++ b/robot_smach_states/examples/manipulation/grasp_benchmark/grasping_height_test.py @@ -1,5 +1,14 @@ #! /usr/bin/env python +""" +This script has the robot navigate to the couch table. Next, on a number of different heights, a (virtual) entity is + added to the world model, the robot tries to grasp it and subsequently it is removed again. At the end, the results + (height, outcome, duration) at each height are printed. + +Since the position of the entity (X_ENTITY, Y_ENTITY) and the id of the supporting entity (SUPPORTING_ENTITY_ID) are +heavily dependent, these are not parametrized. You can provide a different robot though. +""" + # System from collections import namedtuple @@ -22,7 +31,9 @@ Z_MAX = 1.2 X_ENTITY = 3.2 -Y_ENTITY = 1.55 +Y_ENTITY = 1. + +SUPPORTING_ENTITY_ID = "couch_table" Result = namedtuple("Result", ["height", "result", "duration"]) @@ -33,7 +44,7 @@ robot = get_robot_from_argv(1) # Navigate to the couch table - nav_designator = EntityByIdDesignator(robot=robot, id="couch_table") + nav_designator = EntityByIdDesignator(robot=robot, id=SUPPORTING_ENTITY_ID) nav_state = NavigateToSymbolic(robot, {nav_designator: "in_front_of"}, nav_designator) nav_state.execute() From 9e12e56a5e4b0ec4ce0b364245bab356b8fd6181 Mon Sep 17 00:00:00 2001 From: Janno Lunenburg Date: Tue, 10 Mar 2020 21:49:57 +0100 Subject: [PATCH 6/9] fix(smach)Grasppointdetermination: added check on frame ids --- .../manipulation/grasp_point_determination.py | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/robot_smach_states/src/robot_smach_states/manipulation/grasp_point_determination.py b/robot_smach_states/src/robot_smach_states/manipulation/grasp_point_determination.py index cde3827f3a..0111f8d333 100644 --- a/robot_smach_states/src/robot_smach_states/manipulation/grasp_point_determination.py +++ b/robot_smach_states/src/robot_smach_states/manipulation/grasp_point_determination.py @@ -39,9 +39,11 @@ def get_grasp_pose(self, entity, arm): return self._get_grasp_pose(entity, arm) # noinspection PyBroadException except Exception as e: - rospy.logwarn("Cannot compute grasp pose, defaulting to entity position and " - "orientation the same as the robot") + rospy.logwarn("Cannot compute grasp pose ({}), defaulting to entity position and " + "orientation the same as the robot".format(e.message)) robot_pose_map = self._robot.base.get_location() # type: FrameStamped + assert entity.frame_id == robot_pose_map.frame_id, "It is assumed that the entity frame id and the " \ + "world frame id are equal (typically 'map')" return FrameStamped(kdl.Frame(robot_pose_map.frame.M, entity.pose.frame.p), entity.frame_id) def _get_grasp_pose(self, entity, arm): From 575f91f6509e923e85a9e5147861a310f545ce8e Mon Sep 17 00:00:00 2001 From: Janno Lunenburg Date: Tue, 17 Mar 2020 20:14:28 +0100 Subject: [PATCH 7/9] Revert "fix(smach)Grasp point determination also works without convex hull" This reverts commit 0645e51f7f36c4c86ae1fa6ab43aa852324fc133. --- .../manipulation/grasp_point_determination.py | 20 ------------------- 1 file changed, 20 deletions(-) diff --git a/robot_smach_states/src/robot_smach_states/manipulation/grasp_point_determination.py b/robot_smach_states/src/robot_smach_states/manipulation/grasp_point_determination.py index 0111f8d333..31727fd81f 100644 --- a/robot_smach_states/src/robot_smach_states/manipulation/grasp_point_determination.py +++ b/robot_smach_states/src/robot_smach_states/manipulation/grasp_point_determination.py @@ -34,26 +34,6 @@ def get_grasp_pose(self, entity, arm): :param arm: arm to use :return: FrameStamped with grasp pose in map frame """ - try: - # Try to compute the grasp pose based on the entity - return self._get_grasp_pose(entity, arm) - # noinspection PyBroadException - except Exception as e: - rospy.logwarn("Cannot compute grasp pose ({}), defaulting to entity position and " - "orientation the same as the robot".format(e.message)) - robot_pose_map = self._robot.base.get_location() # type: FrameStamped - assert entity.frame_id == robot_pose_map.frame_id, "It is assumed that the entity frame id and the " \ - "world frame id are equal (typically 'map')" - return FrameStamped(kdl.Frame(robot_pose_map.frame.M, entity.pose.frame.p), entity.frame_id) - - def _get_grasp_pose(self, entity, arm): - """ Computes the most suitable grasp pose to grasp the specified entity with the specified arm - - :param entity: entity to grasp - :param arm: arm to use - :return: FrameStamped with grasp pose in map frame - """ - candidates = [] starttime = rospy.Time.now() # ToDo: divide into functions From 09de5169d321815ae17541f8b80f5dd0331ad6cd Mon Sep 17 00:00:00 2001 From: Janno Lunenburg Date: Tue, 17 Mar 2020 20:39:22 +0100 Subject: [PATCH 8/9] example(smach)Patch 'get_grasp_pose' from grasping height test script --- .../grasp_benchmark/grasping_height_test.py | 29 ++++++++++++++++++- 1 file changed, 28 insertions(+), 1 deletion(-) diff --git a/robot_smach_states/examples/manipulation/grasp_benchmark/grasping_height_test.py b/robot_smach_states/examples/manipulation/grasp_benchmark/grasping_height_test.py index db622417cb..d70ba8e678 100644 --- a/robot_smach_states/examples/manipulation/grasp_benchmark/grasping_height_test.py +++ b/robot_smach_states/examples/manipulation/grasp_benchmark/grasping_height_test.py @@ -11,6 +11,7 @@ # System from collections import namedtuple +from functools import partial # ROS import PyKDL as kdl @@ -25,6 +26,8 @@ from robot_skills import arms from robot_skills.hero_parts.hero_arm import HeroArm from robot_skills.get_robot import get_robot_from_argv +from robot_skills.robot import Robot +from robot_skills.util.entity import Entity from robot_skills.util.kdl_conversions import FrameStamped DZ = 0.1 @@ -37,6 +40,25 @@ Result = namedtuple("Result", ["height", "result", "duration"]) + +# noinspection PyUnusedLocal +def get_grasp_pose(_robot, entity, _arm): + # type: (Robot, Entity, HeroArm) -> FrameStamped + """ + Overrides the 'get_grasp_pose' method of the GraspPointDetermination class so that a grasp pose can be determined if + an entity does not have a convex hull. + + :param _robot: (Robot) api object + :param entity: entity to grasp + :param _arm: arm to use + :return: FrameStamped with grasp pose in map frame + """ + robot_pose_map = _robot.base.get_location() # type: FrameStamped + assert entity_pose.frame_id == robot_pose_map.frame_id, "It is assumed that the entity frame id and the " \ + "world frame id are equal (typically 'map')" + return FrameStamped(kdl.Frame(robot_pose_map.frame.M, entity.pose.frame.p), entity.frame_id) + + if __name__ == "__main__": rospy.init_node("test_grasping") @@ -74,6 +96,11 @@ "required_trajectories": ["prepare_grasp"], "required_gripper_types": [arms.GripperTypes.GRASPING] })) + + # Override the 'get_grasp_pose' method + # noinspection PyProtectedMember + grasp_state["GRAB"]._gpd.get_grasp_pose = partial(get_grasp_pose, robot) + grasp_result = grasp_state.execute() grasp_duration = (rospy.Time.now() - t_start).to_sec() results.append(Result(z, grasp_result, grasp_duration)) @@ -89,5 +116,5 @@ # Print the results print("\n ---------- \n") for result in results: - print "Height: {} --> {} (duration: {})".format(result.height, result.result, result.duration) + print("Height: {} --> {} (duration: {})".format(result.height, result.result, result.duration)) print("\n ---------- \n") From e46ac0b674f16e4b693c83ad3a5be73f5312103b Mon Sep 17 00:00:00 2001 From: Janno Lunenburg Date: Tue, 17 Mar 2020 20:48:40 +0100 Subject: [PATCH 9/9] example(smach)Fixed import in test grasp heights --- .../manipulation/grasp_benchmark/grasping_height_test.py | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/robot_smach_states/examples/manipulation/grasp_benchmark/grasping_height_test.py b/robot_smach_states/examples/manipulation/grasp_benchmark/grasping_height_test.py index d70ba8e678..b0b3c811c3 100644 --- a/robot_smach_states/examples/manipulation/grasp_benchmark/grasping_height_test.py +++ b/robot_smach_states/examples/manipulation/grasp_benchmark/grasping_height_test.py @@ -24,7 +24,6 @@ # Robot Skills from robot_skills import arms -from robot_skills.hero_parts.hero_arm import HeroArm from robot_skills.get_robot import get_robot_from_argv from robot_skills.robot import Robot from robot_skills.util.entity import Entity @@ -43,7 +42,7 @@ # noinspection PyUnusedLocal def get_grasp_pose(_robot, entity, _arm): - # type: (Robot, Entity, HeroArm) -> FrameStamped + # type: (Robot, Entity, arms.PublicArm) -> FrameStamped """ Overrides the 'get_grasp_pose' method of the GraspPointDetermination class so that a grasp pose can be determined if an entity does not have a convex hull. @@ -107,7 +106,7 @@ def get_grasp_pose(_robot, entity, _arm): # Lose the entity # noinspection PyProtectedMember - arm = robot._arms.values()[0] # type: HeroArm + arm = robot._arms.values()[0] # type: arms.PublicArm arm.send_gripper_goal(state=arms.GripperState.OPEN) if rospy.is_shutdown():