[PR2ProcessModulesPyCRAP] used pycrap in pr2 process modules.

cram2 · Nov 30, 2024 · 75ba1e8 · 75ba1e8
1 parent 8788797
commit 75ba1e8
Showing 1 changed file with 8 additions and 7 deletions.
diff --git a/src/pycram/process_modules/pr2_process_modules.py b/src/pycram/process_modules/pr2_process_modules.py
@@ -5,8 +5,9 @@
 import numpy as np
 import rospy
 
+import pycrap
 from .. import world_reasoning as btr
-from ..datastructures.enums import JointType, ObjectType, Arms, ExecutionType, GripperState, MovementType
+from ..datastructures.enums import JointType, Arms, ExecutionType, GripperState, MovementType
 from ..datastructures.world import World
 from ..designators.motion_designator import MoveMotion, LookingMotion, \
     DetectingMotion, MoveTCPMotion, MoveArmJointsMotion, WorldStateDetectingMotion, MoveJointsMotion, \
@@ -223,7 +224,6 @@ def _execute(self, designator: MoveMotion) -> Any:
             joint_positions = World.current_world.robot.get_positions_of_controllable_joints()
             giskard.set_joint_goal(joint_positions)
         giskard.achieve_cartesian_goal(designator.target, RobotDescription.current_robot_description.base_link, "map",
-                                       allow_gripper_collision_=False,
                                        use_monitor=designator.monitor_motion)
         if not World.current_world.robot.pose.almost_equal(designator.target, 0.05, 3):
             raise NavigationGoalNotReachedError(World.current_world.robot.pose, designator.target)
@@ -274,11 +274,11 @@ def _execute(self, designator: DetectingMotion) -> Any:
         if world_obj:
             world_obj[0].set_pose(obj_pose)
             return world_obj[0]
-        elif designator.object_type == ObjectType.JEROEN_CUP:
-            cup = Object("cup", ObjectType.JEROEN_CUP, "jeroen_cup.stl", pose=obj_pose)
+        elif issubclass(designator.object_type, pycrap.Cup):
+            cup = Object("cup", pycrap.Cup, "jeroen_cup.stl", pose=obj_pose)
             return cup
-        elif designator.object_type == ObjectType.BOWL:
-            bowl = Object("bowl", ObjectType.BOWL, "bowl.stl", pose=obj_pose)
+        elif issubclass(designator.object_type, pycrap.Bowl):
+            bowl = Object("bowl", pycrap.Bowl, "bowl.stl", pose=obj_pose)
             return bowl
 
         return world_obj[0]
@@ -295,6 +295,7 @@ def _execute(self, designator: MoveTCPMotion) -> Any:
         tip_link = RobotDescription.current_robot_description.get_arm_chain(designator.arm).get_tool_frame()
         root_link = "map"
 
+        gripper_that_can_collide = designator.arm if designator.allow_gripper_collision else None
         if designator.allow_gripper_collision:
             giskard.allow_gripper_collision(designator.arm)
 
@@ -306,7 +307,7 @@ def _execute(self, designator: MoveTCPMotion) -> Any:
             giskard.achieve_translation_goal(pose_in_map.position_as_list(), tip_link, root_link)
         elif designator.movement_type == MovementType.CARTESIAN:
             giskard.achieve_cartesian_goal(pose_in_map, tip_link, root_link,
-                                           allow_gripper_collision_=designator.allow_gripper_collision,
+                                           grippers_that_can_collide=gripper_that_can_collide,
                                            use_monitor=designator.monitor_motion)
         if not World.current_world.robot.get_link_pose(tip_link).almost_equal(designator.target, 0.01, 3):
             raise ToolPoseNotReachedError(World.current_world.robot.get_link_pose(tip_link), designator.target)