From 716eb491077462716d71253ccd75e63759df704e Mon Sep 17 00:00:00 2001 From: Vanessa Hassouna Date: Sun, 8 Dec 2024 12:48:36 +0100 Subject: [PATCH 1/2] [CostmapLocation] for reachability should restrict to hand_links corresponding to the selected arm. --- src/pycram/designators/location_designator.py | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/src/pycram/designators/location_designator.py b/src/pycram/designators/location_designator.py index 395a46c2f..da9a74dbd 100644 --- a/src/pycram/designators/location_designator.py +++ b/src/pycram/designators/location_designator.py @@ -186,7 +186,9 @@ def __iter__(self): if self.reachable_for: hand_links = [] for description in RobotDescription.current_robot_description.get_manipulator_chains(): - hand_links += description.end_effector.links + if isinstance(self.reachable_arm, Arms): + if self.reachable_arm == description.arm_type: + hand_links += description.end_effector.links valid, arms = reachability_validator(maybe_pose, test_robot, target_pose, allowed_collision={test_robot: hand_links}) if self.reachable_arm: From 253249f7f8b60ffd0c71cda472cf25693346e04a Mon Sep 17 00:00:00 2001 From: Vanessa Hassouna <33067562+sunava@users.noreply.github.com> Date: Wed, 11 Dec 2024 14:31:31 +0100 Subject: [PATCH 2/2] [location_designator] Updated for loop --- src/pycram/designators/location_designator.py | 11 +++++++---- 1 file changed, 7 insertions(+), 4 deletions(-) diff --git a/src/pycram/designators/location_designator.py b/src/pycram/designators/location_designator.py index da9a74dbd..95d862153 100644 --- a/src/pycram/designators/location_designator.py +++ b/src/pycram/designators/location_designator.py @@ -185,10 +185,13 @@ def __iter__(self): World.current_world) if self.reachable_for: hand_links = [] - for description in RobotDescription.current_robot_description.get_manipulator_chains(): - if isinstance(self.reachable_arm, Arms): - if self.reachable_arm == description.arm_type: - hand_links += description.end_effector.links + + if self.reachable_arm: + arm_chain = RobotDescription.current_robot_description.get_arm_chain(self.reachable_arm) + hand_links += arm_chain.end_effector.links + else: + for chain in RobotDescription.current_robot_description.get_manipulator_chains(): + hand_links += chain.end_effector.links valid, arms = reachability_validator(maybe_pose, test_robot, target_pose, allowed_collision={test_robot: hand_links}) if self.reachable_arm: