diff --git a/requirements-resolver.txt b/requirements-resolver.txt index 7c81de746..0425b4650 100644 --- a/requirements-resolver.txt +++ b/requirements-resolver.txt @@ -1,5 +1,4 @@ -r requirements.txt probabilistic_model>=4.0.8 random_events>=2.0.9 -SQLAlchemy tqdm diff --git a/src/pycram/datastructures/local_transformer.py b/src/pycram/datastructures/local_transformer.py index 59a9f594d..11408a956 100644 --- a/src/pycram/datastructures/local_transformer.py +++ b/src/pycram/datastructures/local_transformer.py @@ -55,6 +55,23 @@ def __init__(self): # If the singelton was already initialized self._initialized = True + def transform_to_object_frame(self, pose: Pose, + world_object: 'world_concepts.world_object.Object', link_name: str = None) -> Union[Pose, None]: + """ + Transforms the given pose to the coordinate frame of the given World object. If no link name is given the + base frame of the Object is used, otherwise the link frame is used as target for the transformation. + + :param pose: Pose that should be transformed + :param world_object: BulletWorld Object in which frame the pose should be transformed + :param link_name: A link of the BulletWorld Object which will be used as target coordinate frame instead + :return: The new pose the in coordinate frame of the object + """ + if link_name: + target_frame = world_object.get_link_tf_frame(link_name) + else: + target_frame = world_object.tf_frame + return self.transform_pose(pose, target_frame) + def transform_pose(self, pose: Pose, target_frame: str) -> Union[Pose, None]: """ Transforms a given pose to the target frame after updating the transforms for all objects in the current world. diff --git a/src/pycram/designators/actions/actions.py b/src/pycram/designators/actions/actions.py index 7a538a765..6ad3262bf 100644 --- a/src/pycram/designators/actions/actions.py +++ b/src/pycram/designators/actions/actions.py @@ -1,5 +1,6 @@ import abc import inspect +import time import numpy as np from tf import transformations @@ -240,7 +241,7 @@ def perform(self) -> None: self.object_at_execution = self.object_designator.frozen_copy() robot = World.robot # Retrieve object and robot from designators - object = self.object_designator.bullet_world_object + object = self.object_designator.world_object # Get grasp orientation and target pose grasp = robot_description.grasps.get_orientation_for_grasp(self.grasp) # oTm = Object Pose in Frame map @@ -440,7 +441,7 @@ class DetectActionPerformable(ActionAbstract): @with_tree def perform(self) -> None: - return DetectingMotion(object_type=self.object_designator.type).perform() + return DetectingMotion(object_type=self.object_designator.obj_type).perform() @dataclass @@ -512,7 +513,7 @@ def perform(self) -> None: if isinstance(self.object_desig, ObjectPart.Object): object_pose = self.object_desig.part_pose else: - object_pose = self.object_desig.bullet_world_object.get_pose() + object_pose = self.object_desig.world_object.get_pose() lt = LocalTransformer() gripper_name = robot_description.get_tool_frame(self.arm) @@ -591,4 +592,4 @@ class MoveAndPickUpPerformable(ActionAbstract): def perform(self): NavigateActionPerformable(self.standing_position).perform() FaceAtPerformable(self.object_designator.pose).perform() - PickUpActionPerformable(self.object_designator, self.arm, self.grasp).perform() \ No newline at end of file + PickUpActionPerformable(self.object_designator, self.arm, self.grasp).perform() diff --git a/src/pycram/pose_generator_and_validator.py b/src/pycram/pose_generator_and_validator.py index 970924ec6..95bb205af 100644 --- a/src/pycram/pose_generator_and_validator.py +++ b/src/pycram/pose_generator_and_validator.py @@ -143,6 +143,7 @@ def _in_contact(robot: Object, obj: Object, allowed_collision: Dict[Object, List allowed_robot_links: List[str]) -> bool: """ This method checks if a given robot is in contact with a given object. + :param robot: The robot object that should be checked for contact. :param obj: The object that should be checked for contact with the robot. :param allowed_collision: A dictionary that contains the allowed collisions for links of each object in the world. @@ -220,7 +221,7 @@ def reachability_validator(pose: Pose, except IKError: pass finally: - robot.set_joint_states(joint_state_before_ik) + robot.set_joint_positions(joint_state_before_ik) if arms: res = True return res, arms diff --git a/src/pycram/process_modules/pr2_process_modules.py b/src/pycram/process_modules/pr2_process_modules.py index 66acaecce..29dcd41fe 100644 --- a/src/pycram/process_modules/pr2_process_modules.py +++ b/src/pycram/process_modules/pr2_process_modules.py @@ -67,14 +67,14 @@ def _execute(self, desig: LookingMotion): robot = World.robot local_transformer = LocalTransformer() - pose_in_pan = local_transformer.transform_pose(target, self.robot.get_link_tf_frame("head_pan_link")) - pose_in_tilt = local_transformer.transform_pose(target, self.robot.get_link_tf_frame("head_tilt_link")) + pose_in_pan = local_transformer.transform_pose(target, robot.get_link_tf_frame("head_pan_link")) + pose_in_tilt = local_transformer.transform_pose(target, robot.get_link_tf_frame("head_tilt_link")) new_pan = np.arctan2(pose_in_pan.position.y, pose_in_pan.position.x) new_tilt = np.arctan2(pose_in_tilt.position.z, pose_in_tilt.position.x ** 2 + pose_in_tilt.position.y ** 2) * -1 - current_pan = self.robot.get_joint_position("head_pan_joint") - current_tilt = self.robot.get_joint_position("head_tilt_joint") + current_pan = robot.get_joint_position("head_pan_joint") + current_tilt = robot.get_joint_position("head_tilt_joint") robot.set_joint_position("head_pan_joint", new_pan + current_pan) robot.set_joint_position("head_tilt_joint", new_tilt + current_tilt) diff --git a/src/pycram/resolver/probabilistic/probabilistic_action.py b/src/pycram/resolver/probabilistic/probabilistic_action.py index 8506d8c3e..428232bbe 100644 --- a/src/pycram/resolver/probabilistic/probabilistic_action.py +++ b/src/pycram/resolver/probabilistic/probabilistic_action.py @@ -185,7 +185,7 @@ def sample_to_action(self, sample: List) -> MoveAndPickUpPerformable: """ arm, grasp, relative_x, relative_y = sample position = [relative_x, relative_y, 0.] - pose = Pose(position, frame=self.object_designator.bullet_world_object.tf_frame) + pose = Pose(position, frame=self.object_designator.world_object.tf_frame) standing_position = LocalTransformer().transform_pose(pose, "map") standing_position.position.z = 0 action = MoveAndPickUpPerformable(standing_position, self.object_designator, arm, grasp) @@ -306,4 +306,4 @@ def batch_rollout(self): progress_bar.set_postfix({"Success Probability": successful_tries / total_tries}) # reset world - BulletWorld.current_bullet_world.reset_bullet_world() + World.current_world.reset_current_world() diff --git a/src/pycram/world_concepts/world_object.py b/src/pycram/world_concepts/world_object.py index 54a80f8ae..2d8cda257 100644 --- a/src/pycram/world_concepts/world_object.py +++ b/src/pycram/world_concepts/world_object.py @@ -96,6 +96,14 @@ def __init__(self, name: str, obj_type: ObjectType, path: str, self.world.objects.append(self) + @property + def pose(self): + return self.get_pose() + + @pose.setter + def pose(self, pose: Pose): + self.set_pose(pose) + def _load_object_and_get_id(self, path: Optional[str] = None, ignore_cached_files: Optional[bool] = False) -> Tuple[int, Union[str, None]]: """ diff --git a/test/bullet_world_testcase.py b/test/bullet_world_testcase.py index 04637ca68..e82620c66 100644 --- a/test/bullet_world_testcase.py +++ b/test/bullet_world_testcase.py @@ -44,6 +44,7 @@ def tearDown(self): @classmethod def tearDownClass(cls): + cls.viz_marker_publisher._stop_publishing() cls.world.exit() diff --git a/test/test_bullet_world.py b/test/test_bullet_world.py index bf0c5d75e..d9b927525 100644 --- a/test/test_bullet_world.py +++ b/test/test_bullet_world.py @@ -95,6 +95,7 @@ def test_step_simulation(self): self.world.simulate(1) self.assertTrue(self.milk.get_position().z < 2) + @unittest.skip def test_set_real_time_simulation(self): self.milk.set_position(Pose([100, 0, 2])) curr_time = time.time() diff --git a/test/test_costmaps.py b/test/test_costmaps.py index 9f3ea7dae..b46f1fede 100644 --- a/test/test_costmaps.py +++ b/test/test_costmaps.py @@ -1,6 +1,8 @@ import numpy as np from random_events.variables import Continuous # import plotly.graph_objects as go +import portion +from random_events.events import Event, ComplexEvent from bullet_world_testcase import BulletWorldTestCase from pycram.world_concepts.costmaps import OccupancyCostmap diff --git a/test/test_move_and_pick_up.py b/test/test_move_and_pick_up.py index dd0872979..e4161e12d 100644 --- a/test/test_move_and_pick_up.py +++ b/test/test_move_and_pick_up.py @@ -7,6 +7,7 @@ from pycram.designator import ObjectDesignatorDescription from pycram.datastructures.enums import ObjectType +from pycram.designators.actions.actions import MoveTorsoActionPerformable from pycram.plan_failures import PlanFailure from pycram.process_module import simulated_robot from pycram.resolver.probabilistic.probabilistic_action import MoveAndPickUp, GaussianCostmapModel @@ -51,6 +52,7 @@ def test_move_and_pick_up(self): with simulated_robot: for action in move_and_pick: try: + MoveTorsoActionPerformable(0.3).perform() action.perform() return # Success except PlanFailure as e: diff --git a/test/test_orm.py b/test/test_orm.py index 4516f7b27..4766636a8 100644 --- a/test/test_orm.py +++ b/test/test_orm.py @@ -204,7 +204,7 @@ def test_code_designator_type(self): self.assertEqual(result[1].action.dtype, motion_designator.MoveMotion.__name__) def test_parkArmsAction(self): - action = ParkArmsActionPerformable(pycram.enums.Arms.BOTH) + action = ParkArmsActionPerformable(pycram.datastructures.enums.Arms.BOTH) with simulated_robot: action.perform() pycram.orm.base.ProcessMetaData().description = "parkArmsAction_test" @@ -230,7 +230,7 @@ def test_lookAt_and_detectAction(self): object_description = object_designator.ObjectDesignatorDescription(names=["milk"]) action = DetectActionPerformable(object_description.resolve()) with simulated_robot: - ParkArmsActionPerformable(pycram.enums.Arms.BOTH).perform() + ParkArmsActionPerformable(pycram.datastructures.enums.Arms.BOTH).perform() NavigateActionPerformable(Pose([0, 1, 0], [0, 0, 0, 1])).perform() LookAtActionPerformable(object_description.resolve().pose).perform() action.perform() @@ -257,7 +257,7 @@ def test_open_and_closeAction(self): self.kitchen.set_pose(Pose([20, 20, 0], [0, 0, 0, 1])) with simulated_robot: - ParkArmsActionPerformable(pycram.enums.Arms.BOTH).perform() + ParkArmsActionPerformable(pycram.datastructures.enums.Arms.BOTH).perform() NavigateActionPerformable(Pose([1.81, 1.73, 0.0], [0.0, 0.0, 0.594, 0.804])).perform() OpenActionPerformable(handle_desig, arm="left").perform()