From b34c45d16cf4c8fb34ea20cd11f1c0009f6cdea4 Mon Sep 17 00:00:00 2001 From: Abdelrhman Bassiouny Date: Tue, 15 Oct 2024 12:58:54 +0200 Subject: [PATCH 01/91] [MultiverseTest] changed cup spawn position to a bit nearer to the milk surface such that the cup becomes staple and does not fall from the milk surface. --- test/test_multiverse.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/test/test_multiverse.py b/test/test_multiverse.py index d0e75ce91..dd2a3c89a 100644 --- a/test/test_multiverse.py +++ b/test/test_multiverse.py @@ -367,7 +367,7 @@ def test_get_object_contact_points(self): self.assertEqual(len(contact_points), 1) self.assertIsInstance(contact_points[0], ContactPoint) self.assertTrue(contact_points[0].link_b.object, self.multiverse.floor) - cup = self.spawn_cup([1, 1, 0.15]) + cup = self.spawn_cup([1, 1, 0.12]) # This is needed because the cup is spawned in the air, so it needs to fall # to get in contact with the milk self.multiverse.simulate(0.3) @@ -381,7 +381,7 @@ def test_get_object_contact_points(self): def test_get_contact_points_between_two_objects(self): for i in range(3): milk = self.spawn_milk([1, 1, 0.01], [0, -0.707, 0, 0.707]) - cup = self.spawn_cup([1, 1, 0.15]) + cup = self.spawn_cup([1, 1, 0.12]) # This is needed because the cup is spawned in the air so it needs to fall # to get in contact with the milk self.multiverse.simulate(0.3) From 497f84d34edbb0085d65128f7294bf74636acdc9 Mon Sep 17 00:00:00 2001 From: Abdelrhman Bassiouny Date: Tue, 15 Oct 2024 18:03:54 +0200 Subject: [PATCH 02/91] [MultiverseContact] (WIP) Adding support for multiple contact points. --- src/pycram/datastructures/dataclasses.py | 5 +- src/pycram/datastructures/enums.py | 1 + src/pycram/worlds/multiverse.py | 19 ++-- .../multiverse_communication/clients.py | 97 ++++++++++++++++--- 4 files changed, 97 insertions(+), 25 deletions(-) diff --git a/src/pycram/datastructures/dataclasses.py b/src/pycram/datastructures/dataclasses.py index c6f55bf52..ba9190f49 100644 --- a/src/pycram/datastructures/dataclasses.py +++ b/src/pycram/datastructures/dataclasses.py @@ -864,9 +864,8 @@ class MultiverseContactPoint: """ A dataclass to store the contact point returned from Multiverse. """ - body_name: str - contact_force: List[float] - contact_torque: List[float] + position: List[float] + normal: List[float] @dataclass diff --git a/src/pycram/datastructures/enums.py b/src/pycram/datastructures/enums.py index 04d81b105..699ff1aff 100644 --- a/src/pycram/datastructures/enums.py +++ b/src/pycram/datastructures/enums.py @@ -242,6 +242,7 @@ class MultiverseAPIName(Enum): """ Enum for the different APIs of the Multiverse. """ + GET_CONTACT_POINTS = "get_contact_points" GET_CONTACT_BODIES = "get_contact_bodies" GET_CONSTRAINT_EFFORT = "get_constraint_effort" ATTACH = "attach" diff --git a/src/pycram/worlds/multiverse.py b/src/pycram/worlds/multiverse.py index 6cbeb0d78..a846eac80 100644 --- a/src/pycram/worlds/multiverse.py +++ b/src/pycram/worlds/multiverse.py @@ -516,24 +516,27 @@ def get_object_contact_points(self, obj: Object) -> ContactPointsList: """ Note: Currently Multiverse only gets one contact point per contact objects. """ - multiverse_contact_points = self.api_requester.get_contact_points(obj) + multiverse_contact_data = self.api_requester.get_all_contact_data_of_object(obj) + force = multiverse_contact_data.data.contact_force + torque = multiverse_contact_data.data.contact_torque + normal = multiverse_contact_data.data.points contact_points = ContactPointsList([]) body_link = None - for point in multiverse_contact_points: - if point.body_name == "world": - point.body_name = "floor" - body_object = self.get_object_by_name(point.body_name) + for body_name in multiverse_contact_data.body_names: + if body_name == "world": + body_name = "floor" + body_object = self.get_object_by_name(body_name) if body_object is None: for obj in self.objects: for link in obj.links.values(): - if link.name == point.body_name: + if link.name == body_name: body_link = link break else: body_link = body_object.root_link if body_link is None: - logging.error(f"Body link not found: {point.body_name}") - raise ValueError(f"Body link not found: {point.body_name}") + logging.error(f"Body link not found: {body_name}") + raise ValueError(f"Body link not found: {body_name}") contact_points.append(ContactPoint(obj.root_link, body_link)) contact_points[-1].force_x_in_world_frame = point.contact_force[0] contact_points[-1].force_y_in_world_frame = point.contact_force[1] diff --git a/src/pycram/worlds/multiverse_communication/clients.py b/src/pycram/worlds/multiverse_communication/clients.py index b10959a90..700638661 100644 --- a/src/pycram/worlds/multiverse_communication/clients.py +++ b/src/pycram/worlds/multiverse_communication/clients.py @@ -8,7 +8,8 @@ from .socket import MultiverseSocket, MultiverseMetaData from ...config.multiverse_conf import MultiverseConfig as Conf -from ...datastructures.dataclasses import RayResult, MultiverseContactPoint +from ...datastructures.dataclasses import RayResult, MultiverseObjectContactData, MultiverseContactPoint, \ + MultiverseContactData from ...datastructures.enums import (MultiverseAPIName as API, MultiverseBodyProperty as BodyProperty, MultiverseProperty as Property) from ...datastructures.pose import Pose @@ -666,18 +667,61 @@ def check_object_exists(self, obj: Object) -> bool: """ return self._request_single_api_callback(API.EXIST, obj.name)[0] == 'yes' - def get_contact_points(self, obj: Object) -> List[MultiverseContactPoint]: + def get_all_contact_data_of_object(self, obj: Object) -> MultiverseObjectContactData: """ - Request the contact points of an object, this includes the object names and the contact forces and torques. + Request the contact data of an object, this includes the object names, the contact forces/torques, and the + contact points. :param obj: The object. - :return: The contact points of the object as a list of MultiverseContactPoint. + :return: The contact data of the object as a MultiverseObjectContactData. """ - api_response_data = self._get_contact_points(obj.name) + api_response_data = self._get_all_contact_data_of_object(obj.name) body_names = api_response_data[API.GET_CONTACT_BODIES] - contact_efforts = self._parse_constraint_effort(api_response_data[API.GET_CONSTRAINT_EFFORT]) - return [MultiverseContactPoint(body_names[i], contact_efforts[:3], contact_efforts[3:]) - for i in range(len(body_names))] + points = self._parse_contact_points(api_response_data[API.GET_CONTACT_POINTS]) + forces, torques = self._parse_constraint_effort(api_response_data[API.GET_CONSTRAINT_EFFORT]) + return MultiverseObjectContactData(body_names, MultiverseContactData(points, {obj.name: forces}, + {obj.name: torques})) + + def get_all_contact_data_between_objects(self, obj1: Object, obj2: Object) -> MultiverseContactData: + """ + Request the contact data between two objects, this includes the contact forces/torques, and the contact points. + + :param obj1: The first object. + :param obj2: The second object. + :return: The contact data between the objects as a MultiverseContactData. + """ + api_response = self._get_all_contact_data_between_objects(obj1.name, obj2.name) + points = self._parse_contact_points(api_response[API.GET_CONTACT_POINTS]) + force_on_a, torque_on_a = self._parse_constraint_effort(api_response[API.GET_CONSTRAINT_EFFORT][obj1.name]) + force_on_b, torque_on_b = self._parse_constraint_effort(api_response[API.GET_CONSTRAINT_EFFORT][obj2.name]) + return MultiverseContactData(points, {obj1.name: force_on_a, obj2.name: force_on_b}, + {obj1.name: torque_on_a, obj2.name: torque_on_b}) + + def _get_all_contact_data_between_objects(self, obj1_name: str, obj2_name: str) -> Dict[API, List]: + """ + Request the contact data between two objects, this includes the contact forces/torques, and the contact points. + + :param obj1_name: The name of the first object. + :param obj2_name: The name of the second object. + :return: The contact data between the objects as a dictionary. + """ + data_1 = self._request_apis_callbacks({API.GET_CONTACT_POINTS: [obj1_name, obj2_name], + API.GET_CONSTRAINT_EFFORT: [obj1_name], + }) + data_2 = self._request_single_api_callback(API.GET_CONSTRAINT_EFFORT, obj2_name) + data_1[API.GET_CONSTRAINT_EFFORT].extend(data_2) + return data_1 + + def _get_contact_points_between_objects(self, obj1_name: str, obj2_name: str) -> List[MultiverseContactPoint]: + """ + Request the contact points between two objects. + + :param obj1_name: The name of the first object. + :param obj2_name: The name of the second object. + :return: The contact points between the objects as a list of MultiverseContactPoint. + """ + points = self._request_single_api_callback(API.GET_CONTACT_POINTS, obj1_name, obj2_name) + return self._parse_contact_points(points) def get_objects_intersected_with_rays(self, from_positions: List[List[float]], to_positions: List[List[float]]) -> List[RayResult]: @@ -733,7 +777,7 @@ def list_of_positions_to_string(positions: List[List[float]]) -> str: return " ".join([f"{position[0]} {position[1]} {position[2]}" for position in positions]) @staticmethod - def _parse_constraint_effort(contact_effort: List[str]) -> List[float]: + def _parse_constraint_effort(contact_effort: List[str]) -> Tuple[List[float], List[float]]: """ Parse the contact effort of an object. @@ -744,17 +788,42 @@ def _parse_constraint_effort(contact_effort: List[str]) -> List[float]: if 'failed' in contact_effort: logwarn("Failed to get contact effort") return [0.0] * 6 - return list(map(float, contact_effort)) + contact_effort = list(map(float, contact_effort)) + forces, torques = contact_effort[:3], contact_effort[3:] + return forces, torques + + def _get_contact_points(self, obj: Object) -> List[MultiverseContactPoint]: + """ + Get the 3d positions of the contacts of an object. + + :param obj: The object. + :return: The contact points of the object as a list of MultiverseContactPoint. + """ + api_response = self._request_single_api_callback(API.GET_CONTACT_POINTS, obj.name) + return self._parse_contact_points(api_response) + + @staticmethod + def _parse_contact_points(contact_points: List[str]) -> List[MultiverseContactPoint]: + """ + Parse the contact points of an object. + + :param contact_points: The contact points of the object as a list of strings. + :return: The contact positions, and normal vectors as a list of MultiverseContactPoint. + """ + contact_point_data = [list(map(float, contact_point.split())) for contact_point in contact_points] + return [MultiverseContactPoint(point[:3], point[3:]) for point in contact_point_data] - def _get_contact_points(self, object_name) -> Dict[API, List]: + def _get_all_contact_data_of_object(self, object_name) -> Dict[API, List]: """ - Request the contact points of an object. + Request the names of bodies/objects that are in contact of an object, + the contact forces and torques, and the contact points. :param object_name: The name of the object. - :return: The contact points api response as a dictionary. + :return: The contact bodies, contact forces and torques, and contact points as a dictionary. """ return self._request_apis_callbacks({API.GET_CONTACT_BODIES: [object_name], - API.GET_CONSTRAINT_EFFORT: [object_name] + API.GET_CONSTRAINT_EFFORT: [object_name], + API.GET_CONTACT_POINTS: [object_name] }) def pause_simulation(self) -> None: From 0a56902ca4d4a47ec82c61429c32c3d99c0bf116 Mon Sep 17 00:00:00 2001 From: Abdelrhman Bassiouny Date: Wed, 16 Oct 2024 18:48:17 +0200 Subject: [PATCH 03/91] [MultiverseContact] Multiple contact points with normals now working. --- resources/worlds/pycram_test_empty.muv | 52 +++++++++++++ src/pycram/datastructures/dataclasses.py | 16 ++-- src/pycram/worlds/multiverse.py | 58 +++++++------- .../multiverse_communication/clients.py | 75 +++++++------------ test/test_multiverse.py | 6 +- 5 files changed, 128 insertions(+), 79 deletions(-) create mode 100644 resources/worlds/pycram_test_empty.muv diff --git a/resources/worlds/pycram_test_empty.muv b/resources/worlds/pycram_test_empty.muv new file mode 100644 index 000000000..86d286c2f --- /dev/null +++ b/resources/worlds/pycram_test_empty.muv @@ -0,0 +1,52 @@ +resources: + - ../cached + - ../robots + - ../worlds + - ../objects + +worlds: + pycram_test: + rtf_desired: 1 + prospection_pycram_test: + rtf_desired: 1 + +simulations: + pycram_test: + simulator: mujoco + world: + name: world + path: floor.xml + apply: + body: + gravcomp: 1 + config: + max_time_step: 0.002 + min_time_step: 0.001 + prospection_pycram_test: + simulator: mujoco + world: + name: prospection_world + path: floor.xml + apply: + body: + gravcomp: 1 + config: + max_time_step: 0.002 + min_time_step: 0.001 + +multiverse_server: + host: "tcp://127.0.0.1" + port: 7000 + +multiverse_clients: + pycram_test: + port: 7500 + send: + body: ["position", "quaternion", "relative_velocity"] + joint: ["joint_rvalue", "joint_tvalue", "joint_linear_velocity", "joint_angular_velocity", "joint_force", "joint_torque"] + + prospection_pycram_test: + port: 7600 + send: + body: [ "position", "quaternion", "relative_velocity" ] + joint: [ "joint_rvalue", "joint_tvalue", "joint_linear_velocity", "joint_angular_velocity", "joint_force", "joint_torque" ] \ No newline at end of file diff --git a/src/pycram/datastructures/dataclasses.py b/src/pycram/datastructures/dataclasses.py index ba9190f49..856c429f1 100644 --- a/src/pycram/datastructures/dataclasses.py +++ b/src/pycram/datastructures/dataclasses.py @@ -633,14 +633,11 @@ class ContactPoint: link_b: Link position_on_object_a: Optional[List[float]] = None position_on_object_b: Optional[List[float]] = None - normal_on_b: Optional[List[float]] = None # normal on object b pointing towards object a - distance: Optional[float] = None + normal_on_b: Optional[List[float]] = None # the contact normal vector on object b pointing towards object a + distance: Optional[float] = None # distance between the two objects (+ve for separation, -ve for penetration) normal_force: Optional[List[float]] = None # normal force applied during last step simulation lateral_friction_1: Optional[LateralFriction] = None lateral_friction_2: Optional[LateralFriction] = None - force_x_in_world_frame: Optional[float] = None - force_y_in_world_frame: Optional[float] = None - force_z_in_world_frame: Optional[float] = None def __str__(self): return f"ContactPoint: {self.link_a.object.name} - {self.link_b.object.name}" @@ -859,6 +856,15 @@ def intersected(self) -> bool: return self.distance >= 0 and self.body_name != "" +@dataclass +class MultiverseObjectContactData: + """ + A dataclass to store all the contact data returned from Multiverse for a single object. + """ + body_names: List[str] + data: List[MultiverseContactPoint] + + @dataclass class MultiverseContactPoint: """ diff --git a/src/pycram/worlds/multiverse.py b/src/pycram/worlds/multiverse.py index a846eac80..0fd2a52d9 100644 --- a/src/pycram/worlds/multiverse.py +++ b/src/pycram/worlds/multiverse.py @@ -8,7 +8,8 @@ from .multiverse_communication.client_manager import MultiverseClientManager from .multiverse_communication.clients import MultiverseController, MultiverseReader, MultiverseWriter, MultiverseAPI from ..config.multiverse_conf import MultiverseConfig -from ..datastructures.dataclasses import AxisAlignedBoundingBox, Color, ContactPointsList, ContactPoint +from ..datastructures.dataclasses import AxisAlignedBoundingBox, Color, ContactPointsList, ContactPoint, \ + MultiverseContactPoint from ..datastructures.enums import WorldMode, JointType, ObjectType, MultiverseBodyProperty, MultiverseJointPosition, \ MultiverseJointCMD from ..datastructures.pose import Pose @@ -516,35 +517,42 @@ def get_object_contact_points(self, obj: Object) -> ContactPointsList: """ Note: Currently Multiverse only gets one contact point per contact objects. """ - multiverse_contact_data = self.api_requester.get_all_contact_data_of_object(obj) - force = multiverse_contact_data.data.contact_force - torque = multiverse_contact_data.data.contact_torque - normal = multiverse_contact_data.data.points + contact_bodies = self.api_requester.get_contact_bodies_of_object(obj) contact_points = ContactPointsList([]) - body_link = None - for body_name in multiverse_contact_data.body_names: - if body_name == "world": - body_name = "floor" - body_object = self.get_object_by_name(body_name) - if body_object is None: - for obj in self.objects: - for link in obj.links.values(): - if link.name == body_name: - body_link = link - break - else: - body_link = body_object.root_link + for body_name in contact_bodies: + multiverse_contact_points = self.api_requester.get_contact_points_between_objects(obj.name, body_name) + body_object, body_link = self.get_object_with_body_name(body_name) if body_link is None: - logging.error(f"Body link not found: {body_name}") + logerr(f"Body link not found: {body_name}") raise ValueError(f"Body link not found: {body_name}") - contact_points.append(ContactPoint(obj.root_link, body_link)) - contact_points[-1].force_x_in_world_frame = point.contact_force[0] - contact_points[-1].force_y_in_world_frame = point.contact_force[1] - contact_points[-1].force_z_in_world_frame = point.contact_force[2] - contact_points[-1].normal_on_b = point.contact_force[2] - contact_points[-1].normal_force = point.contact_force[2] + for point in multiverse_contact_points: + contact_points.append(ContactPoint(obj.root_link, body_link)) + contact_points[-1].normal_on_b = point.normal + contact_points[-1].position_on_b = point.position return contact_points + def get_object_with_body_name(self, body_name: str) -> Tuple[Optional[Object],Optional[Link]]: + """ + Get the object with the body name in the simulator, the body name can be the name of the object or the link. + + :param body_name: The name of the body. + :return: The object if found otherwise None, and the link if the body name is a link. + """ + if body_name == "world": + body_name = "floor" + body_object = self.get_object_by_name(body_name) + body_link = None + if body_object is None: + for obj in self.objects: + for link in obj.links.values(): + if link.name == body_name: + body_link = link + body_object = obj + break + else: + body_link = body_object.root_link + return body_object, body_link + @staticmethod def _get_normal_force_on_object_from_contact_force(obj: Object, contact_force: List[float]) -> float: """ diff --git a/src/pycram/worlds/multiverse_communication/clients.py b/src/pycram/worlds/multiverse_communication/clients.py index 700638661..9e58bab27 100644 --- a/src/pycram/worlds/multiverse_communication/clients.py +++ b/src/pycram/worlds/multiverse_communication/clients.py @@ -8,8 +8,7 @@ from .socket import MultiverseSocket, MultiverseMetaData from ...config.multiverse_conf import MultiverseConfig as Conf -from ...datastructures.dataclasses import RayResult, MultiverseObjectContactData, MultiverseContactPoint, \ - MultiverseContactData +from ...datastructures.dataclasses import RayResult, MultiverseObjectContactData, MultiverseContactPoint from ...datastructures.enums import (MultiverseAPIName as API, MultiverseBodyProperty as BodyProperty, MultiverseProperty as Property) from ...datastructures.pose import Pose @@ -667,52 +666,29 @@ def check_object_exists(self, obj: Object) -> bool: """ return self._request_single_api_callback(API.EXIST, obj.name)[0] == 'yes' - def get_all_contact_data_of_object(self, obj: Object) -> MultiverseObjectContactData: + def get_resultant_force_and_torque_on_object(self, obj: Object) -> Tuple[List[float], List[float]]: """ - Request the contact data of an object, this includes the object names, the contact forces/torques, and the - contact points. + Get the resultant force and torque on the object. :param obj: The object. - :return: The contact data of the object as a MultiverseObjectContactData. - """ - api_response_data = self._get_all_contact_data_of_object(obj.name) - body_names = api_response_data[API.GET_CONTACT_BODIES] - points = self._parse_contact_points(api_response_data[API.GET_CONTACT_POINTS]) - forces, torques = self._parse_constraint_effort(api_response_data[API.GET_CONSTRAINT_EFFORT]) - return MultiverseObjectContactData(body_names, MultiverseContactData(points, {obj.name: forces}, - {obj.name: torques})) - - def get_all_contact_data_between_objects(self, obj1: Object, obj2: Object) -> MultiverseContactData: + :return: The resultant force and torque on the object. """ - Request the contact data between two objects, this includes the contact forces/torques, and the contact points. + effort = self._request_single_api_callback(API.GET_CONSTRAINT_EFFORT, obj.name) + return self._parse_constraint_effort(effort) - :param obj1: The first object. - :param obj2: The second object. - :return: The contact data between the objects as a MultiverseContactData. + def get_contact_data_of_object(self, obj: Object) -> MultiverseObjectContactData: """ - api_response = self._get_all_contact_data_between_objects(obj1.name, obj2.name) - points = self._parse_contact_points(api_response[API.GET_CONTACT_POINTS]) - force_on_a, torque_on_a = self._parse_constraint_effort(api_response[API.GET_CONSTRAINT_EFFORT][obj1.name]) - force_on_b, torque_on_b = self._parse_constraint_effort(api_response[API.GET_CONSTRAINT_EFFORT][obj2.name]) - return MultiverseContactData(points, {obj1.name: force_on_a, obj2.name: force_on_b}, - {obj1.name: torque_on_a, obj2.name: torque_on_b}) + Request the contact data of an object, this includes the object names, and the contact points. - def _get_all_contact_data_between_objects(self, obj1_name: str, obj2_name: str) -> Dict[API, List]: - """ - Request the contact data between two objects, this includes the contact forces/torques, and the contact points. - - :param obj1_name: The name of the first object. - :param obj2_name: The name of the second object. - :return: The contact data between the objects as a dictionary. + :param obj: The object. + :return: The contact data of the object as a MultiverseObjectContactData. """ - data_1 = self._request_apis_callbacks({API.GET_CONTACT_POINTS: [obj1_name, obj2_name], - API.GET_CONSTRAINT_EFFORT: [obj1_name], - }) - data_2 = self._request_single_api_callback(API.GET_CONSTRAINT_EFFORT, obj2_name) - data_1[API.GET_CONSTRAINT_EFFORT].extend(data_2) - return data_1 + api_response_data = self._get_contact_data_of_object(obj.name) + body_names = api_response_data[API.GET_CONTACT_BODIES] + points = self._parse_contact_points(api_response_data[API.GET_CONTACT_POINTS]) + return MultiverseObjectContactData(body_names, points) - def _get_contact_points_between_objects(self, obj1_name: str, obj2_name: str) -> List[MultiverseContactPoint]: + def get_contact_points_between_objects(self, obj1_name: str, obj2_name: str) -> List[MultiverseContactPoint]: """ Request the contact points between two objects. @@ -792,9 +768,18 @@ def _parse_constraint_effort(contact_effort: List[str]) -> Tuple[List[float], Li forces, torques = contact_effort[:3], contact_effort[3:] return forces, torques - def _get_contact_points(self, obj: Object) -> List[MultiverseContactPoint]: + def get_contact_bodies_of_object(self, obj: Object) -> List[str]: + """ + Get the names of bodies/objects that are in contact with an object. + + :param obj: The object. + :return: The names of the bodies/objects that are in contact with the object. + """ + return self._request_single_api_callback(API.GET_CONTACT_BODIES, obj.name) + + def get_contact_points_of_object(self, obj: Object) -> List[MultiverseContactPoint]: """ - Get the 3d positions of the contacts of an object. + Get the 3d positions of the contacts of an object and the normal vectors at the contact points. :param obj: The object. :return: The contact points of the object as a list of MultiverseContactPoint. @@ -813,16 +798,14 @@ def _parse_contact_points(contact_points: List[str]) -> List[MultiverseContactPo contact_point_data = [list(map(float, contact_point.split())) for contact_point in contact_points] return [MultiverseContactPoint(point[:3], point[3:]) for point in contact_point_data] - def _get_all_contact_data_of_object(self, object_name) -> Dict[API, List]: + def _get_contact_data_of_object(self, object_name) -> Dict[API, List]: """ - Request the names of bodies/objects that are in contact of an object, - the contact forces and torques, and the contact points. + Request the names of bodies/objects that are in contact of an object, and the contact points. :param object_name: The name of the object. - :return: The contact bodies, contact forces and torques, and contact points as a dictionary. + :return: The contact bodies, and contact points as a dictionary. """ return self._request_apis_callbacks({API.GET_CONTACT_BODIES: [object_name], - API.GET_CONSTRAINT_EFFORT: [object_name], API.GET_CONTACT_POINTS: [object_name] }) diff --git a/test/test_multiverse.py b/test/test_multiverse.py index dd2a3c89a..6888fdf72 100644 --- a/test/test_multiverse.py +++ b/test/test_multiverse.py @@ -364,7 +364,7 @@ def test_get_object_contact_points(self): milk = self.spawn_milk([1, 1, 0.01], [0, -0.707, 0, 0.707]) contact_points = self.multiverse.get_object_contact_points(milk) self.assertIsInstance(contact_points, ContactPointsList) - self.assertEqual(len(contact_points), 1) + self.assertTrue(len(contact_points) >= 1) self.assertIsInstance(contact_points[0], ContactPoint) self.assertTrue(contact_points[0].link_b.object, self.multiverse.floor) cup = self.spawn_cup([1, 1, 0.12]) @@ -373,7 +373,7 @@ def test_get_object_contact_points(self): self.multiverse.simulate(0.3) contact_points = self.multiverse.get_object_contact_points(cup) self.assertIsInstance(contact_points, ContactPointsList) - self.assertEqual(len(contact_points), 1) + self.assertTrue(len(contact_points) >= 1) self.assertIsInstance(contact_points[0], ContactPoint) self.assertTrue(contact_points[0].link_b.object, milk) self.tearDown() @@ -387,7 +387,7 @@ def test_get_contact_points_between_two_objects(self): self.multiverse.simulate(0.3) contact_points = self.multiverse.get_contact_points_between_two_objects(milk, cup) self.assertIsInstance(contact_points, ContactPointsList) - self.assertEqual(len(contact_points), 1) + self.assertTrue(len(contact_points) >= 1) self.assertIsInstance(contact_points[0], ContactPoint) self.assertTrue(contact_points[0].link_a.object, milk) self.assertTrue(contact_points[0].link_b.object, cup) From d441ac7af16825335e045322f35011092d928e7e Mon Sep 17 00:00:00 2001 From: Abdelrhman Bassiouny Date: Thu, 17 Oct 2024 13:05:55 +0200 Subject: [PATCH 04/91] [World] Changed bounding box methods to not being abstract since there is a default implementation for them now. --- src/pycram/datastructures/world.py | 6 ++---- 1 file changed, 2 insertions(+), 4 deletions(-) diff --git a/src/pycram/datastructures/world.py b/src/pycram/datastructures/world.py index 51b7034af..e10131f0e 100644 --- a/src/pycram/datastructures/world.py +++ b/src/pycram/datastructures/world.py @@ -886,14 +886,13 @@ def get_colors_of_object_links(self, obj: Object) -> Dict[str, Color]: """ pass - @abstractmethod def get_object_axis_aligned_bounding_box(self, obj: Object) -> AxisAlignedBoundingBox: """ :param obj: The object for which the bounding box should be returned. :return: the axis aligned bounding box of this object. The return of this method are two points in world coordinate frame which define a bounding box. """ - pass + raise NotImplementedError def get_object_rotated_bounding_box(self, obj: Object) -> RotatedBoundingBox: """ @@ -903,14 +902,13 @@ def get_object_rotated_bounding_box(self, obj: Object) -> RotatedBoundingBox: """ raise NotImplementedError - @abstractmethod def get_link_axis_aligned_bounding_box(self, link: Link) -> AxisAlignedBoundingBox: """ :param link: The link for which the bounding box should be returned. :return: The axis aligned bounding box of the link. The return of this method are two points in world coordinate frame which define a bounding box. """ - pass + raise NotImplementedError def get_link_rotated_bounding_box(self, link: Link) -> RotatedBoundingBox: """ From c7472f51b4e4327c95aee0acf0d6ca58fd36eca6 Mon Sep 17 00:00:00 2001 From: Abdelrhman Bassiouny Date: Tue, 22 Oct 2024 13:05:31 +0200 Subject: [PATCH 05/91] [Multiverse] create object from generic_description. --- src/pycram/worlds/multiverse.py | 12 +++++++++++- test/test_multiverse.py | 13 ++++++++++++- 2 files changed, 23 insertions(+), 2 deletions(-) diff --git a/src/pycram/worlds/multiverse.py b/src/pycram/worlds/multiverse.py index 0fd2a52d9..ef0da3f94 100644 --- a/src/pycram/worlds/multiverse.py +++ b/src/pycram/worlds/multiverse.py @@ -1,4 +1,5 @@ import logging +import os from time import sleep import numpy as np @@ -15,7 +16,8 @@ from ..datastructures.pose import Pose from ..datastructures.world import World from ..description import Link, Joint -from ..object_descriptors.mjcf import ObjectDescription as MJCF +from ..object_descriptors.mjcf import ObjectDescription as MJCF, ObjectFactory, PrimitiveObjectFactory +from ..object_descriptors.generic import ObjectDescription as GenericObjectDescription from ..robot_description import RobotDescription from ..ros.logging import logwarn, logerr from ..utils import RayTestUtils, wxyz_to_xyzw, xyzw_to_wxyz @@ -154,6 +156,14 @@ def _spawn_floor(self): self.floor = Object("floor", ObjectType.ENVIRONMENT, "plane.urdf", world=self) + def load_generic_object_and_get_id(self, description: GenericObjectDescription, + pose: Optional[Pose] = None) -> int: + save_path = os.path.join(self.cache_manager.cache_dir, description.name + ".xml") + object_factory = PrimitiveObjectFactory(description.name, description.links[0].geometry, save_path) + object_factory.build_shape() + object_factory.export_to_mjcf(save_path) + return self.load_object_and_get_id(description.name, pose, ObjectType.GENERIC_OBJECT) + def get_images_for_target(self, target_pose: Pose, cam_pose: Pose, size: int = 256, diff --git a/test/test_multiverse.py b/test/test_multiverse.py index 6888fdf72..dd096ec4f 100644 --- a/test/test_multiverse.py +++ b/test/test_multiverse.py @@ -7,13 +7,14 @@ from tf.transformations import quaternion_from_euler, quaternion_multiply from typing_extensions import Optional, List -from pycram.datastructures.dataclasses import ContactPointsList, ContactPoint, AxisAlignedBoundingBox +from pycram.datastructures.dataclasses import ContactPointsList, ContactPoint, AxisAlignedBoundingBox, Color from pycram.datastructures.enums import ObjectType, Arms, JointType from pycram.datastructures.pose import Pose from pycram.robot_description import RobotDescriptionManager from pycram.world_concepts.world_object import Object from pycram.validation.error_checkers import calculate_angle_between_quaternions from pycram.helper import get_robot_mjcf_path, parse_mjcf_actuators +from pycram.object_descriptors.generic import ObjectDescription as GenericObjectDescription multiverse_installed = True try: @@ -53,6 +54,16 @@ def tearDownClass(cls): def tearDown(self): self.multiverse.remove_all_objects() + def test_load_generic_object(self): + obj_desc = GenericObjectDescription('test_cube', [0, 0, 0], [0.1, 0.1, 0.1], + color=Color(1, 0, 0, 1)) + obj = Object(obj_desc.name, ObjectType.GENERIC_OBJECT, description=obj_desc) + self.assertIsInstance(obj, Object) + self.assertTrue(obj in self.multiverse.objects) + obj.set_position([1, 1, 0.1]) + pose = obj.get_pose() + self.assert_poses_are_equal(pose, Pose([1, 1, 0.1], [0, 0, 0, 1])) + def test_save_and_restore_state(self): milk = self.spawn_milk([1, 1, 0.1]) robot = self.spawn_robot() From 20a31cb722951ade4618f747a24fa8e20dd9f550 Mon Sep 17 00:00:00 2001 From: Abdelrhman Bassiouny Date: Tue, 22 Oct 2024 19:30:22 +0200 Subject: [PATCH 06/91] [ObjectType] Added Imagined Surface as an object type. --- src/pycram/datastructures/enums.py | 1 + 1 file changed, 1 insertion(+) diff --git a/src/pycram/datastructures/enums.py b/src/pycram/datastructures/enums.py index 699ff1aff..1c38b5bc5 100644 --- a/src/pycram/datastructures/enums.py +++ b/src/pycram/datastructures/enums.py @@ -68,6 +68,7 @@ class ObjectType(int, Enum): ENVIRONMENT = auto() GENERIC_OBJECT = auto() HUMAN = auto() + IMAGINED_SURFACE = auto() class State(int, Enum): From 65ab19077a0aadd1d6b36e0e3a4401a74f57248d Mon Sep 17 00:00:00 2001 From: Abdelrhman Bassiouny Date: Wed, 23 Oct 2024 11:31:49 +0200 Subject: [PATCH 07/91] [Multiverse] corrected multiple removal of same simulator state. --- src/pycram/datastructures/world.py | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/src/pycram/datastructures/world.py b/src/pycram/datastructures/world.py index e10131f0e..d993dd237 100644 --- a/src/pycram/datastructures/world.py +++ b/src/pycram/datastructures/world.py @@ -1262,8 +1262,9 @@ def remove_saved_states(self) -> None: Remove all saved states of the World. """ if self.conf.use_physics_simulator_state: - for state in self.saved_states.values(): - self.remove_physics_simulator_state(state.simulator_state_id) + simulator_state_ids = set([state.simulator_state_id for state in self.saved_states.values()]) + for ssid in simulator_state_ids: + self.remove_physics_simulator_state(ssid) else: self.remove_objects_saved_states() super().remove_saved_states() From af8a51df3e890e062bd23f439ce26a14e80e3e35 Mon Sep 17 00:00:00 2001 From: Abdelrhman Bassiouny Date: Fri, 25 Oct 2024 18:41:43 +0200 Subject: [PATCH 08/91] [WorldSync] pause and resume world sync is not used anymore as the sync function is called directly from with UseProspectionWorld. --- src/pycram/datastructures/world.py | 9 +++------ 1 file changed, 3 insertions(+), 6 deletions(-) diff --git a/src/pycram/datastructures/world.py b/src/pycram/datastructures/world.py index d993dd237..fd21dfc65 100644 --- a/src/pycram/datastructures/world.py +++ b/src/pycram/datastructures/world.py @@ -313,7 +313,6 @@ def _sync_prospection_world(self): self.world_sync = None else: self.world_sync: WorldSync = WorldSync(self, self.prospection_world) - self.pause_world_sync() self.world_sync.start() def preprocess_object_file_and_get_its_cache_path(self, path: str, ignore_cached_files: bool, @@ -1022,7 +1021,6 @@ def terminate_world_sync(self) -> None: Terminate the world sync thread. """ self.world_sync.terminate = True - self.resume_world_sync() self.world_sync.join() def save_state(self, state_id: Optional[int] = None, use_same_id: bool = False) -> int: @@ -1560,7 +1558,8 @@ def resume_world_sync(self) -> None: """ Resume the world synchronization. """ - self.world_sync.sync_lock.release() + if self.world_sync.sync_lock.locked(): + self.world_sync.sync_lock.release() def add_vis_axis(self, pose: Pose) -> int: """ @@ -1704,10 +1703,8 @@ def run(self): """ while not self.terminate: self.sync_lock.acquire() - if not self.terminate: - self.sync_worlds() - self.sync_lock.release() time.sleep(WorldSync.WAIT_TIME_AS_N_SIMULATION_STEPS * self.world.simulation_time_step) + self.sync_lock.release() def get_world_object(self, prospection_object: Object) -> Object: """ From b78d078718bd0c228e24a8fbfdc7df44e2526b0a Mon Sep 17 00:00:00 2001 From: A_Bassiouny Date: Mon, 28 Oct 2024 13:09:48 +0100 Subject: [PATCH 09/91] [MultiverseFallschoolDemo] Created a demo for the fallschool for multiverse with pycram. --- .../pycram_multiverse_demo/fallschool_demo.py | 49 +++++++++++++++++++ 1 file changed, 49 insertions(+) diff --git a/demos/pycram_multiverse_demo/fallschool_demo.py b/demos/pycram_multiverse_demo/fallschool_demo.py index e69de29bb..8d924a4dc 100644 --- a/demos/pycram_multiverse_demo/fallschool_demo.py +++ b/demos/pycram_multiverse_demo/fallschool_demo.py @@ -0,0 +1,49 @@ +from pycram.datastructures.dataclasses import Color +from pycram.datastructures.enums import ObjectType, Arms +from pycram.datastructures.pose import Pose +from pycram.designators.action_designator import ParkArmsAction, MoveTorsoAction, TransportAction, NavigateAction, \ + LookAtAction, DetectAction +from pycram.designators.object_designator import BelieveObject +from pycram.process_module import simulated_robot, with_simulated_robot +from pycram.world_concepts.world_object import Object +from pycram.worlds.multiverse import Multiverse + + +@with_simulated_robot +def move_and_detect(obj_type: ObjectType, pick_pose: Pose): + NavigateAction(target_locations=[Pose([1.7, 2, 0])]).resolve().perform() + + LookAtAction(targets=[pick_pose]).resolve().perform() + + object_desig = DetectAction(BelieveObject(types=[obj_type])).resolve().perform() + + return object_desig + +world = Multiverse() +robot = Object('pr2', ObjectType.ROBOT, f'pr2.urdf', pose=Pose([1.3, 2, 0.01])) +apartment = Object("apartment", ObjectType.ENVIRONMENT, f"apartment.urdf") + +milk = Object("milk", ObjectType.MILK, f"milk.stl", pose=Pose([2.4, 2, 1.02]), + color=Color(1, 0, 0, 1)) + +robot_desig = BelieveObject(names=[robot.name]) +apartment_desig = BelieveObject(names=[apartment.name]) + +with simulated_robot: + + # Transport the milk + ParkArmsAction([Arms.BOTH]).resolve().perform() + + MoveTorsoAction([0.25]).resolve().perform() + + NavigateAction(target_locations=[Pose([1.7, 2, 0])]).resolve().perform() + + LookAtAction(targets=[Pose([2.6, 2.15, 1])]).resolve().perform() + + milk_desig = DetectAction(BelieveObject(types=[milk.obj_type])).resolve().perform() + + TransportAction(milk_desig, [Arms.LEFT], [Pose([2.4, 3, 1.02])]).resolve().perform() + + ParkArmsAction([Arms.BOTH]).resolve().perform() + +world.exit() From 065a93b2a5592d0f2b78562dc9680babb7aa598a Mon Sep 17 00:00:00 2001 From: Abdelrhman Bassiouny Date: Thu, 17 Oct 2024 13:06:50 +0200 Subject: [PATCH 10/91] [euROBIN] Added ur5e description and a demo for using it in eurobin. --- .../demo_euROBIN_industrial_robotics.py | 36 ++++++++++++ launch/ik_and_description.launch | 5 ++ .../ur5e_controlled_description.py | 55 +++++++++++++++++++ src/pycram/worlds/multiverse.py | 12 +--- 4 files changed, 98 insertions(+), 10 deletions(-) create mode 100644 demos/pycram_multiverse_demo/demo_euROBIN_industrial_robotics.py create mode 100644 src/pycram/robot_descriptions/ur5e_controlled_description.py diff --git a/demos/pycram_multiverse_demo/demo_euROBIN_industrial_robotics.py b/demos/pycram_multiverse_demo/demo_euROBIN_industrial_robotics.py new file mode 100644 index 000000000..fda93211f --- /dev/null +++ b/demos/pycram_multiverse_demo/demo_euROBIN_industrial_robotics.py @@ -0,0 +1,36 @@ +import logging +import os + +from rospkg import RosPack + +from pycram.datastructures.enums import ObjectType +from pycram.world_concepts.world_object import Object +from pycram.datastructures.pose import Pose +from pycram.ros_utils.force_torque_sensor import ForceTorqueSensor +from pycram.ros_utils.joint_state_publisher import JointStatePublisher +from pycram.worlds.multiverse import Multiverse + +SCRIPT_DIR = os.path.abspath(os.path.dirname(__file__)) +PYCRAM_DIR = os.path.join(SCRIPT_DIR, os.pardir, os.pardir) +RESOURCE_DIR = os.path.join(PYCRAM_DIR, "resources") + +SPAWNING_POSES = { + "robot": Pose([0, 0, 0], [0.0, 0.0, 0.0, 1.0]), # x,y,z,qx,qy,qz,qw + "cereal": Pose([0.5, 0.5, 2.0], [0.0, 0.0, 0.0, 1.0]) +} + + +if __name__ == '__main__': + root = logging.getLogger() + root.setLevel(logging.INFO) + + world = Multiverse(simulation_name="ur5e_with_task_board") + + # Load environment, robot and objects + rospack = RosPack() + + robot = Object("ur5e", ObjectType.ROBOT, "ur5e_with_gripper/urdf/ur5e.urdf") + jsp = JointStatePublisher() + # fts = ForceTorqueSensor("ee_fixed_joint") + + world.simulate(1) diff --git a/launch/ik_and_description.launch b/launch/ik_and_description.launch index 4484eaf86..a438ec4b4 100644 --- a/launch/ik_and_description.launch +++ b/launch/ik_and_description.launch @@ -26,6 +26,11 @@ + + + + ContactPointsList: contact_points[-1].position_on_b = point.position return contact_points - def get_object_with_body_name(self, body_name: str) -> Tuple[Optional[Object],Optional[Link]]: + def get_object_with_body_name(self, body_name: str) -> Tuple[Optional[Object], Optional[Link]]: """ Get the object with the body name in the simulator, the body name can be the name of the object or the link. @@ -654,14 +654,6 @@ def get_colors_of_object_links(self, obj: Object) -> Dict[str, Color]: logwarn("get_colors_of_object_links is not implemented in Multiverse") return {} - def get_object_axis_aligned_bounding_box(self, obj: Object) -> AxisAlignedBoundingBox: - logerr("get_object_axis_aligned_bounding_box for multi-link objects is not implemented in Multiverse") - raise NotImplementedError - - def get_link_axis_aligned_bounding_box(self, link: Link) -> AxisAlignedBoundingBox: - logerr("get_link_axis_aligned_bounding_box is not implemented in Multiverse") - raise NotImplementedError - def set_realtime(self, real_time: bool) -> None: logwarn("set_realtime is not implemented as an API in Multiverse, it is configured in the" "multiverse configuration file (.muv file) as rtf_required where a value of 1 means real-time") From fc5f933dd8144113a2522e1eaab4f78301f34865 Mon Sep 17 00:00:00 2001 From: Abdelrhman Bassiouny Date: Fri, 18 Oct 2024 13:03:54 +0200 Subject: [PATCH 11/91] [euROBIN] new process_module for gripper, accept grippers as separate pycram objects, closing and opening gripper works. --- .../demo_euROBIN_industrial_robotics.py | 19 ++++++++- launch/ik_and_description.launch | 2 +- src/pycram/datastructures/enums.py | 1 + src/pycram/datastructures/world.py | 14 +++++-- src/pycram/designator.py | 6 ++- src/pycram/helper.py | 6 +++ src/pycram/process_module.py | 4 +- src/pycram/process_modules/__init__.py | 2 + .../default_process_modules.py | 13 +++--- .../robotiq_gripper_process_module.py | 41 +++++++++++++++++++ src/pycram/robot_description.py | 12 +++++- .../robot_descriptions/pr2_description.py | 5 ++- .../ur5e_controlled_description.py | 19 ++++++--- src/pycram/worlds/bullet_world.py | 10 +++-- src/pycram/worlds/multiverse.py | 11 +++-- 15 files changed, 134 insertions(+), 31 deletions(-) create mode 100644 src/pycram/process_modules/robotiq_gripper_process_module.py diff --git a/demos/pycram_multiverse_demo/demo_euROBIN_industrial_robotics.py b/demos/pycram_multiverse_demo/demo_euROBIN_industrial_robotics.py index fda93211f..7ed898df3 100644 --- a/demos/pycram_multiverse_demo/demo_euROBIN_industrial_robotics.py +++ b/demos/pycram_multiverse_demo/demo_euROBIN_industrial_robotics.py @@ -3,12 +3,14 @@ from rospkg import RosPack -from pycram.datastructures.enums import ObjectType +from pycram.datastructures.enums import ObjectType, GripperState, Arms +from pycram.process_module import simulated_robot, real_robot from pycram.world_concepts.world_object import Object from pycram.datastructures.pose import Pose from pycram.ros_utils.force_torque_sensor import ForceTorqueSensor from pycram.ros_utils.joint_state_publisher import JointStatePublisher from pycram.worlds.multiverse import Multiverse +from pycram.designators.action_designator import SetGripperAction SCRIPT_DIR = os.path.abspath(os.path.dirname(__file__)) PYCRAM_DIR = os.path.join(SCRIPT_DIR, os.pardir, os.pardir) @@ -20,6 +22,15 @@ } +def spawn_ur5e_with_gripper(): + robot = Object("ur5e", ObjectType.ROBOT, "universal_robot/ur5e/urdf/ur5e.urdf") + gripper = Object("gripper-2F-85", ObjectType.GRIPPER, "robotiq/gripper-2F-85/gripper-2F-85.urdf") + wrist_3_tf_frame = robot.get_link_tf_frame("wrist_3_link") + gripper.set_pose(Pose([0, 0.1, 0], [1.0, 0.0, 0.0, -1.0], frame=wrist_3_tf_frame)) + robot.attach(gripper, parent_link="wrist_3_link") + return robot, gripper + + if __name__ == '__main__': root = logging.getLogger() root.setLevel(logging.INFO) @@ -29,8 +40,12 @@ # Load environment, robot and objects rospack = RosPack() - robot = Object("ur5e", ObjectType.ROBOT, "ur5e_with_gripper/urdf/ur5e.urdf") + robot, gripper = spawn_ur5e_with_gripper() + jsp = JointStatePublisher() # fts = ForceTorqueSensor("ee_fixed_joint") + robot_arms = [chain.arm_type for chain in robot.robot_description.get_manipulator_chains()] + with real_robot: + SetGripperAction(robot_arms, [GripperState.CLOSE]).resolve().perform() world.simulate(1) diff --git a/launch/ik_and_description.launch b/launch/ik_and_description.launch index a438ec4b4..52565a7fa 100644 --- a/launch/ik_and_description.launch +++ b/launch/ik_and_description.launch @@ -29,7 +29,7 @@ + textfile="$(find multiverse_control)../../../../multiverse/resources/robots/universal_robot/ur5e/urdf/ur5e.urdf"/> diff --git a/src/pycram/datastructures/enums.py b/src/pycram/datastructures/enums.py index 1c38b5bc5..0249d7f5e 100644 --- a/src/pycram/datastructures/enums.py +++ b/src/pycram/datastructures/enums.py @@ -65,6 +65,7 @@ class ObjectType(int, Enum): BREAKFAST_CEREAL = auto() JEROEN_CUP = auto() ROBOT = auto() + GRIPPER = auto() ENVIRONMENT = auto() GENERIC_OBJECT = auto() HUMAN = auto() diff --git a/src/pycram/datastructures/world.py b/src/pycram/datastructures/world.py index fd21dfc65..c4f3ed73f 100644 --- a/src/pycram/datastructures/world.py +++ b/src/pycram/datastructures/world.py @@ -612,7 +612,8 @@ def get_object_link_names(self, obj: Object) -> List[str]: """ pass - def simulate(self, seconds: float, real_time: Optional[bool] = False) -> None: + def simulate(self, seconds: float, real_time: Optional[bool] = False, + func: Optional[Callable[[], None]] = None) -> None: """ Simulate Physics in the World for a given amount of seconds. Usually this simulation is faster than real time. By setting the 'real_time' parameter this simulation is slowed down such that the simulated time is equal @@ -620,11 +621,12 @@ def simulate(self, seconds: float, real_time: Optional[bool] = False) -> None: :param seconds: The amount of seconds that should be simulated. :param real_time: If the simulation should happen in real time or faster. + :param func: A function that should be called during the simulation """ self.set_realtime(real_time) for i in range(0, int(seconds * self.conf.simulation_frequency)): curr_time = Time().now() - self.step() + self.step(func) for objects, callbacks in self.coll_callbacks.items(): contact_points = self.get_contact_points_between_two_objects(objects[0], objects[1]) if len(contact_points) > 0: @@ -843,9 +845,13 @@ def reset_multiple_objects_base_poses(self, objects: Dict[Object, Pose]) -> bool pass @abstractmethod - def step(self): + def step(self, func: Optional[Callable[[], None]] = None, step_seconds: Optional[float] = None) -> None: """ - Step the world simulation using forward dynamics + Step the world simulation using forward dynamics. + + :param func: An optional function to be called during the step. + :param step_seconds: The amount of seconds to step the simulation if None the simulation is stepped by the + simulation time step. """ pass diff --git a/src/pycram/designator.py b/src/pycram/designator.py index 6ccb0fead..fac5a6a7c 100644 --- a/src/pycram/designator.py +++ b/src/pycram/designator.py @@ -178,8 +178,10 @@ class Action: def __post_init__(self): self.robot_position = World.robot.get_pose() - self.robot_torso_height = World.robot.get_joint_position( - RobotDescription.current_robot_description.torso_joint) + if RobotDescription.current_robot_description.torso_joint != "": + self.robot_torso_height = World.robot.get_joint_position(RobotDescription.current_robot_description.torso_joint) + else: + self.robot_torso_height = 0.0 self.robot_type = World.robot.obj_type def perform(self) -> Any: diff --git a/src/pycram/helper.py b/src/pycram/helper.py index 08ae416f7..fa5aface7 100644 --- a/src/pycram/helper.py +++ b/src/pycram/helper.py @@ -47,6 +47,12 @@ def parse_mjcf_actuators(file_path: str) -> Dict[str, str]: return joint_actuators +def get_robot_urdf_path_from_multiverse(relative_dir: str, robot_name: str, resources_dir: Optional[str] = None) -> str: + if resources_dir is None: + resources_dir = find_multiverse_resources_path() + return os.path.join(resources_dir, 'robots', relative_dir, robot_name, f'urdf/{robot_name}.urdf') + + def get_robot_mjcf_path(robot_relative_dir: str, robot_name: str, xml_name: Optional[str] = None) -> Optional[str]: """ Get the path to the MJCF file of a robot. diff --git a/src/pycram/process_module.py b/src/pycram/process_module.py index 957d0f5e9..cd7d65090 100644 --- a/src/pycram/process_module.py +++ b/src/pycram/process_module.py @@ -285,8 +285,10 @@ def get_manager() -> Union[ProcessModuleManager, None]: f"No execution_type is set, did you use the with_simulated_robot or with_real_robot decorator?") return + robot_description = RobotDescription.current_robot_description for pm_manager in ProcessModuleManager.available_pms: - if pm_manager.robot_name == RobotDescription.current_robot_description.name: + if pm_manager.robot_name == robot_description.name or\ + ((pm_manager.robot_name == robot_description.gripper_name) and robot_description.gripper_name): manager = pm_manager if pm_manager.robot_name == "default": _default_manager = pm_manager diff --git a/src/pycram/process_modules/__init__.py b/src/pycram/process_modules/__init__.py index caa3dfe41..c8ca456c9 100644 --- a/src/pycram/process_modules/__init__.py +++ b/src/pycram/process_modules/__init__.py @@ -4,6 +4,7 @@ from .hsrb_process_modules import HSRBManager from .default_process_modules import DefaultManager from .stretch_process_modules import StretchManager +from .robotiq_gripper_process_module import RobotiqManager from .tiago_process_modules import TiagoManager Pr2Manager() @@ -13,3 +14,4 @@ DefaultManager() StretchManager() TiagoManager() +RobotiqManager() diff --git a/src/pycram/process_modules/default_process_modules.py b/src/pycram/process_modules/default_process_modules.py index b8df93b52..5fd8e7327 100644 --- a/src/pycram/process_modules/default_process_modules.py +++ b/src/pycram/process_modules/default_process_modules.py @@ -67,16 +67,19 @@ def _execute(self, desig: LookingMotion): class DefaultMoveGripper(ProcessModule): """ This process module controls the gripper of the robot. They can either be opened or closed. - Furthermore, it can only moved one gripper at a time. + Furthermore, it can only move one gripper at a time. """ def _execute(self, desig: MoveGripperMotion): - robot = World.robot + robot_description = RobotDescription.current_robot_description + if robot_description.gripper_name is not None: + robot = World.current_world.get_object_by_name(robot_description.gripper_name) + else: + robot = World.robot gripper = desig.gripper motion = desig.motion - for joint, state in RobotDescription.current_robot_description.get_arm_chain(gripper).get_static_gripper_state( - motion).items(): - robot.set_joint_position(joint, state) + arm_chain = robot_description.get_arm_chain(gripper) + robot.set_multiple_joint_positions(arm_chain.get_static_gripper_state(motion)) class DefaultDetecting(ProcessModule): diff --git a/src/pycram/process_modules/robotiq_gripper_process_module.py b/src/pycram/process_modules/robotiq_gripper_process_module.py new file mode 100644 index 000000000..c28a3a45d --- /dev/null +++ b/src/pycram/process_modules/robotiq_gripper_process_module.py @@ -0,0 +1,41 @@ +from threading import Lock + +import rospy +from std_msgs.msg import Float64 +from typing_extensions import Any + +from .default_process_modules import DefaultMoveGripper +from ..datastructures.world import World +from ..datastructures.enums import GripperState, ExecutionType +from ..designators.motion_designator import MoveGripperMotion +from ..process_module import ProcessModule, ProcessModuleManager +from ..robot_description import RobotDescription + +GRIPPER_NAME = "gripper-2F-85" +GRIPPER_CMD_TOPIC = "/gripper_command" +OPEN_VALUE = 0.0 +CLOSE_VALUE = 255.0 + + +class RobotiqMoveGripperReal(ProcessModule): + """ + Opens or closes the gripper of the real Robotiq gripper, uses a topic for this instead of giskard + """ + def _execute(self, designator: MoveGripperMotion) -> Any: + value = OPEN_VALUE if designator.motion == GripperState.OPEN else CLOSE_VALUE + publisher = rospy.Publisher(GRIPPER_CMD_TOPIC, Float64, queue_size=10, latch=True) + World.current_world.step(func=lambda: publisher.publish(Float64(value))) + return True + + +class RobotiqManager(ProcessModuleManager): + + def __init__(self): + super().__init__(GRIPPER_NAME) + self._move_gripper_lock = Lock() + + def move_gripper(self): + if ProcessModuleManager.execution_type == ExecutionType.SIMULATED: + return DefaultMoveGripper(self._move_gripper_lock) + elif ProcessModuleManager.execution_type == ExecutionType.REAL: + return RobotiqMoveGripperReal(self._move_gripper_lock) diff --git a/src/pycram/robot_description.py b/src/pycram/robot_description.py index 495267a3a..681ba7c36 100644 --- a/src/pycram/robot_description.py +++ b/src/pycram/robot_description.py @@ -114,11 +114,17 @@ class RobotDescription: virtual_mobile_base_joints: Optional[VirtualMobileBaseJoints] = None """ Virtual mobile base joint names for mobile robots, these joints are not part of the URDF, however they are used to - move the robot in the simulation (e.g. set_pose for the robot would actually move these joints) + move the robot in the simulation (e.g. set_pose for the robot would actually move these joints) + """ + gripper_name: Optional[str] = None + """ + Name of the gripper of the robot if it has one, this is used when the gripper is a different Object with its own + description file outside the robot description file. """ def __init__(self, name: str, base_link: str, torso_link: str, torso_joint: str, urdf_path: str, - virtual_mobile_base_joints: Optional[VirtualMobileBaseJoints] = None, mjcf_path: Optional[str] = None): + virtual_mobile_base_joints: Optional[VirtualMobileBaseJoints] = None, mjcf_path: Optional[str] = None, + gripper_name: Optional[str] = None): """ Initialize the RobotDescription. The URDF is loaded from the given path and used as basis for the kinematic chains. @@ -130,6 +136,7 @@ def __init__(self, name: str, base_link: str, torso_link: str, torso_joint: str, :param urdf_path: Path to the URDF file of the robot :param virtual_mobile_base_joints: Virtual mobile base joint names for mobile robots :param mjcf_path: Path to the MJCF file of the robot + :param gripper_name: Name of the gripper of the robot if it has one and is a separate Object. """ self.name = name self.base_link = base_link @@ -146,6 +153,7 @@ def __init__(self, name: str, base_link: str, torso_link: str, torso_joint: str, self.links: List[str] = [l.name for l in self.urdf_object.links] self.joints: List[str] = [j.name for j in self.urdf_object.joints] self.virtual_mobile_base_joints: Optional[VirtualMobileBaseJoints] = virtual_mobile_base_joints + self.gripper_name = gripper_name @property def has_actuators(self): diff --git a/src/pycram/robot_descriptions/pr2_description.py b/src/pycram/robot_descriptions/pr2_description.py index 35ee28838..d532123ac 100644 --- a/src/pycram/robot_descriptions/pr2_description.py +++ b/src/pycram/robot_descriptions/pr2_description.py @@ -10,8 +10,9 @@ mjcf_filename = get_robot_mjcf_path("", "pr2") -pr2_description = RobotDescription("pr2", "base_link", "torso_lift_link", "torso_lift_joint", - filename, virtual_mobile_base_joints=VirtualMobileBaseJoints(), mjcf_path=mjcf_filename) +pr2_description = RobotDescription("pr2", "base_link", "torso_lift_link", + "torso_lift_joint", filename, + virtual_mobile_base_joints=VirtualMobileBaseJoints(), mjcf_path=mjcf_filename) ################################## Left Arm ################################## left_arm = KinematicChainDescription("left", "torso_lift_link", "l_wrist_roll_link", diff --git a/src/pycram/robot_descriptions/ur5e_controlled_description.py b/src/pycram/robot_descriptions/ur5e_controlled_description.py index e0f39cd57..5056d1e6c 100644 --- a/src/pycram/robot_descriptions/ur5e_controlled_description.py +++ b/src/pycram/robot_descriptions/ur5e_controlled_description.py @@ -1,18 +1,21 @@ import os from ..datastructures.enums import Arms, GripperState -from ..helper import get_robot_mjcf_path, find_multiverse_resources_path +from ..helper import get_robot_mjcf_path, find_multiverse_resources_path, get_robot_urdf_path_from_multiverse from ..robot_description import RobotDescription, KinematicChainDescription, EndEffectorDescription, \ RobotDescriptionManager +from ..object_descriptors.urdf import ObjectDescription as URDFObject + multiverse_resources = find_multiverse_resources_path() robot_relative_dir = "universal_robot" robot_name = "ur5e" -filename = os.path.join(multiverse_resources, 'robots', robot_relative_dir, 'ur5e_with_gripper', 'urdf/ur5e.urdf') -mjcf_filename = get_robot_mjcf_path(robot_relative_dir, 'ur5e') +gripper_name = "gripper-2F-85" +filename = get_robot_urdf_path_from_multiverse(robot_relative_dir, robot_name, resources_dir=multiverse_resources) +mjcf_filename = get_robot_mjcf_path(robot_relative_dir, robot_name) ur5_description = RobotDescription(robot_name, "base_link", "", "", - filename, mjcf_path=mjcf_filename) + filename, mjcf_path=mjcf_filename, gripper_name=gripper_name) ################################## Arm ################################## arm = KinematicChainDescription("manipulator", "base_link", "wrist_3_link", @@ -28,8 +31,12 @@ ur5_description.add_kinematic_chain_description(arm) ################################## Gripper ################################## -gripper = EndEffectorDescription("gripper", "gripper_2F_85", "right_pad", - ur5_description.urdf_object) +gripper_relative_dir = "robotiq" +gripper_filename = get_robot_urdf_path_from_multiverse(gripper_relative_dir, gripper_name, + resources_dir=multiverse_resources) +gripper_urdf_obj = URDFObject(gripper_filename) +gripper = EndEffectorDescription("gripper", gripper_name, "right_pad", + gripper_urdf_obj) gripper.add_static_joint_states(GripperState.OPEN, {'right_driver_joint': 0.0, 'right_coupler_joint': 0.0, diff --git a/src/pycram/worlds/bullet_world.py b/src/pycram/worlds/bullet_world.py index c3a3b5f86..3c875c708 100755 --- a/src/pycram/worlds/bullet_world.py +++ b/src/pycram/worlds/bullet_world.py @@ -8,7 +8,7 @@ import numpy as np import pycram_bullet as p from geometry_msgs.msg import Point -from typing_extensions import List, Optional, Dict, Any +from typing_extensions import List, Optional, Dict, Any, Callable from pycrap import Floor from ..datastructures.dataclasses import Color, AxisAlignedBoundingBox, MultiBody, VisualShape, BoxVisualShape, \ @@ -18,7 +18,7 @@ from ..datastructures.world import World from ..object_descriptors.generic import ObjectDescription as GenericObjectDescription from ..object_descriptors.urdf import ObjectDescription -from ..ros.logging import loginfo +from ..ros.logging import logwarn, loginfo from ..validation.goal_validator import (validate_multiple_joint_positions, validate_joint_position, validate_object_pose, validate_multiple_object_poses) from ..world_concepts.constraints import Constraint @@ -245,7 +245,11 @@ def _set_object_pose_by_id(self, obj_id: int, pose: Pose) -> bool: physicsClientId=self.id) return True - def step(self): + def step(self, func: Optional[Callable[[], None]] = None, step_seconds: Optional[float] = None) -> None: + if func is not None: + logwarn("The BulletWorld step function does not support a function argument.") + if step_seconds is not None: + logwarn("The BulletWorld step function does not support a step_seconds argument.") p.stepSimulation(physicsClientId=self.id) def get_multiple_object_poses(self, objects: List[Object]) -> Dict[str, Pose]: diff --git a/src/pycram/worlds/multiverse.py b/src/pycram/worlds/multiverse.py index 6c1607000..b905434fb 100644 --- a/src/pycram/worlds/multiverse.py +++ b/src/pycram/worlds/multiverse.py @@ -4,7 +4,7 @@ import numpy as np from tf.transformations import quaternion_matrix -from typing_extensions import List, Dict, Optional, Union, Tuple +from typing_extensions import List, Dict, Optional, Union, Tuple, Callable from .multiverse_communication.client_manager import MultiverseClientManager from .multiverse_communication.clients import MultiverseController, MultiverseReader, MultiverseWriter, MultiverseAPI @@ -620,13 +620,18 @@ def ray_test_batch(self, from_positions: List[List[float]], else: return results - def step(self): + def step(self, func: Optional[Callable[[], None]] = None, step_seconds: Optional[float] = None) -> None: """ Perform a simulation step in the simulator, this is useful when use_static_mode is True. + + :param func: A function to be called after the simulation step. + :param step_seconds: The number of seconds to step the simulation. """ if self.conf.use_static_mode: self.api_requester.unpause_simulation() - sleep(self.simulation_time_step) + if func is not None: + func() + sleep(self.simulation_time_step if step_seconds is None else step_seconds) self.api_requester.pause_simulation() def save_physics_simulator_state(self, state_id: Optional[int] = None, use_same_id: bool = False) -> int: From 80b5b2e55029f0a4635f43ea485bf4411daac8e0 Mon Sep 17 00:00:00 2001 From: Abdelrhman Bassiouny Date: Fri, 18 Oct 2024 17:26:18 +0200 Subject: [PATCH 12/91] [euROBIN] changed gripper_name to be in end_effector_description instead of robot_description. --- config/multiverse_conf.py | 2 +- .../demo_euROBIN_industrial_robotics.py | 42 ++----- src/pycram/helper.py | 5 +- src/pycram/process_module.py | 6 +- .../default_process_modules.py | 8 +- .../robotiq_gripper_process_module.py | 4 +- src/pycram/robot_description.py | 21 ++-- .../ur5e_controlled_description.py | 108 +++++++++--------- 8 files changed, 91 insertions(+), 105 deletions(-) diff --git a/config/multiverse_conf.py b/config/multiverse_conf.py index 421e30828..5014d361d 100644 --- a/config/multiverse_conf.py +++ b/config/multiverse_conf.py @@ -63,7 +63,7 @@ class MultiverseConfig(WorldConfig): Whether to use the physics simulator state when restoring or saving the world state. """ - clear_cache_at_start = False + clear_cache_at_start = True let_pycram_move_attached_objects = False let_pycram_handle_spawning = False diff --git a/demos/pycram_multiverse_demo/demo_euROBIN_industrial_robotics.py b/demos/pycram_multiverse_demo/demo_euROBIN_industrial_robotics.py index 7ed898df3..cd6a85a58 100644 --- a/demos/pycram_multiverse_demo/demo_euROBIN_industrial_robotics.py +++ b/demos/pycram_multiverse_demo/demo_euROBIN_industrial_robotics.py @@ -1,51 +1,29 @@ -import logging -import os - -from rospkg import RosPack - from pycram.datastructures.enums import ObjectType, GripperState, Arms from pycram.process_module import simulated_robot, real_robot from pycram.world_concepts.world_object import Object from pycram.datastructures.pose import Pose -from pycram.ros_utils.force_torque_sensor import ForceTorqueSensor -from pycram.ros_utils.joint_state_publisher import JointStatePublisher from pycram.worlds.multiverse import Multiverse from pycram.designators.action_designator import SetGripperAction -SCRIPT_DIR = os.path.abspath(os.path.dirname(__file__)) -PYCRAM_DIR = os.path.join(SCRIPT_DIR, os.pardir, os.pardir) -RESOURCE_DIR = os.path.join(PYCRAM_DIR, "resources") - -SPAWNING_POSES = { - "robot": Pose([0, 0, 0], [0.0, 0.0, 0.0, 1.0]), # x,y,z,qx,qy,qz,qw - "cereal": Pose([0.5, 0.5, 2.0], [0.0, 0.0, 0.0, 1.0]) -} +if __name__ == '__main__': + # Create a new world + world = Multiverse(simulation_name="ur5e_with_task_board") -def spawn_ur5e_with_gripper(): + # Load the robot and the gripper robot = Object("ur5e", ObjectType.ROBOT, "universal_robot/ur5e/urdf/ur5e.urdf") gripper = Object("gripper-2F-85", ObjectType.GRIPPER, "robotiq/gripper-2F-85/gripper-2F-85.urdf") + + # Attach the gripper to the robot at the wrist_3_link with the correct pose wrist_3_tf_frame = robot.get_link_tf_frame("wrist_3_link") gripper.set_pose(Pose([0, 0.1, 0], [1.0, 0.0, 0.0, -1.0], frame=wrist_3_tf_frame)) robot.attach(gripper, parent_link="wrist_3_link") - return robot, gripper - -if __name__ == '__main__': - root = logging.getLogger() - root.setLevel(logging.INFO) - - world = Multiverse(simulation_name="ur5e_with_task_board") - - # Load environment, robot and objects - rospack = RosPack() - - robot, gripper = spawn_ur5e_with_gripper() - - jsp = JointStatePublisher() - # fts = ForceTorqueSensor("ee_fixed_joint") + # Get the robot arms robot_arms = [chain.arm_type for chain in robot.robot_description.get_manipulator_chains()] + + # Perform the plan with real_robot: SetGripperAction(robot_arms, [GripperState.CLOSE]).resolve().perform() - world.simulate(1) + world.exit() diff --git a/src/pycram/helper.py b/src/pycram/helper.py index fa5aface7..a396f5b5b 100644 --- a/src/pycram/helper.py +++ b/src/pycram/helper.py @@ -53,7 +53,8 @@ def get_robot_urdf_path_from_multiverse(relative_dir: str, robot_name: str, reso return os.path.join(resources_dir, 'robots', relative_dir, robot_name, f'urdf/{robot_name}.urdf') -def get_robot_mjcf_path(robot_relative_dir: str, robot_name: str, xml_name: Optional[str] = None) -> Optional[str]: +def get_robot_mjcf_path(robot_relative_dir: str, robot_name: str, xml_name: Optional[str] = None, + multiverse_resources: Optional[str] = None) -> Optional[str]: """ Get the path to the MJCF file of a robot. @@ -62,10 +63,10 @@ def get_robot_mjcf_path(robot_relative_dir: str, robot_name: str, xml_name: Opti :param xml_name: The name of the XML file of the robot. :return: The path to the MJCF file of the robot if it exists, otherwise None. """ + multiverse_resources = find_multiverse_resources_path() if multiverse_resources is None else multiverse_resources xml_name = xml_name if xml_name is not None else robot_name if '.xml' not in xml_name: xml_name = xml_name + '.xml' - multiverse_resources = find_multiverse_resources_path() try: robot_folder = os.path.join(multiverse_resources, 'robots', robot_relative_dir, robot_name) except TypeError: diff --git a/src/pycram/process_module.py b/src/pycram/process_module.py index cd7d65090..c696a68fc 100644 --- a/src/pycram/process_module.py +++ b/src/pycram/process_module.py @@ -286,9 +286,13 @@ def get_manager() -> Union[ProcessModuleManager, None]: return robot_description = RobotDescription.current_robot_description + chains = robot_description.get_manipulator_chains() + gripper_name = [chain.end_effector.gripper_object_name for chain in chains + if chain.end_effector.gripper_object_name] + gripper_name = gripper_name[0] if len(gripper_name) > 0 else None for pm_manager in ProcessModuleManager.available_pms: if pm_manager.robot_name == robot_description.name or\ - ((pm_manager.robot_name == robot_description.gripper_name) and robot_description.gripper_name): + ((pm_manager.robot_name == gripper_name) and gripper_name): manager = pm_manager if pm_manager.robot_name == "default": _default_manager = pm_manager diff --git a/src/pycram/process_modules/default_process_modules.py b/src/pycram/process_modules/default_process_modules.py index 5fd8e7327..29148e4aa 100644 --- a/src/pycram/process_modules/default_process_modules.py +++ b/src/pycram/process_modules/default_process_modules.py @@ -72,13 +72,13 @@ class DefaultMoveGripper(ProcessModule): def _execute(self, desig: MoveGripperMotion): robot_description = RobotDescription.current_robot_description - if robot_description.gripper_name is not None: - robot = World.current_world.get_object_by_name(robot_description.gripper_name) + gripper = desig.gripper + arm_chain = robot_description.get_arm_chain(gripper) + if arm_chain.end_effector.gripper_object_name is not None: + robot = World.current_world.get_object_by_name(arm_chain.end_effector.gripper_object_name) else: robot = World.robot - gripper = desig.gripper motion = desig.motion - arm_chain = robot_description.get_arm_chain(gripper) robot.set_multiple_joint_positions(arm_chain.get_static_gripper_state(motion)) diff --git a/src/pycram/process_modules/robotiq_gripper_process_module.py b/src/pycram/process_modules/robotiq_gripper_process_module.py index c28a3a45d..e685a685b 100644 --- a/src/pycram/process_modules/robotiq_gripper_process_module.py +++ b/src/pycram/process_modules/robotiq_gripper_process_module.py @@ -9,7 +9,7 @@ from ..datastructures.enums import GripperState, ExecutionType from ..designators.motion_designator import MoveGripperMotion from ..process_module import ProcessModule, ProcessModuleManager -from ..robot_description import RobotDescription + GRIPPER_NAME = "gripper-2F-85" GRIPPER_CMD_TOPIC = "/gripper_command" @@ -24,7 +24,7 @@ class RobotiqMoveGripperReal(ProcessModule): def _execute(self, designator: MoveGripperMotion) -> Any: value = OPEN_VALUE if designator.motion == GripperState.OPEN else CLOSE_VALUE publisher = rospy.Publisher(GRIPPER_CMD_TOPIC, Float64, queue_size=10, latch=True) - World.current_world.step(func=lambda: publisher.publish(Float64(value))) + World.current_world.step(func=lambda: publisher.publish(Float64(value)), step_seconds=0.1) return True diff --git a/src/pycram/robot_description.py b/src/pycram/robot_description.py index 681ba7c36..9c004a004 100644 --- a/src/pycram/robot_description.py +++ b/src/pycram/robot_description.py @@ -116,15 +116,9 @@ class RobotDescription: Virtual mobile base joint names for mobile robots, these joints are not part of the URDF, however they are used to move the robot in the simulation (e.g. set_pose for the robot would actually move these joints) """ - gripper_name: Optional[str] = None - """ - Name of the gripper of the robot if it has one, this is used when the gripper is a different Object with its own - description file outside the robot description file. - """ def __init__(self, name: str, base_link: str, torso_link: str, torso_joint: str, urdf_path: str, - virtual_mobile_base_joints: Optional[VirtualMobileBaseJoints] = None, mjcf_path: Optional[str] = None, - gripper_name: Optional[str] = None): + virtual_mobile_base_joints: Optional[VirtualMobileBaseJoints] = None, mjcf_path: Optional[str] = None): """ Initialize the RobotDescription. The URDF is loaded from the given path and used as basis for the kinematic chains. @@ -136,7 +130,6 @@ def __init__(self, name: str, base_link: str, torso_link: str, torso_joint: str, :param urdf_path: Path to the URDF file of the robot :param virtual_mobile_base_joints: Virtual mobile base joint names for mobile robots :param mjcf_path: Path to the MJCF file of the robot - :param gripper_name: Name of the gripper of the robot if it has one and is a separate Object. """ self.name = name self.base_link = base_link @@ -153,7 +146,6 @@ def __init__(self, name: str, base_link: str, torso_link: str, torso_joint: str, self.links: List[str] = [l.name for l in self.urdf_object.links] self.joints: List[str] = [j.name for j in self.urdf_object.joints] self.virtual_mobile_base_joints: Optional[VirtualMobileBaseJoints] = virtual_mobile_base_joints - self.gripper_name = gripper_name @property def has_actuators(self): @@ -638,8 +630,14 @@ class EndEffectorDescription: """ Distance the gripper can open, in cm """ + gripper_object_name: Optional[str] = None + """ + Name of the gripper of the robot if it has one, this is used when the gripper is a different Object with its own + description file outside the robot description file. + """ - def __init__(self, name: str, start_link: str, tool_frame: str, urdf_object: URDFObject): + def __init__(self, name: str, start_link: str, tool_frame: str, urdf_object: URDFObject, + gripper_object_name: Optional[str] = None): """ Initialize the EndEffectorDescription object. @@ -647,6 +645,7 @@ def __init__(self, name: str, start_link: str, tool_frame: str, urdf_object: URD :param start_link: Root link of the end effector, every link below this link in the URDF is part of the end effector :param tool_frame: Name of the tool frame link in the URDf :param urdf_object: URDF object of the robot + :param gripper_object_name: Name of the gripper if it is a separate Object outside the robot description. """ self.name: str = name self.start_link: str = start_link @@ -656,6 +655,8 @@ def __init__(self, name: str, start_link: str, tool_frame: str, urdf_object: URD self.joint_names: List[str] = [] self.static_joint_states: Dict[GripperState, Dict[str, float]] = {} self._init_links_joints() + self.gripper_object_name = gripper_object_name + def _init_links_joints(self): """ diff --git a/src/pycram/robot_descriptions/ur5e_controlled_description.py b/src/pycram/robot_descriptions/ur5e_controlled_description.py index 5056d1e6c..4936f0135 100644 --- a/src/pycram/robot_descriptions/ur5e_controlled_description.py +++ b/src/pycram/robot_descriptions/ur5e_controlled_description.py @@ -5,58 +5,60 @@ from ..robot_description import RobotDescription, KinematicChainDescription, EndEffectorDescription, \ RobotDescriptionManager from ..object_descriptors.urdf import ObjectDescription as URDFObject - +from ..ros.logging import logwarn multiverse_resources = find_multiverse_resources_path() -robot_relative_dir = "universal_robot" -robot_name = "ur5e" -gripper_name = "gripper-2F-85" -filename = get_robot_urdf_path_from_multiverse(robot_relative_dir, robot_name, resources_dir=multiverse_resources) -mjcf_filename = get_robot_mjcf_path(robot_relative_dir, robot_name) - -ur5_description = RobotDescription(robot_name, "base_link", "", "", - filename, mjcf_path=mjcf_filename, gripper_name=gripper_name) - -################################## Arm ################################## -arm = KinematicChainDescription("manipulator", "base_link", "wrist_3_link", - ur5_description.urdf_object, arm_type=Arms.RIGHT) - -arm.add_static_joint_states("home", {'shoulder_pan_joint': 3.14, - 'shoulder_lift_joint': -1.56, - 'elbow_joint': 1.58, - 'wrist_1_joint': -1.57, - 'wrist_2_joint': -1.57, - 'wrist_3_joint': 0.0}) - -ur5_description.add_kinematic_chain_description(arm) - -################################## Gripper ################################## -gripper_relative_dir = "robotiq" -gripper_filename = get_robot_urdf_path_from_multiverse(gripper_relative_dir, gripper_name, - resources_dir=multiverse_resources) -gripper_urdf_obj = URDFObject(gripper_filename) -gripper = EndEffectorDescription("gripper", gripper_name, "right_pad", - gripper_urdf_obj) - -gripper.add_static_joint_states(GripperState.OPEN, {'right_driver_joint': 0.0, - 'right_coupler_joint': 0.0, - 'right_spring_link_joint': 0.0, - 'right_follower_joint': 0.0, - 'left_driver_joint': 0.0, - 'left_coupler_joint': 0.0, - 'left_spring_link_joint': 0.0, - 'left_follower_joint': 0.0}) -gripper.add_static_joint_states(GripperState.CLOSE, {'right_driver_joint': 0.798, - 'right_coupler_joint': 0.00366, - 'right_spring_link_joint': 0.796, - 'right_follower_joint': -0.793, - 'left_driver_joint': 0.798, - 'left_coupler_joint': 0.00366, - 'left_spring_link_joint': 0.796, - 'left_follower_joint': -0.793}) - -arm.end_effector = gripper - -# Add to RobotDescriptionManager -rdm = RobotDescriptionManager() -rdm.register_description(ur5_description) +if multiverse_resources is None: + logwarn("Could not initialize ur5e description as Multiverse resources path not found.") +else: + robot_relative_dir = "universal_robot" + robot_name = "ur5e" + gripper_name = "gripper-2F-85" + filename = get_robot_urdf_path_from_multiverse(robot_relative_dir, robot_name, resources_dir=multiverse_resources) + mjcf_filename = get_robot_mjcf_path(robot_relative_dir, robot_name, multiverse_resources=multiverse_resources) + ur5_description = RobotDescription(robot_name, "base_link", "", "", + filename, mjcf_path=mjcf_filename) + + ################################## Arm ################################## + arm = KinematicChainDescription("manipulator", "base_link", "wrist_3_link", + ur5_description.urdf_object, arm_type=Arms.RIGHT) + + arm.add_static_joint_states("home", {'shoulder_pan_joint': 3.14, + 'shoulder_lift_joint': -1.56, + 'elbow_joint': 1.58, + 'wrist_1_joint': -1.57, + 'wrist_2_joint': -1.57, + 'wrist_3_joint': 0.0}) + + ur5_description.add_kinematic_chain_description(arm) + + ################################## Gripper ################################## + gripper_relative_dir = "robotiq" + gripper_filename = get_robot_urdf_path_from_multiverse(gripper_relative_dir, gripper_name, + resources_dir=multiverse_resources) + gripper_urdf_obj = URDFObject(gripper_filename) + gripper = EndEffectorDescription("gripper", gripper_name, "right_pad", + gripper_urdf_obj, gripper_object_name=gripper_name) + + gripper.add_static_joint_states(GripperState.OPEN, {'right_driver_joint': 0.0, + 'right_coupler_joint': 0.0, + 'right_spring_link_joint': 0.0, + 'right_follower_joint': 0.0, + 'left_driver_joint': 0.0, + 'left_coupler_joint': 0.0, + 'left_spring_link_joint': 0.0, + 'left_follower_joint': 0.0}) + gripper.add_static_joint_states(GripperState.CLOSE, {'right_driver_joint': 0.798, + 'right_coupler_joint': 0.00366, + 'right_spring_link_joint': 0.796, + 'right_follower_joint': -0.793, + 'left_driver_joint': 0.798, + 'left_coupler_joint': 0.00366, + 'left_spring_link_joint': 0.796, + 'left_follower_joint': -0.793}) + + arm.end_effector = gripper + + # Add to RobotDescriptionManager + rdm = RobotDescriptionManager() + rdm.register_description(ur5_description) From dc021f8d9a86316489e9c90be8f20dd6b1da20d5 Mon Sep 17 00:00:00 2001 From: Abdelrhman Bassiouny Date: Fri, 25 Oct 2024 16:22:35 +0200 Subject: [PATCH 13/91] [Multiverse] No need for simulation name anymore for Multiverse, a standard is used instead (real, belief_state, and prospection) for the names. --- config/world_conf.py | 2 +- demos/pycram_multiverse_demo/demo.py | 2 +- src/pycram/worlds/multiverse.py | 16 +--------------- .../worlds/multiverse_communication/clients.py | 5 ++--- 4 files changed, 5 insertions(+), 20 deletions(-) diff --git a/config/world_conf.py b/config/world_conf.py index 8d57645c2..a81c34d82 100644 --- a/config/world_conf.py +++ b/config/world_conf.py @@ -35,7 +35,7 @@ class WorldConfig: Whether to clear the cache directory at the start. """ - prospection_world_prefix: str = "prospection_" + prospection_world_prefix: str = "prospection" """ The prefix for the prospection world name. """ diff --git a/demos/pycram_multiverse_demo/demo.py b/demos/pycram_multiverse_demo/demo.py index 6e71ec112..bf2a9cb98 100644 --- a/demos/pycram_multiverse_demo/demo.py +++ b/demos/pycram_multiverse_demo/demo.py @@ -21,7 +21,7 @@ def move_and_detect(obj_type: ObjectType, pick_pose: Pose): return object_desig -world = Multiverse(simulation_name='pycram_test') +world = Multiverse() extension = ObjectDescription.get_file_extension() robot = Object('pr2', ObjectType.ROBOT, f'pr2{extension}', pose=Pose([1.3, 2, 0.01])) apartment = Object("apartment", ObjectType.ENVIRONMENT, f"apartment{extension}") diff --git a/src/pycram/worlds/multiverse.py b/src/pycram/worlds/multiverse.py index b905434fb..aa64917f4 100644 --- a/src/pycram/worlds/multiverse.py +++ b/src/pycram/worlds/multiverse.py @@ -47,12 +47,6 @@ class Multiverse(World): A flag to check if the multiverse resources have been added. """ - simulation: Optional[str] = None - """ - The simulation name to be used in the Multiverse world (this is the name defined in - the multiverse configuration file). - """ - Object.extension_to_description_type[MJCF.get_file_extension()] = MJCF """ Add the MJCF description extension to the extension to description type mapping for the objects. @@ -60,14 +54,12 @@ class Multiverse(World): def __init__(self, mode: Optional[WorldMode] = WorldMode.DIRECT, is_prospection: Optional[bool] = False, - simulation_name: str = "pycram_test", clear_cache: bool = False): """ Initialize the Multiverse Socket and the PyCram World. :param mode: The mode of the world (DIRECT or GUI). :param is_prospection: Whether the world is prospection or not. - :param simulation_name: The name of the simulation. :param clear_cache: Whether to clear the cache or not. """ @@ -75,13 +67,7 @@ def __init__(self, mode: Optional[WorldMode] = WorldMode.DIRECT, self.saved_simulator_states: Dict = {} self._make_sure_multiverse_resources_are_added(clear_cache=clear_cache) - if Multiverse.simulation is None: - if simulation_name is None: - logging.error("Simulation name not provided") - raise ValueError("Simulation name not provided") - Multiverse.simulation = simulation_name - - self.simulation = (self.conf.prospection_world_prefix if is_prospection else "") + Multiverse.simulation + self.simulation = self.conf.prospection_world_prefix if is_prospection else "belief_state" self.client_manager = MultiverseClientManager(self.conf.simulation_wait_time_factor) self._init_clients(is_prospection=is_prospection) diff --git a/src/pycram/worlds/multiverse_communication/clients.py b/src/pycram/worlds/multiverse_communication/clients.py index 9e58bab27..62bc110c0 100644 --- a/src/pycram/worlds/multiverse_communication/clients.py +++ b/src/pycram/worlds/multiverse_communication/clients.py @@ -32,9 +32,8 @@ def __init__(self, name: str, port: int, is_prospection_world: bool = False, increase or decrease the wait time for the simulation. """ meta_data = MultiverseMetaData() - meta_data.simulation_name = (Conf.prospection_world_prefix if is_prospection_world else "") + name - meta_data.world_name = ((Conf.prospection_world_prefix if is_prospection_world else "") - + meta_data.world_name) + meta_data.simulation_name = (Conf.prospection_world_prefix if is_prospection_world else "belief_state") + name + meta_data.world_name = Conf.prospection_world_prefix if is_prospection_world else "belief_state" self.is_prospection_world = is_prospection_world super().__init__(port=str(port), meta_data=meta_data) self.simulation_wait_time_factor = simulation_wait_time_factor From e15fc98fb3df40866e9b7a256f7f1899a84c2117 Mon Sep 17 00:00:00 2001 From: Abdelrhman Bassiouny Date: Fri, 25 Oct 2024 16:24:03 +0200 Subject: [PATCH 14/91] [euROBIN] update belief state world with the real world info. --- .../demo_euROBIN_industrial_robotics.py | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/demos/pycram_multiverse_demo/demo_euROBIN_industrial_robotics.py b/demos/pycram_multiverse_demo/demo_euROBIN_industrial_robotics.py index cd6a85a58..d4226240e 100644 --- a/demos/pycram_multiverse_demo/demo_euROBIN_industrial_robotics.py +++ b/demos/pycram_multiverse_demo/demo_euROBIN_industrial_robotics.py @@ -1,14 +1,17 @@ from pycram.datastructures.enums import ObjectType, GripperState, Arms +from pycram.datastructures.world import UseProspectionWorld from pycram.process_module import simulated_robot, real_robot from pycram.world_concepts.world_object import Object from pycram.datastructures.pose import Pose from pycram.worlds.multiverse import Multiverse from pycram.designators.action_designator import SetGripperAction +from pycram.ros_utils.robot_state_updater import RobotStateUpdater if __name__ == '__main__': # Create a new world - world = Multiverse(simulation_name="ur5e_with_task_board") + world = Multiverse() + RobotStateUpdater(tf_topic="/tf", joint_state_topic="/real/ur5e/joint_states") # Load the robot and the gripper robot = Object("ur5e", ObjectType.ROBOT, "universal_robot/ur5e/urdf/ur5e.urdf") @@ -26,4 +29,5 @@ with real_robot: SetGripperAction(robot_arms, [GripperState.CLOSE]).resolve().perform() + world.exit() From 65811e7a8b63ee6a2efa1d7b344f5ca59d5dec6a Mon Sep 17 00:00:00 2001 From: Abdelrhman Bassiouny Date: Fri, 25 Oct 2024 16:25:10 +0200 Subject: [PATCH 15/91] [ros] fixed issue with wait_for_message and create_timer methods. Optimized setting joint states by using set_multiple joints instead of looping over all joints. --- src/pycram/ros/ros_tools.py | 2 +- src/pycram/ros_utils/robot_state_updater.py | 69 +++++++++++++++++++++ 2 files changed, 70 insertions(+), 1 deletion(-) create mode 100644 src/pycram/ros_utils/robot_state_updater.py diff --git a/src/pycram/ros/ros_tools.py b/src/pycram/ros/ros_tools.py index bd1fbfefb..a89152e41 100644 --- a/src/pycram/ros/ros_tools.py +++ b/src/pycram/ros/ros_tools.py @@ -46,5 +46,5 @@ def get_time(): return rospy.get_time() -def create_timer(duration: int, callback, oneshot=False): +def create_timer(duration: rospy.Duration, callback, oneshot=False): return rospy.Timer(duration, callback, oneshot=oneshot) diff --git a/src/pycram/ros_utils/robot_state_updater.py b/src/pycram/ros_utils/robot_state_updater.py new file mode 100644 index 000000000..0e797a973 --- /dev/null +++ b/src/pycram/ros_utils/robot_state_updater.py @@ -0,0 +1,69 @@ +import atexit +import tf +import time + +from geometry_msgs.msg import TransformStamped +from sensor_msgs.msg import JointState +from ..datastructures.world import World +from ..robot_descriptions import robot_description +from ..datastructures.pose import Pose +from ..ros.data_types import Time, Duration +from ..ros.ros_tools import wait_for_message, create_timer + + +class RobotStateUpdater: + """ + Updates the robot in the World with information of the real robot published to ROS topics. + Infos used to update the robot are: + + * The current pose of the robot + * The current joint state of the robot + """ + + def __init__(self, tf_topic: str, joint_state_topic: str): + """ + The robot state updater uses a TF topic and a joint state topic to get the current state of the robot. + + :param tf_topic: Name of the TF topic, needs to publish geometry_msgs/TransformStamped + :param joint_state_topic: Name of the joint state topic, needs to publish sensor_msgs/JointState + """ + self.tf_listener = tf.TransformListener() + time.sleep(1) + self.tf_topic = tf_topic + self.joint_state_topic = joint_state_topic + + self.tf_timer = create_timer(Duration().from_sec(0.1), self._subscribe_tf) + self.joint_state_timer = create_timer(Duration().from_sec(0.1), self._subscribe_joint_state) + + atexit.register(self._stop_subscription) + + def _subscribe_tf(self, msg: TransformStamped) -> None: + """ + Callback for the TF timer, will do a lookup of the transform between map frame and the robot base frame. + + :param msg: TransformStamped message published to the topic + """ + trans, rot = self.tf_listener.lookupTransform("/map", robot_description.base_frame, Time(0)) + World.robot.set_pose(Pose(trans, rot)) + + def _subscribe_joint_state(self, msg: JointState) -> None: + """ + Sets the current joint configuration of the robot in the world to the configuration published on the + topic. Since this uses rospy.wait_for_message which can have errors when used with threads there might be an + attribute error in the rospy implementation. + + :param msg: JointState message published to the topic. + """ + try: + msg = wait_for_message(self.joint_state_topic, JointState) + joint_positions = dict(zip(msg.name, msg.position)) + World.robot.set_multiple_joint_positions(joint_positions) + except AttributeError: + pass + + def _stop_subscription(self) -> None: + """ + Stops the Timer for TF and joint states and therefore the updating of the robot in the world. + """ + self.tf_timer.shutdown() + self.joint_state_timer.shutdown() From 2484cef96f28839cc9ed8ef77b85825fd17c96b2 Mon Sep 17 00:00:00 2001 From: Abdelrhman Bassiouny Date: Tue, 29 Oct 2024 19:32:44 +0100 Subject: [PATCH 16/91] [Costmaps] check if the maximum of the map is 0 before dividing and raise a warning if so. --- src/pycram/costmaps.py | 12 ++++++++++-- 1 file changed, 10 insertions(+), 2 deletions(-) diff --git a/src/pycram/costmaps.py b/src/pycram/costmaps.py index 4fdcfac83..7816cbc69 100644 --- a/src/pycram/costmaps.py +++ b/src/pycram/costmaps.py @@ -20,6 +20,9 @@ from .datastructures.dataclasses import AxisAlignedBoundingBox, BoxVisualShape, Color from .datastructures.pose import Pose, Transform +from .ros.logging import logwarn +from .datastructures.dataclasses import AxisAlignedBoundingBox +from .datastructures.pose import Pose from .datastructures.world import UseProspectionWorld from .datastructures.world import World from .description import Link @@ -214,10 +217,15 @@ def merge(self, other_cm: Costmap) -> Costmap: elif self.resolution != other_cm.resolution: raise ValueError("To merge two costmaps their resolution must be equal.") new_map = np.zeros((self.height, self.width)) - # A nunpy array of the positions where both costmaps are greater than 0 + # A numpy array of the positions where both costmaps are greater than 0 merge = np.logical_and(self.map > 0, other_cm.map > 0) new_map[merge] = self.map[merge] * other_cm.map[merge] - new_map = (new_map / np.max(new_map)).reshape((self.height, self.width)) + max_val = np.max(new_map) + if max_val > 0: + new_map = (new_map / np.max(new_map)).reshape((self.height, self.width)) + else: + new_map = new_map.reshape((self.height, self.width)) + logwarn("Merged costmap is empty.") return Costmap(self.resolution, self.height, self.width, self.origin, new_map) def __add__(self, other: Costmap) -> Costmap: From 39e83897851d1eaf4034a73886ee8a5e6300aed2 Mon Sep 17 00:00:00 2001 From: Abdelrhman Bassiouny Date: Tue, 29 Oct 2024 19:33:18 +0100 Subject: [PATCH 17/91] [ProcessModules] use camera link name not the frame name when getting the link as they are not the same. --- src/pycram/process_modules/default_process_modules.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/pycram/process_modules/default_process_modules.py b/src/pycram/process_modules/default_process_modules.py index 29148e4aa..f3900ee67 100644 --- a/src/pycram/process_modules/default_process_modules.py +++ b/src/pycram/process_modules/default_process_modules.py @@ -91,7 +91,7 @@ class DefaultDetecting(ProcessModule): def _execute(self, designator: DetectingMotion): robot = World.robot - cam_frame_name = RobotDescription.current_robot_description.get_camera_link() + cam_link_name = RobotDescription.current_robot_description.get_camera_link() camera_description = RobotDescription.current_robot_description.cameras[ list(RobotDescription.current_robot_description.cameras.keys())[0]] front_facing_axis = camera_description.front_facing_axis @@ -116,7 +116,7 @@ def _execute(self, designator: DetectingMotion): elif designator.technique == DetectionTechnique.HUMAN_WAVING: raise NotImplementedError("Detection by waving human is not yet implemented in simulation") for obj in world_objects: - if visible(obj, robot.get_link_pose(cam_frame_name), front_facing_axis): + if visible(obj, robot.get_link_pose(cam_link_name), front_facing_axis): query_result.append(obj) if query_result is None: raise PerceptionObjectNotFound( From 43e4d51ed25131ca258157b97822fdec09871e26 Mon Sep 17 00:00:00 2001 From: Abdelrhman Bassiouny Date: Tue, 29 Oct 2024 19:35:57 +0100 Subject: [PATCH 18/91] [RobotDescription] some cleaning and added a method for getting camera link. --- src/pycram/robot_description.py | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/pycram/robot_description.py b/src/pycram/robot_description.py index 9c004a004..eeef47601 100644 --- a/src/pycram/robot_description.py +++ b/src/pycram/robot_description.py @@ -247,21 +247,21 @@ def get_manipulator_chains(self) -> List[KinematicChainDescription]: result.append(chain) return result - def get_camera_link(self) -> str: + def get_camera_frame(self) -> str: """ Quick method to get the name of a link of a camera. Uses the first camera in the list of cameras. :return: A name of the link of a camera """ - return self.cameras[list(self.cameras.keys())[0]].link_name + return f"{self.name}/{self.get_camera_link()}" - def get_camera_frame(self) -> str: + def get_camera_link(self) -> str: """ Quick method to get the name of a link of a camera. Uses the first camera in the list of cameras. :return: A name of the link of a camera """ - return f"{self.name}/{self.cameras[list(self.cameras.keys())[0]].link_name}" + return self.get_default_camera().link_name def get_default_camera(self) -> CameraDescription: """ From ab2caa82d8dff5874015927e3a4348acefd0ee8c Mon Sep 17 00:00:00 2001 From: Abdelrhman Bassiouny Date: Tue, 29 Oct 2024 19:37:02 +0100 Subject: [PATCH 19/91] [JointLimits] added a check and clipping step to avoid exceeding joint limits when setting joint positions. --- src/pycram/world_concepts/world_object.py | 17 +++++++++++++++++ 1 file changed, 17 insertions(+) diff --git a/src/pycram/world_concepts/world_object.py b/src/pycram/world_concepts/world_object.py index 099439951..1ac455bb5 100644 --- a/src/pycram/world_concepts/world_object.py +++ b/src/pycram/world_concepts/world_object.py @@ -1135,6 +1135,11 @@ def set_joint_position(self, joint_name: str, joint_position: float) -> None: :param joint_name: The name of the joint :param joint_position: The target pose for this joint """ + if (self.joints[joint_name].has_limits and + (not self.joints[joint_name].lower_limit <= joint_position <= self.joints[joint_name].upper_limit)): + joint_position = np.clip(joint_position, self.joints[joint_name].lower_limit, + self.joints[joint_name].upper_limit) + logwarn(f"Joint position for joint {joint_name} was clipped to the joint limits.") if self.world.reset_joint_position(self.joints[joint_name], joint_position): self._update_on_joint_position_change() @@ -1154,6 +1159,18 @@ def set_multiple_joint_positions(self, joint_positions: Dict[str, float]) -> Non if self.world.set_multiple_joint_positions(joint_positions): self._update_on_joint_position_change() + def clip_joint_positions_to_limits(self, joint_positions: Dict[str, float]) -> Dict[str, float]: + """ + Clip the given joint positions to the joint limits. + + :param joint_positions: A dictionary with the joint names as keys and the target positions as values. + :return: A dictionary with the joint names as keys and the clipped positions as values. + """ + return {joint_name: np.clip(joint_position, self.joints[joint_name].lower_limit, + self.joints[joint_name].upper_limit) + if self.joints[joint_name].has_limits else joint_position + for joint_name, joint_position in joint_positions.items()} + def _update_on_joint_position_change(self): self.update_pose() self._update_all_links_poses() From f0a775ce472b5686aa0d8b982062909916707432 Mon Sep 17 00:00:00 2001 From: Abdelrhman Bassiouny Date: Tue, 29 Oct 2024 19:37:44 +0100 Subject: [PATCH 20/91] [Multiverse] can use controller in static mode as well when un pausing the simulation. --- config/multiverse_conf.py | 3 +-- src/pycram/worlds/multiverse.py | 26 +++++++++++++++++++++----- 2 files changed, 22 insertions(+), 7 deletions(-) diff --git a/config/multiverse_conf.py b/config/multiverse_conf.py index 5014d361d..7821eb171 100644 --- a/config/multiverse_conf.py +++ b/config/multiverse_conf.py @@ -47,8 +47,7 @@ class MultiverseConfig(WorldConfig): similar to bullet_world which uses the bullet physics engine. """ - use_controller: bool = False - use_controller = use_controller and not use_static_mode + use_controller: bool = True """ Only used when use_static_mode is False. This turns on the controller for the robot joints. """ diff --git a/src/pycram/worlds/multiverse.py b/src/pycram/worlds/multiverse.py index aa64917f4..70d2772b8 100644 --- a/src/pycram/worlds/multiverse.py +++ b/src/pycram/worlds/multiverse.py @@ -77,11 +77,13 @@ def __init__(self, mode: Optional[WorldMode] = WorldMode.DIRECT, self.ray_test_utils = RayTestUtils(self.ray_test_batch, self.object_id_to_name) + self.is_paused: bool = False + if not self.is_prospection_world: self._spawn_floor() if self.conf.use_static_mode: - self.api_requester.pause_simulation() + self.pause_simulation() def _init_clients(self, is_prospection: bool = False): """ @@ -142,6 +144,20 @@ def _spawn_floor(self): self.floor = Object("floor", ObjectType.ENVIRONMENT, "plane.urdf", world=self) + def pause_simulation(self) -> None: + """ + Pause the simulation. + """ + self.api_requester.pause_simulation() + self.is_paused = True + + def unpause_simulation(self) -> None: + """ + Unpause the simulation. + """ + self.api_requester.unpause_simulation() + self.is_paused = False + def load_generic_object_and_get_id(self, description: GenericObjectDescription, pose: Optional[Pose] = None) -> int: save_path = os.path.join(self.cache_manager.cache_dir, description.name + ".xml") @@ -255,7 +271,7 @@ def get_multiple_link_orientations(self, links: List[Link]) -> Dict[str, List[fl @validate_joint_position def reset_joint_position(self, joint: Joint, joint_position: float) -> bool: - if self.conf.use_controller and self.joint_has_actuator(joint): + if not self.is_paused and self.conf.use_controller and self.joint_has_actuator(joint): self._reset_joint_position_using_controller(joint, joint_position) else: self._set_multiple_joint_positions_without_controller({joint: joint_position}) @@ -285,7 +301,7 @@ def set_multiple_joint_positions(self, joint_positions: Dict[Joint, float]) -> b errors, but not necessarily that the joint positions are set to the specified values). """ - if self.conf.use_controller: + if not self.is_paused and self.conf.use_controller: controlled_joints = self.get_controlled_joints(list(joint_positions.keys())) if len(controlled_joints) > 0: controlled_joint_positions = {joint: joint_positions[joint] for joint in controlled_joints} @@ -614,11 +630,11 @@ def step(self, func: Optional[Callable[[], None]] = None, step_seconds: Optional :param step_seconds: The number of seconds to step the simulation. """ if self.conf.use_static_mode: - self.api_requester.unpause_simulation() + self.unpause_simulation() if func is not None: func() sleep(self.simulation_time_step if step_seconds is None else step_seconds) - self.api_requester.pause_simulation() + self.pause_simulation() def save_physics_simulator_state(self, state_id: Optional[int] = None, use_same_id: bool = False) -> int: if state_id is None: From 2aed1664261c8ee9bc1d22c3f6517a0a83b3837d Mon Sep 17 00:00:00 2001 From: Abdelrhman Bassiouny Date: Tue, 29 Oct 2024 19:38:39 +0100 Subject: [PATCH 21/91] [MultiverseFallschoolDemo] updated the demo with the milk in the fridge. --- .../pycram_multiverse_demo/fallschool_demo.py | 20 ++++++++++++++----- 1 file changed, 15 insertions(+), 5 deletions(-) diff --git a/demos/pycram_multiverse_demo/fallschool_demo.py b/demos/pycram_multiverse_demo/fallschool_demo.py index 8d924a4dc..28d984b2a 100644 --- a/demos/pycram_multiverse_demo/fallschool_demo.py +++ b/demos/pycram_multiverse_demo/fallschool_demo.py @@ -1,3 +1,5 @@ +from tf.transformations import quaternion_from_euler + from pycram.datastructures.dataclasses import Color from pycram.datastructures.enums import ObjectType, Arms from pycram.datastructures.pose import Pose @@ -19,12 +21,20 @@ def move_and_detect(obj_type: ObjectType, pick_pose: Pose): return object_desig + world = Multiverse() -robot = Object('pr2', ObjectType.ROBOT, f'pr2.urdf', pose=Pose([1.3, 2, 0.01])) +robot = Object('pr2', ObjectType.ROBOT, f'pr2.urdf', pose=Pose([1.3, 2.6, 0.01])) apartment = Object("apartment", ObjectType.ENVIRONMENT, f"apartment.urdf") - -milk = Object("milk", ObjectType.MILK, f"milk.stl", pose=Pose([2.4, 2, 1.02]), +milk = Object("milk", ObjectType.MILK, f"milk.stl", pose=Pose([0.4, 2.6, 1.34]), color=Color(1, 0, 0, 1)) +apartment.set_joint_position("fridge_door1_joint", 2) +# milk.set_orientation(Pose(orientation=[1, 0, 0, 1])) +# apartment.attach(milk, 'fridge_base') +fridge_base_pose = apartment.get_link_pose("fridge_base") +fridge_base_pose.position.z -= 0.12 +fridge_base_pose.position.x += 0.2 +milk.set_pose(fridge_base_pose, base=True) + robot_desig = BelieveObject(names=[robot.name]) apartment_desig = BelieveObject(names=[apartment.name]) @@ -36,9 +46,9 @@ def move_and_detect(obj_type: ObjectType, pick_pose: Pose): MoveTorsoAction([0.25]).resolve().perform() - NavigateAction(target_locations=[Pose([1.7, 2, 0])]).resolve().perform() + NavigateAction(target_locations=[Pose([1.3, 3, 0.01], quaternion_from_euler(0, 0, 3.14))]).resolve().perform() - LookAtAction(targets=[Pose([2.6, 2.15, 1])]).resolve().perform() + LookAtAction(targets=[Pose(milk.get_position_as_list())]).resolve().perform() milk_desig = DetectAction(BelieveObject(types=[milk.obj_type])).resolve().perform() From fee0adab57f1f3ebdef6f3f1331eb3ec0931c348 Mon Sep 17 00:00:00 2001 From: Abdelrhman Bassiouny Date: Wed, 30 Oct 2024 14:39:53 +0100 Subject: [PATCH 22/91] [Costmaps] Ignore floor when making an occupanct map. --- src/pycram/costmaps.py | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/src/pycram/costmaps.py b/src/pycram/costmaps.py index 7816cbc69..724fbe267 100644 --- a/src/pycram/costmaps.py +++ b/src/pycram/costmaps.py @@ -454,6 +454,7 @@ def _create_from_world(self, size: int, resolution: float) -> np.ndarray: # 16383 is the maximal number of rays that can be processed in a batch i = 0 j = 0 + floor_id = self.world.get_object_by_name("floor").id for n in self._chunks(np.array(rays), 16380): r_t = World.current_world.ray_test_batch(n[:, 0], n[:, 1], num_threads=0) while r_t is None: @@ -462,10 +463,10 @@ def _create_from_world(self, size: int, resolution: float) -> np.ndarray: if World.robot: attached_objs_id = [o.id for o in self.world.robot.attachments.keys()] res[i:j] = [ - 1 if ray[0] == -1 or ray[0] == self.world.robot.id or ray[0] in attached_objs_id else 0 for + 1 if ray[0] in [-1, self.world.robot.id, floor_id] + attached_objs_id else 0 for ray in r_t] else: - res[i:j] = [1 if ray[0] == -1 else 0 for ray in r_t] + res[i:j] = [1 if ray[0] in [-1, floor_id] else 0 for ray in r_t] i += len(n) res = np.flip(np.reshape(np.array(res), (size, size))) From 40f4d8b106e00affeced595be0c4a8b077a67129 Mon Sep 17 00:00:00 2001 From: Abdelrhman Bassiouny Date: Fri, 1 Nov 2024 17:41:08 +0100 Subject: [PATCH 23/91] [WorldSync] Copy object with original pose not current pose such that the original_pose attribute is the same for both objects. --- src/pycram/world_concepts/world_object.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/pycram/world_concepts/world_object.py b/src/pycram/world_concepts/world_object.py index 1ac455bb5..f0eeb760e 100644 --- a/src/pycram/world_concepts/world_object.py +++ b/src/pycram/world_concepts/world_object.py @@ -1470,7 +1470,7 @@ def copy_to_world(self, world: World) -> Object: :param world: The world to which the object should be copied. :return: The copied object in the given world. """ - obj = Object(self.name, self.obj_type, self.path, self.description, self.get_pose(), + obj = Object(self.name, self.obj_type, self.path, self.description, self.original_pose, world, self.color) return obj From 5f41047d187515f53e07e056c8ceaa051e6752cd Mon Sep 17 00:00:00 2001 From: Abdelrhman Bassiouny Date: Fri, 1 Nov 2024 17:42:17 +0100 Subject: [PATCH 24/91] [MultiverseFallschoolDemo] The demo works. --- demos/pycram_multiverse_demo/fallschool_demo.py | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/demos/pycram_multiverse_demo/fallschool_demo.py b/demos/pycram_multiverse_demo/fallschool_demo.py index 28d984b2a..1ef5dc2fb 100644 --- a/demos/pycram_multiverse_demo/fallschool_demo.py +++ b/demos/pycram_multiverse_demo/fallschool_demo.py @@ -32,7 +32,8 @@ def move_and_detect(obj_type: ObjectType, pick_pose: Pose): # apartment.attach(milk, 'fridge_base') fridge_base_pose = apartment.get_link_pose("fridge_base") fridge_base_pose.position.z -= 0.12 -fridge_base_pose.position.x += 0.2 +fridge_base_pose.position.x += 0.21 +fridge_base_pose.position.y += 0.1 milk.set_pose(fridge_base_pose, base=True) @@ -44,9 +45,9 @@ def move_and_detect(obj_type: ObjectType, pick_pose: Pose): # Transport the milk ParkArmsAction([Arms.BOTH]).resolve().perform() - MoveTorsoAction([0.25]).resolve().perform() + MoveTorsoAction([0.2]).resolve().perform() - NavigateAction(target_locations=[Pose([1.3, 3, 0.01], quaternion_from_euler(0, 0, 3.14))]).resolve().perform() + NavigateAction(target_locations=[Pose([1.1, 3.15, 0.01], quaternion_from_euler(0, 0, 3.14))]).resolve().perform() LookAtAction(targets=[Pose(milk.get_position_as_list())]).resolve().perform() From 3ed812907b55e2d668d2f88e0c72ddc19e2319ac Mon Sep 17 00:00:00 2001 From: Abdelrhman Bassiouny Date: Fri, 1 Nov 2024 19:22:52 +0100 Subject: [PATCH 25/91] [Multiverse] Made tests more independent. Remove save folder of simulator state when removing the state. --- src/pycram/worlds/multiverse.py | 5 ++++- src/pycram/worlds/multiverse_communication/clients.py | 4 ++-- test/test_multiverse.py | 11 +++++------ 3 files changed, 11 insertions(+), 9 deletions(-) diff --git a/src/pycram/worlds/multiverse.py b/src/pycram/worlds/multiverse.py index 70d2772b8..d33863c5c 100644 --- a/src/pycram/worlds/multiverse.py +++ b/src/pycram/worlds/multiverse.py @@ -1,5 +1,7 @@ import logging import os +import shutil +from pathlib import Path from time import sleep import numpy as np @@ -645,7 +647,8 @@ def save_physics_simulator_state(self, state_id: Optional[int] = None, use_same_ return state_id def remove_physics_simulator_state(self, state_id: int) -> None: - self.saved_simulator_states.pop(state_id) + save_path = self.saved_simulator_states.pop(state_id) + shutil.rmtree(Path(save_path).parent) def restore_physics_simulator_state(self, state_id: int) -> None: self.api_requester.load(self.saved_simulator_states[state_id]) diff --git a/src/pycram/worlds/multiverse_communication/clients.py b/src/pycram/worlds/multiverse_communication/clients.py index 62bc110c0..9582f7097 100644 --- a/src/pycram/worlds/multiverse_communication/clients.py +++ b/src/pycram/worlds/multiverse_communication/clients.py @@ -556,7 +556,7 @@ def get_save_path(save_name: str, save_directory: Optional[str] = None) -> str: :param save_directory: The save directory. :return: The save path. """ - return save_name if save_directory is None else os.path.join(save_directory, save_name) + return save_name if not save_directory else os.path.join(save_directory, save_name) def attach(self, constraint: Constraint) -> None: """ @@ -762,7 +762,7 @@ def _parse_constraint_effort(contact_effort: List[str]) -> Tuple[List[float], Li contact_effort = contact_effort[0].split() if 'failed' in contact_effort: logwarn("Failed to get contact effort") - return [0.0] * 6 + return [0.0] * 3, [0.0]*3 contact_effort = list(map(float, contact_effort)) forces, torques = contact_effort[:3], contact_effort[3:] return forces, torques diff --git a/test/test_multiverse.py b/test/test_multiverse.py index dd096ec4f..a015583d9 100644 --- a/test/test_multiverse.py +++ b/test/test_multiverse.py @@ -100,8 +100,7 @@ def test_spawn_xml_object(self): def test_get_axis_aligned_bounding_box_for_one_link_object(self): position = [1, 1, 0.1] - milk = Object("milk", ObjectType.MILK, "milk.stl", pose=Pose([1, 1, 0.1], - quaternion_from_euler(np.pi/4, 0, 0).tolist())) + milk = self.spawn_milk(position, quaternion_from_euler(np.pi/4, 0, 0).tolist()) aabb = milk.get_axis_aligned_bounding_box() self.assertIsInstance(aabb, AxisAlignedBoundingBox) min_p_1, max_p_1 = aabb.get_min_max() @@ -362,7 +361,7 @@ def test_attach_with_robot(self): robot = self.spawn_robot() ee_link = self.multiverse.get_arm_tool_frame_link(Arms.RIGHT) # Get position of milk relative to robot end effector - robot.attach(milk, ee_link.name, coincide_the_objects=False) + robot.attach(milk, ee_link.name) self.assertTrue(robot in milk.attachments) milk_initial_pose = milk.root_link.get_pose_wrt_link(ee_link) robot_position = 1.57 @@ -381,7 +380,7 @@ def test_get_object_contact_points(self): cup = self.spawn_cup([1, 1, 0.12]) # This is needed because the cup is spawned in the air, so it needs to fall # to get in contact with the milk - self.multiverse.simulate(0.3) + self.multiverse.simulate(0.4) contact_points = self.multiverse.get_object_contact_points(cup) self.assertIsInstance(contact_points, ContactPointsList) self.assertTrue(len(contact_points) >= 1) @@ -395,7 +394,7 @@ def test_get_contact_points_between_two_objects(self): cup = self.spawn_cup([1, 1, 0.12]) # This is needed because the cup is spawned in the air so it needs to fall # to get in contact with the milk - self.multiverse.simulate(0.3) + self.multiverse.simulate(0.4) contact_points = self.multiverse.get_contact_points_between_two_objects(milk, cup) self.assertIsInstance(contact_points, ContactPointsList) self.assertTrue(len(contact_points) >= 1) @@ -452,7 +451,7 @@ def spawn_robot(self, position: Optional[List[float]] = None, @staticmethod def spawn_cup(position: List) -> Object: - cup = Object("cup", ObjectType.GENERIC_OBJECT, "Cup.obj", + cup = Object("cup", ObjectType.GENERIC_OBJECT, "cup.xml", pose=Pose(position, [0, 0, 0, 1])) return cup From 5b8613ba7f476bea30af5f7b9f2d89884d8826b3 Mon Sep 17 00:00:00 2001 From: Abdelrhman Bassiouny Date: Tue, 5 Nov 2024 17:46:40 +0100 Subject: [PATCH 26/91] [GoalValidator] Made goal validation an option and added this option to the world configuration file. --- config/multiverse_conf.py | 2 ++ config/world_conf.py | 5 +++++ src/pycram/validation/goal_validator.py | 11 +++++++++++ 3 files changed, 18 insertions(+) diff --git a/config/multiverse_conf.py b/config/multiverse_conf.py index 7821eb171..e29ec9743 100644 --- a/config/multiverse_conf.py +++ b/config/multiverse_conf.py @@ -62,6 +62,8 @@ class MultiverseConfig(WorldConfig): Whether to use the physics simulator state when restoring or saving the world state. """ + validate_goals = False + clear_cache_at_start = True let_pycram_move_attached_objects = False diff --git a/config/world_conf.py b/config/world_conf.py index a81c34d82..c890d06da 100644 --- a/config/world_conf.py +++ b/config/world_conf.py @@ -84,6 +84,11 @@ class WorldConfig: Whether to use a percentage of the goal as the acceptable error. """ + validate_goals: bool = False + """ + Whether to validate the goals when executing them. + """ + raise_goal_validator_error: bool = False """ Whether to raise an error if the goals are not achieved. diff --git a/src/pycram/validation/goal_validator.py b/src/pycram/validation/goal_validator.py index 66756ae52..1f318d7aa 100644 --- a/src/pycram/validation/goal_validator.py +++ b/src/pycram/validation/goal_validator.py @@ -468,6 +468,9 @@ def validate_object_pose(pose_setter_func): def wrapper(world: 'World', obj: 'Object', pose: 'Pose'): + if not world.current_world.conf.validate_goals: + return True + world.pose_goal_validator.register_goal(pose, obj) if not pose_setter_func(world, obj, pose): @@ -489,6 +492,9 @@ def validate_multiple_object_poses(pose_setter_func): def wrapper(world: 'World', object_poses: Dict['Object', 'Pose']): + if not world.current_world.conf.validate_goals: + return True + world.multi_pose_goal_validator.register_goal(list(object_poses.values()), list(object_poses.keys())) @@ -511,6 +517,9 @@ def validate_joint_position(position_setter_func): def wrapper(world: 'World', joint: 'Joint', position: float): + if not world.current_world.conf.validate_goals: + return True + joint_type = joint.type world.joint_position_goal_validator.register_goal(position, joint_type, joint) @@ -535,6 +544,8 @@ def validate_multiple_joint_positions(position_setter_func): """ def wrapper(world: 'World', joint_positions: Dict['Joint', float]): + if not world.current_world.conf.validate_goals: + return True joint_positions_to_validate = {joint: position for joint, position in joint_positions.items() if not joint.is_virtual} joint_types = [joint.type for joint in joint_positions_to_validate.keys()] From cb9d1356844bfd4fe2c7ba007beb15880ab6eba5 Mon Sep 17 00:00:00 2001 From: Abdelrhman Bassiouny Date: Tue, 5 Nov 2024 17:48:33 +0100 Subject: [PATCH 27/91] [RobotDescription] Add an attribute that holds ignored joints, for example virtual joints that cannot be interacted with in the real world. --- src/pycram/robot_description.py | 5 ++++- src/pycram/robot_descriptions/pr2_description.py | 7 ++++++- src/pycram/world_concepts/world_object.py | 5 ++++- 3 files changed, 14 insertions(+), 3 deletions(-) diff --git a/src/pycram/robot_description.py b/src/pycram/robot_description.py index eeef47601..708bcd690 100644 --- a/src/pycram/robot_description.py +++ b/src/pycram/robot_description.py @@ -118,7 +118,8 @@ class RobotDescription: """ def __init__(self, name: str, base_link: str, torso_link: str, torso_joint: str, urdf_path: str, - virtual_mobile_base_joints: Optional[VirtualMobileBaseJoints] = None, mjcf_path: Optional[str] = None): + virtual_mobile_base_joints: Optional[VirtualMobileBaseJoints] = None, mjcf_path: Optional[str] = None, + ignore_joints: Optional[List[str]] = None): """ Initialize the RobotDescription. The URDF is loaded from the given path and used as basis for the kinematic chains. @@ -130,11 +131,13 @@ def __init__(self, name: str, base_link: str, torso_link: str, torso_joint: str, :param urdf_path: Path to the URDF file of the robot :param virtual_mobile_base_joints: Virtual mobile base joint names for mobile robots :param mjcf_path: Path to the MJCF file of the robot + :param ignore_joints: List of joint names that are not used. """ self.name = name self.base_link = base_link self.torso_link = torso_link self.torso_joint = torso_joint + self.ignore_joints = ignore_joints if ignore_joints else [] with suppress_stdout_stderr(): # Since parsing URDF causes a lot of warning messages which can't be deactivated, we suppress them self.urdf_object = URDFObject(urdf_path) diff --git a/src/pycram/robot_descriptions/pr2_description.py b/src/pycram/robot_descriptions/pr2_description.py index d532123ac..1a6b534ba 100644 --- a/src/pycram/robot_descriptions/pr2_description.py +++ b/src/pycram/robot_descriptions/pr2_description.py @@ -12,7 +12,12 @@ pr2_description = RobotDescription("pr2", "base_link", "torso_lift_link", "torso_lift_joint", filename, - virtual_mobile_base_joints=VirtualMobileBaseJoints(), mjcf_path=mjcf_filename) + virtual_mobile_base_joints=VirtualMobileBaseJoints(), + mjcf_path=mjcf_filename, + ignore_joints=['torso_lift_motor_screw_joint','r_gripper_motor_slider_joint', + 'r_gripper_motor_screw_joint', 'r_gripper_joint', + 'l_gripper_motor_slider_joint', 'l_gripper_motor_screw_joint', + 'l_gripper_joint']) ################################## Left Arm ################################## left_arm = KinematicChainDescription("left", "torso_lift_link", "l_wrist_roll_link", diff --git a/src/pycram/world_concepts/world_object.py b/src/pycram/world_concepts/world_object.py index f0eeb760e..384bf61f3 100644 --- a/src/pycram/world_concepts/world_object.py +++ b/src/pycram/world_concepts/world_object.py @@ -444,7 +444,10 @@ def joint_names(self) -> List[str]: """ The names of the joints as a list. """ - return self.world.get_object_joint_names(self) + joint_names = self.world.get_object_joint_names(self) + if self.obj_type == ObjectType.ROBOT and self.robot_description.ignore_joints: + joint_names = [joint for joint in joint_names if joint not in self.robot_description.ignore_joints] + return joint_names def get_link(self, link_name: str) -> ObjectDescription.Link: """ From a5332d239b0d3bd454e87fa19889f5a85bc0fe74 Mon Sep 17 00:00:00 2001 From: Abdelrhman Bassiouny Date: Tue, 5 Nov 2024 17:50:17 +0100 Subject: [PATCH 28/91] [RobotStateUpdater] Updated to the new version of RobotDescription. The ros timers duration is added as an argument that can be modified by the user if needed, this is good for example when using a heavy simulator and want to lower the update rate. --- src/pycram/ros_utils/robot_state_updater.py | 14 +++++++++----- 1 file changed, 9 insertions(+), 5 deletions(-) diff --git a/src/pycram/ros_utils/robot_state_updater.py b/src/pycram/ros_utils/robot_state_updater.py index 0e797a973..e3346a9dd 100644 --- a/src/pycram/ros_utils/robot_state_updater.py +++ b/src/pycram/ros_utils/robot_state_updater.py @@ -1,11 +1,13 @@ import atexit +from datetime import timedelta + import tf import time from geometry_msgs.msg import TransformStamped from sensor_msgs.msg import JointState from ..datastructures.world import World -from ..robot_descriptions import robot_description +from ..robot_description import RobotDescription from ..datastructures.pose import Pose from ..ros.data_types import Time, Duration from ..ros.ros_tools import wait_for_message, create_timer @@ -20,7 +22,7 @@ class RobotStateUpdater: * The current joint state of the robot """ - def __init__(self, tf_topic: str, joint_state_topic: str): + def __init__(self, tf_topic: str, joint_state_topic: str, update_rate: timedelta = timedelta(milliseconds=100)): """ The robot state updater uses a TF topic and a joint state topic to get the current state of the robot. @@ -32,8 +34,9 @@ def __init__(self, tf_topic: str, joint_state_topic: str): self.tf_topic = tf_topic self.joint_state_topic = joint_state_topic - self.tf_timer = create_timer(Duration().from_sec(0.1), self._subscribe_tf) - self.joint_state_timer = create_timer(Duration().from_sec(0.1), self._subscribe_joint_state) + self.tf_timer = create_timer(Duration().from_sec(update_rate.total_seconds()), self._subscribe_tf) + self.joint_state_timer = create_timer(Duration().from_sec(update_rate.total_seconds()), + self._subscribe_joint_state) atexit.register(self._stop_subscription) @@ -43,7 +46,8 @@ def _subscribe_tf(self, msg: TransformStamped) -> None: :param msg: TransformStamped message published to the topic """ - trans, rot = self.tf_listener.lookupTransform("/map", robot_description.base_frame, Time(0)) + base_link = RobotDescription.current_robot_description.base_link + trans, rot = self.tf_listener.lookupTransform("/map", base_link, Time(0)) World.robot.set_pose(Pose(trans, rot)) def _subscribe_joint_state(self, msg: JointState) -> None: From 78c2dc5094a368c69ce4c2c766610db43ca43d53 Mon Sep 17 00:00:00 2001 From: Abdelrhman Bassiouny Date: Tue, 5 Nov 2024 17:50:55 +0100 Subject: [PATCH 29/91] [MultiverseFallschoolDemo] (WIP) Navigate works, but LookinAt is not working. --- demos/pycram_multiverse_demo/fallschool_demo.py | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/demos/pycram_multiverse_demo/fallschool_demo.py b/demos/pycram_multiverse_demo/fallschool_demo.py index 1ef5dc2fb..3060ec507 100644 --- a/demos/pycram_multiverse_demo/fallschool_demo.py +++ b/demos/pycram_multiverse_demo/fallschool_demo.py @@ -1,3 +1,5 @@ +from datetime import timedelta + from tf.transformations import quaternion_from_euler from pycram.datastructures.dataclasses import Color @@ -6,7 +8,8 @@ from pycram.designators.action_designator import ParkArmsAction, MoveTorsoAction, TransportAction, NavigateAction, \ LookAtAction, DetectAction from pycram.designators.object_designator import BelieveObject -from pycram.process_module import simulated_robot, with_simulated_robot +from pycram.process_module import simulated_robot, with_simulated_robot, real_robot +from pycram.ros_utils.robot_state_updater import RobotStateUpdater from pycram.world_concepts.world_object import Object from pycram.worlds.multiverse import Multiverse @@ -24,6 +27,7 @@ def move_and_detect(obj_type: ObjectType, pick_pose: Pose): world = Multiverse() robot = Object('pr2', ObjectType.ROBOT, f'pr2.urdf', pose=Pose([1.3, 2.6, 0.01])) +RobotStateUpdater(tf_topic="/tf", joint_state_topic="/real/pr2/joint_states", update_rate=timedelta(seconds=1)) apartment = Object("apartment", ObjectType.ENVIRONMENT, f"apartment.urdf") milk = Object("milk", ObjectType.MILK, f"milk.stl", pose=Pose([0.4, 2.6, 1.34]), color=Color(1, 0, 0, 1)) @@ -40,7 +44,7 @@ def move_and_detect(obj_type: ObjectType, pick_pose: Pose): robot_desig = BelieveObject(names=[robot.name]) apartment_desig = BelieveObject(names=[apartment.name]) -with simulated_robot: +with real_robot: # Transport the milk ParkArmsAction([Arms.BOTH]).resolve().perform() From 68f701aefa9066d6d5da9bcc4211d0bd314ea7e6 Mon Sep 17 00:00:00 2001 From: Abdelrhman Bassiouny Date: Thu, 7 Nov 2024 17:22:31 +0100 Subject: [PATCH 30/91] [MultiverseFallschoolDemo] (WIP) Need to figure out why giskard state is not synced correctly. Fixed Goal Validator. robot_state_updater.py also updates states of objects. no need for original pose anymore to move mobile robot in multiverse. --- config/multiverse_conf.py | 6 +- .../demo_euROBIN_industrial_robotics.py | 4 +- .../pycram_multiverse_demo/fallschool_demo.py | 24 ++++--- src/pycram/datastructures/dataclasses.py | 45 +++++++++--- src/pycram/designators/motion_designator.py | 1 + src/pycram/external_interfaces/ik.py | 2 +- .../process_modules/pr2_process_modules.py | 42 ++++++++++- src/pycram/ros_utils/robot_state_updater.py | 29 ++++++-- src/pycram/validation/goal_validator.py | 71 +++++++++++++------ src/pycram/world_concepts/world_object.py | 13 ++-- src/pycram/worlds/multiverse.py | 24 +++++-- .../multiverse_communication/clients.py | 8 +-- test/test_multiverse.py | 6 +- 13 files changed, 201 insertions(+), 74 deletions(-) diff --git a/config/multiverse_conf.py b/config/multiverse_conf.py index e29ec9743..fc4b67e78 100644 --- a/config/multiverse_conf.py +++ b/config/multiverse_conf.py @@ -62,7 +62,7 @@ class MultiverseConfig(WorldConfig): Whether to use the physics simulator state when restoring or saving the world state. """ - validate_goals = False + validate_goals = True clear_cache_at_start = True @@ -73,4 +73,6 @@ class MultiverseConfig(WorldConfig): prismatic_joint_position_tolerance = 2e-2 use_giskard_monitor = False - allow_gripper_collision = False + allow_gripper_collision = True + + use_multiverse_process_modules = True diff --git a/demos/pycram_multiverse_demo/demo_euROBIN_industrial_robotics.py b/demos/pycram_multiverse_demo/demo_euROBIN_industrial_robotics.py index d4226240e..0fd3b6b78 100644 --- a/demos/pycram_multiverse_demo/demo_euROBIN_industrial_robotics.py +++ b/demos/pycram_multiverse_demo/demo_euROBIN_industrial_robotics.py @@ -5,13 +5,13 @@ from pycram.datastructures.pose import Pose from pycram.worlds.multiverse import Multiverse from pycram.designators.action_designator import SetGripperAction -from pycram.ros_utils.robot_state_updater import RobotStateUpdater +from pycram.ros_utils.robot_state_updater import WorldStateUpdater if __name__ == '__main__': # Create a new world world = Multiverse() - RobotStateUpdater(tf_topic="/tf", joint_state_topic="/real/ur5e/joint_states") + WorldStateUpdater(tf_topic="/tf", joint_state_topic="/real/ur5e/joint_states") # Load the robot and the gripper robot = Object("ur5e", ObjectType.ROBOT, "universal_robot/ur5e/urdf/ur5e.urdf") diff --git a/demos/pycram_multiverse_demo/fallschool_demo.py b/demos/pycram_multiverse_demo/fallschool_demo.py index 3060ec507..7dc2400c5 100644 --- a/demos/pycram_multiverse_demo/fallschool_demo.py +++ b/demos/pycram_multiverse_demo/fallschool_demo.py @@ -8,8 +8,9 @@ from pycram.designators.action_designator import ParkArmsAction, MoveTorsoAction, TransportAction, NavigateAction, \ LookAtAction, DetectAction from pycram.designators.object_designator import BelieveObject +from pycram.failures import PerceptionObjectNotFound from pycram.process_module import simulated_robot, with_simulated_robot, real_robot -from pycram.ros_utils.robot_state_updater import RobotStateUpdater +from pycram.ros_utils.robot_state_updater import WorldStateUpdater from pycram.world_concepts.world_object import Object from pycram.worlds.multiverse import Multiverse @@ -27,18 +28,22 @@ def move_and_detect(obj_type: ObjectType, pick_pose: Pose): world = Multiverse() robot = Object('pr2', ObjectType.ROBOT, f'pr2.urdf', pose=Pose([1.3, 2.6, 0.01])) -RobotStateUpdater(tf_topic="/tf", joint_state_topic="/real/pr2/joint_states", update_rate=timedelta(seconds=1)) +WorldStateUpdater(tf_topic="/tf", joint_state_topic="/real/pr2/joint_states", update_rate=timedelta(seconds=1), + world=world) apartment = Object("apartment", ObjectType.ENVIRONMENT, f"apartment.urdf") -milk = Object("milk", ObjectType.MILK, f"milk.stl", pose=Pose([0.4, 2.6, 1.34]), +milk = Object("milk", ObjectType.MILK, f"milk.stl", pose=Pose([0.4, 2.6, 1.34], + [1, 0, 0, 0]), color=Color(1, 0, 0, 1)) -apartment.set_joint_position("fridge_door1_joint", 2) +apartment.set_joint_position("fridge_door1_joint", 1.5707963267948966) # milk.set_orientation(Pose(orientation=[1, 0, 0, 1])) # apartment.attach(milk, 'fridge_base') fridge_base_pose = apartment.get_link_pose("fridge_base") fridge_base_pose.position.z -= 0.12 -fridge_base_pose.position.x += 0.21 -fridge_base_pose.position.y += 0.1 +fridge_base_pose.position.x += 0.16 +fridge_base_pose.position.y += -0.1 milk.set_pose(fridge_base_pose, base=True) +print(milk.get_position_as_list()) +print(milk.get_orientation_as_list()) robot_desig = BelieveObject(names=[robot.name]) @@ -51,11 +56,14 @@ def move_and_detect(obj_type: ObjectType, pick_pose: Pose): MoveTorsoAction([0.2]).resolve().perform() - NavigateAction(target_locations=[Pose([1.1, 3.15, 0.01], quaternion_from_euler(0, 0, 3.14))]).resolve().perform() + NavigateAction(target_locations=[Pose([1.1, 3.15, 0.01], quaternion_from_euler(0, 0, -2.9))]).resolve().perform() LookAtAction(targets=[Pose(milk.get_position_as_list())]).resolve().perform() - milk_desig = DetectAction(BelieveObject(types=[milk.obj_type])).resolve().perform() + try: + milk_desig = DetectAction(BelieveObject(types=[milk.obj_type])).resolve().perform() + except PerceptionObjectNotFound: + milk_desig = DetectAction(BelieveObject(types=[milk.obj_type])).resolve().perform() TransportAction(milk_desig, [Arms.LEFT], [Pose([2.4, 3, 1.02])]).resolve().perform() diff --git a/src/pycram/datastructures/dataclasses.py b/src/pycram/datastructures/dataclasses.py index 856c429f1..5d3ba6bf7 100644 --- a/src/pycram/datastructures/dataclasses.py +++ b/src/pycram/datastructures/dataclasses.py @@ -2,7 +2,7 @@ from abc import ABC, abstractmethod from copy import deepcopy, copy -from dataclasses import dataclass, field +from dataclasses import dataclass, fields import numpy as np import trimesh @@ -794,37 +794,60 @@ class TextAnnotation: size: float = 0.1 +@dataclass +class VirtualJoint: + """ + A virtual (not real) joint that is most likely used for simulation purposes. + """ + name: str + type_: JointType + axes: Optional[Point] = None + + @property + def type(self): + return self.type_ + + @property + def is_virtual(self): + return True + + def __hash__(self): + return hash(self.name) + + @dataclass class VirtualMobileBaseJoints: """ Dataclass for storing the names, types and axes of the virtual mobile base joints of a mobile robot. """ - translation_x: Optional[str] = VirtualMobileBaseJointName.LINEAR_X.value - translation_y: Optional[str] = VirtualMobileBaseJointName.LINEAR_Y.value - angular_z: Optional[str] = VirtualMobileBaseJointName.ANGULAR_Z.value + translation_x: Optional[VirtualJoint] = VirtualJoint(VirtualMobileBaseJointName.LINEAR_X.value, + JointType.PRISMATIC, + Point(1, 0, 0)) + translation_y: Optional[VirtualJoint] = VirtualJoint(VirtualMobileBaseJointName.LINEAR_Y.value, + JointType.PRISMATIC, + Point(0, 1, 0)) + angular_z: Optional[VirtualJoint] = VirtualJoint(VirtualMobileBaseJointName.ANGULAR_Z.value, + JointType.REVOLUTE, + Point(0, 0, 1)) @property def names(self) -> List[str]: """ Return the names of the virtual mobile base joints. """ - return [self.translation_x, self.translation_y, self.angular_z] + return [getattr(self, field.name).name for field in fields(self)] def get_types(self) -> Dict[str, JointType]: """ Return the joint types of the virtual mobile base joints. """ - return {self.translation_x: JointType.PRISMATIC, - self.translation_y: JointType.PRISMATIC, - self.angular_z: JointType.REVOLUTE} + return {getattr(self, field.name).name: getattr(self, field.name).type_ for field in fields(self)} def get_axes(self) -> Dict[str, Point]: """ Return the axes (i.e. The axis on which the joint moves) of the virtual mobile base joints. """ - return {self.translation_x: Point(1, 0, 0), - self.translation_y: Point(0, 1, 0), - self.angular_z: Point(0, 0, 1)} + return {getattr(self, field.name).name: getattr(self, field.name).axes for field in fields(self)} @dataclass diff --git a/src/pycram/designators/motion_designator.py b/src/pycram/designators/motion_designator.py index c9cee63f5..d23b0521b 100644 --- a/src/pycram/designators/motion_designator.py +++ b/src/pycram/designators/motion_designator.py @@ -21,6 +21,7 @@ from ..datastructures.pose import Pose from ..tasktree import with_tree from ..designator import BaseMotion +from ..external_interfaces.robokudo import robokudo_found @dataclass diff --git a/src/pycram/external_interfaces/ik.py b/src/pycram/external_interfaces/ik.py index 5a89a679f..9510b2cb3 100644 --- a/src/pycram/external_interfaces/ik.py +++ b/src/pycram/external_interfaces/ik.py @@ -171,7 +171,7 @@ def request_ik(target_pose: Pose, robot: Object, joints: List[str], gripper: str :param gripper: Name of the tool frame which should grasp, this should be at the end of the given joint chain :return: A Pose at which the robt should stand as well as a dictionary of joint values """ - if "/giskard" not in rosnode.get_node_names(): + if "/giskard" not in rosnode.get_node_names() or True: return robot.pose, request_kdl_ik(target_pose, robot, joints, gripper) return request_giskard_ik(target_pose, robot, gripper) diff --git a/src/pycram/process_modules/pr2_process_modules.py b/src/pycram/process_modules/pr2_process_modules.py index 7a7a991b3..ecb51f7fd 100644 --- a/src/pycram/process_modules/pr2_process_modules.py +++ b/src/pycram/process_modules/pr2_process_modules.py @@ -30,8 +30,14 @@ if TYPE_CHECKING: from ..designators.object_designator import ObjectDesignatorDescription +try: + from ..worlds import Multiverse +except ImportError: + Multiverse = type(None) + try: from pr2_controllers_msgs.msg import Pr2GripperCommandGoal, Pr2GripperCommandAction, Pr2 + from control_msgs.msg import GripperCommandGoal, GripperCommandAction except ImportError: logdebug("Pr2GripperCommandGoal not found") @@ -263,6 +269,36 @@ def _execute(self, designator: MoveJointsMotion) -> Any: giskard.achieve_joint_goal(name_to_position) +class Pr2MoveGripperMultiverse(ProcessModule): + """ + Opens or closes the gripper of the real PR2, gripper uses an action server for this instead of giskard + """ + + def _execute(self, designator: MoveGripperMotion) -> Any: + def activate_callback(): + loginfo("Started gripper Movement") + + def done_callback(state, result): + loginfo(f"Reached goal {designator.motion}: {result.reached_goal}") + + def feedback_callback(msg): + pass + + goal = GripperCommandGoal() + goal.command.position = 0.0 if designator.motion == "close" else 0.1 + goal.command.max_effort = 50.0 + if designator.gripper == "right": + controller_topic = "/real/pr2/right_gripper_controller/gripper_cmd" + else: + controller_topic = "/real/pr2/left_gripper_controller/gripper_cmd" + client = actionlib.SimpleActionClient(controller_topic, GripperCommandAction) + loginfo("Waiting for action server") + client.wait_for_server() + client.send_goal(goal, active_cb=activate_callback, done_cb=done_callback, feedback_cb=feedback_callback) + wait = client.wait_for_result() + + + class Pr2MoveGripperReal(ProcessModule): """ Opens or closes the gripper of the real PR2, gripper uses an action server for this instead of giskard @@ -376,7 +412,11 @@ def move_gripper(self): if ProcessModuleManager.execution_type == ExecutionType.SIMULATED: return Pr2MoveGripper(self._move_gripper_lock) elif ProcessModuleManager.execution_type == ExecutionType.REAL: - return Pr2MoveGripperReal(self._move_gripper_lock) + if (isinstance(World.current_world, Multiverse) and + World.current_world.conf.use_multiverse_process_modules): + return Pr2MoveGripperMultiverse(self._move_gripper_lock) + else: + return Pr2MoveGripperReal(self._move_gripper_lock) def open(self): if ProcessModuleManager.execution_type == ExecutionType.SIMULATED: diff --git a/src/pycram/ros_utils/robot_state_updater.py b/src/pycram/ros_utils/robot_state_updater.py index e3346a9dd..c409d09ae 100644 --- a/src/pycram/ros_utils/robot_state_updater.py +++ b/src/pycram/ros_utils/robot_state_updater.py @@ -6,6 +6,8 @@ from geometry_msgs.msg import TransformStamped from sensor_msgs.msg import JointState +from typing_extensions import Optional + from ..datastructures.world import World from ..robot_description import RobotDescription from ..datastructures.pose import Pose @@ -13,7 +15,7 @@ from ..ros.ros_tools import wait_for_message, create_timer -class RobotStateUpdater: +class WorldStateUpdater: """ Updates the robot in the World with information of the real robot published to ROS topics. Infos used to update the robot are: @@ -22,7 +24,8 @@ class RobotStateUpdater: * The current joint state of the robot """ - def __init__(self, tf_topic: str, joint_state_topic: str, update_rate: timedelta = timedelta(milliseconds=100)): + def __init__(self, tf_topic: str, joint_state_topic: str, update_rate: timedelta = timedelta(milliseconds=100), + world: Optional[World] = None) -> None: """ The robot state updater uses a TF topic and a joint state topic to get the current state of the robot. @@ -33,7 +36,7 @@ def __init__(self, tf_topic: str, joint_state_topic: str, update_rate: timedelta time.sleep(1) self.tf_topic = tf_topic self.joint_state_topic = joint_state_topic - + self.world: Optional[World] = world self.tf_timer = create_timer(Duration().from_sec(update_rate.total_seconds()), self._subscribe_tf) self.joint_state_timer = create_timer(Duration().from_sec(update_rate.total_seconds()), self._subscribe_joint_state) @@ -42,13 +45,24 @@ def __init__(self, tf_topic: str, joint_state_topic: str, update_rate: timedelta def _subscribe_tf(self, msg: TransformStamped) -> None: """ - Callback for the TF timer, will do a lookup of the transform between map frame and the robot base frame. + Callback for the TF timer, will do a lookup of the transform between map frame and the objects frames. :param msg: TransformStamped message published to the topic """ - base_link = RobotDescription.current_robot_description.base_link - trans, rot = self.tf_listener.lookupTransform("/map", base_link, Time(0)) - World.robot.set_pose(Pose(trans, rot)) + if self.world is None: + if not World.current_world.is_prospection_world: + self.world = World.current_world + else: + return + for obj in self.world.objects: + if obj.name == self.world.robot.name: + tf_frame = RobotDescription.current_robot_description.base_link + elif obj.has_type_environment(): + continue + else: + tf_frame = obj.tf_frame + trans, rot = self.tf_listener.lookupTransform("/map", tf_frame, Time(0)) + obj.set_pose(Pose(trans, rot)) def _subscribe_joint_state(self, msg: JointState) -> None: """ @@ -71,3 +85,4 @@ def _stop_subscription(self) -> None: """ self.tf_timer.shutdown() self.joint_state_timer.shutdown() + diff --git a/src/pycram/validation/goal_validator.py b/src/pycram/validation/goal_validator.py index 1f318d7aa..ebb03f745 100644 --- a/src/pycram/validation/goal_validator.py +++ b/src/pycram/validation/goal_validator.py @@ -3,10 +3,10 @@ import numpy as np from typing_extensions import Any, Callable, Optional, Union, Iterable, Dict, TYPE_CHECKING, Tuple -from ..datastructures.enums import JointType from .error_checkers import ErrorChecker, PoseErrorChecker, PositionErrorChecker, \ OrientationErrorChecker, SingleValueErrorChecker -from ..ros.logging import logerr, logwarn +from ..datastructures.enums import JointType +from ..ros.logging import logerr if TYPE_CHECKING: from ..datastructures.world import World @@ -90,7 +90,7 @@ def wait_until_goal_is_achieved(self, max_wait_time: Optional[float] = 2, logerr(msg) raise TimeoutError(msg) else: - logwarn(msg) + # logwarn(msg) break current = self.current_value self.reset() @@ -167,7 +167,11 @@ def current_value(self) -> Any: if self.current_value_getter_input is not None: return self.current_value_getter(self.current_value_getter_input) else: - return self.current_value_getter() + try: + return self.current_value_getter() + except Exception as e: + logerr(f"Error while getting current value: {e}") + return None @property def current_error(self) -> np.ndarray: @@ -222,6 +226,8 @@ def relative_initial_error(self) -> np.ndarray: """ The relative initial error (relative to the acceptable error). """ + if self.initial_error is None: + self.update_initial_error(self.goal_value) return np.maximum(self.initial_error, 1e-3) def get_relative_error(self, error: Any, threshold: Optional[float] = 1e-3) -> np.ndarray: @@ -419,7 +425,7 @@ def register_goal(self, goal_value: Any, joint_type: JointType, :param acceptable_error: The acceptable error. """ if acceptable_error is None: - self.error_checker.acceptable_error = self.acceptable_orientation_error if joint_type == JointType.REVOLUTE\ + self.error_checker.acceptable_error = self.acceptable_orientation_error if joint_type == JointType.REVOLUTE \ else self.acceptable_position_error super().register_goal(goal_value, current_value_getter_input, initial_value, acceptable_error) @@ -469,15 +475,19 @@ def validate_object_pose(pose_setter_func): def wrapper(world: 'World', obj: 'Object', pose: 'Pose'): if not world.current_world.conf.validate_goals: - return True + return pose_setter_func(world, obj, pose) - world.pose_goal_validator.register_goal(pose, obj) + if obj is None: + logerr("Object should not be None") + return False + pose_goal_validator = PoseGoalValidator(world.get_object_pose, world.conf.get_pose_tolerance(), + world.conf.acceptable_percentage_of_goal) + pose_goal_validator.register_goal(pose, obj) if not pose_setter_func(world, obj, pose): - world.pose_goal_validator.reset() return False - world.pose_goal_validator.wait_until_goal_is_achieved() + pose_goal_validator.wait_until_goal_is_achieved() return True return wrapper @@ -493,16 +503,18 @@ def validate_multiple_object_poses(pose_setter_func): def wrapper(world: 'World', object_poses: Dict['Object', 'Pose']): if not world.current_world.conf.validate_goals: - return True + return pose_setter_func(world, object_poses) - world.multi_pose_goal_validator.register_goal(list(object_poses.values()), - list(object_poses.keys())) + multi_pose_goal_validator = MultiPoseGoalValidator( + lambda x: list(world.get_multiple_object_poses(x).values()), + world.conf.get_pose_tolerance(), world.conf.acceptable_percentage_of_goal) + multi_pose_goal_validator.register_goal(list(object_poses.values()), + list(object_poses.keys())) if not pose_setter_func(world, object_poses): - world.multi_pose_goal_validator.reset() return False - world.multi_pose_goal_validator.wait_until_goal_is_achieved() + multi_pose_goal_validator.wait_until_goal_is_achieved() return True return wrapper @@ -518,16 +530,21 @@ def validate_joint_position(position_setter_func): def wrapper(world: 'World', joint: 'Joint', position: float): if not world.current_world.conf.validate_goals: - return True + return position_setter_func(world, joint, position) + + joint_position_goal_validator = JointPositionGoalValidator( + world.get_joint_position, + acceptable_revolute_joint_position_error=world.conf.revolute_joint_position_tolerance, + acceptable_prismatic_joint_position_error=world.conf.prismatic_joint_position_tolerance, + acceptable_percentage_of_goal_achieved=world.conf.acceptable_percentage_of_goal) joint_type = joint.type - world.joint_position_goal_validator.register_goal(position, joint_type, joint) + joint_position_goal_validator.register_goal(position, joint_type, joint) if not position_setter_func(world, joint, position): - world.joint_position_goal_validator.reset() return False - world.joint_position_goal_validator.wait_until_goal_is_achieved() + joint_position_goal_validator.wait_until_goal_is_achieved() return True return wrapper @@ -545,17 +562,25 @@ def validate_multiple_joint_positions(position_setter_func): def wrapper(world: 'World', joint_positions: Dict['Joint', float]): if not world.current_world.conf.validate_goals: - return True + return position_setter_func(world, joint_positions) joint_positions_to_validate = {joint: position for joint, position in joint_positions.items() if not joint.is_virtual} + if not joint_positions_to_validate: + return position_setter_func(world, joint_positions) + + multi_joint_position_goal_validator = MultiJointPositionGoalValidator( + lambda x: list(world.get_multiple_joint_positions(x).values()), + acceptable_revolute_joint_position_error=world.conf.revolute_joint_position_tolerance, + acceptable_prismatic_joint_position_error=world.conf.prismatic_joint_position_tolerance, + acceptable_percentage_of_goal_achieved=world.conf.acceptable_percentage_of_goal) + joint_types = [joint.type for joint in joint_positions_to_validate.keys()] - world.multi_joint_position_goal_validator.register_goal(list(joint_positions_to_validate.values()), joint_types, - list(joint_positions_to_validate.keys())) + multi_joint_position_goal_validator.register_goal(list(joint_positions_to_validate.values()), joint_types, + list(joint_positions_to_validate.keys())) if not position_setter_func(world, joint_positions): - world.multi_joint_position_goal_validator.reset() return False - world.multi_joint_position_goal_validator.wait_until_goal_is_achieved() + multi_joint_position_goal_validator.wait_until_goal_is_achieved() return True return wrapper diff --git a/src/pycram/world_concepts/world_object.py b/src/pycram/world_concepts/world_object.py index 384bf61f3..a54fe8852 100644 --- a/src/pycram/world_concepts/world_object.py +++ b/src/pycram/world_concepts/world_object.py @@ -13,7 +13,7 @@ from ..datastructures.dataclasses import (Color, ObjectState, LinkState, JointState, AxisAlignedBoundingBox, VisualShape, ClosestPointsList, - ContactPointsList, RotatedBoundingBox) + ContactPointsList, RotatedBoundingBox, VirtualJoint) from ..datastructures.enums import ObjectType, JointType from ..datastructures.pose import Pose, Transform from ..datastructures.world import World @@ -167,21 +167,22 @@ def set_mobile_robot_pose(self, pose: Pose) -> None: :param pose: The target pose. """ goal = self.get_mobile_base_joint_goal(pose) + goal = {vj.name: pos for vj, pos in goal.items()} self.set_multiple_joint_positions(goal) - def get_mobile_base_joint_goal(self, pose: Pose) -> Dict[str, float]: + def get_mobile_base_joint_goal(self, pose: Pose) -> Dict[VirtualJoint, float]: """ Get the goal for the mobile base joints of a mobile robot to reach a target pose. :param pose: The target pose. :return: The goal for the mobile base joints. """ - target_translation, target_angle = self.get_mobile_base_pose_difference(pose) + # target_translation, target_angle = self.get_mobile_base_pose_difference(pose) # Get the joints of the base link mobile_base_joints = self.world.get_robot_mobile_base_joints() - return {mobile_base_joints.translation_x: target_translation.x, - mobile_base_joints.translation_y: target_translation.y, - mobile_base_joints.angular_z: target_angle} + return {mobile_base_joints.translation_x: pose.position.x, + mobile_base_joints.translation_y: pose.position.y, + mobile_base_joints.angular_z: pose.z_angle} def get_mobile_base_pose_difference(self, pose: Pose) -> Tuple[Point, float]: """ diff --git a/src/pycram/worlds/multiverse.py b/src/pycram/worlds/multiverse.py index d33863c5c..c69a90149 100644 --- a/src/pycram/worlds/multiverse.py +++ b/src/pycram/worlds/multiverse.py @@ -203,9 +203,10 @@ def spawn_robot_with_controller(self, name: str, pose: Pose) -> None: for joint_name, actuator_name in self.robot_joint_actuators.items() } self.joint_controller.init_controller(actuator_joint_commands) - self.writer.spawn_robot_with_actuators(name, pose.position_as_list(), - xyzw_to_wxyz(pose.orientation_as_list()), - actuator_joint_commands) + self.writer.spawn_robot_with_actuators(name, actuator_joint_commands) + if not pose.almost_equal(Pose()): + goal = self.robot.get_mobile_base_joint_goal(pose) + self.set_multiple_joint_positions(goal) def load_object_and_get_id(self, name: Optional[str] = None, pose: Optional[Pose] = None, @@ -236,11 +237,24 @@ def spawn_object(self, name: str, object_type: ObjectType, pose: Pose) -> None: :param object_type: The type of the object. :param pose: The pose of the object. """ - if object_type == ObjectType.ROBOT and self.conf.use_controller: - self.spawn_robot_with_controller(name, pose) + if object_type == ObjectType.ROBOT: + if self.conf.use_controller: + self.spawn_robot_with_controller(name, pose) + else: + self.spawn_robot(name, pose) else: self._set_body_pose(name, pose) + def spawn_robot(self, name: str, pose: Pose) -> None: + """ + Spawn the robot in the simulator. + + :param name: The name of the robot. + :param pose: The pose of the robot. + """ + self._set_body_pose(name, Pose()) + self.robot.set_mobile_robot_pose(pose) + def _update_object_id_name_maps_and_get_latest_id(self, name: str) -> int: """ Update the object id name maps and return the latest object id. diff --git a/src/pycram/worlds/multiverse_communication/clients.py b/src/pycram/worlds/multiverse_communication/clients.py index 9582f7097..ae93e7843 100644 --- a/src/pycram/worlds/multiverse_communication/clients.py +++ b/src/pycram/worlds/multiverse_communication/clients.py @@ -320,20 +320,18 @@ def __init__(self, name: str, port: int, simulation: Optional[str] = None, super().__init__(name, port, is_prospection_world, simulation_wait_time_factor=simulation_wait_time_factor) self.simulation = simulation - def spawn_robot_with_actuators(self, robot_name: str, position: List[float], orientation: List[float], + def spawn_robot_with_actuators(self, robot_name: str, actuator_joint_commands: Optional[Dict[str, List[str]]] = None) -> None: """ Spawn the robot with controlled actuators in the simulation. :param robot_name: The name of the robot. - :param position: The position of the robot. - :param orientation: The orientation of the robot. :param actuator_joint_commands: A dictionary mapping actuator names to joint command names. """ send_meta_data = {robot_name: [BodyProperty.POSITION.value, BodyProperty.ORIENTATION.value, BodyProperty.RELATIVE_VELOCITY.value]} relative_velocity = [0.0] * 6 - data = [self.sim_time, *position, *orientation, *relative_velocity] + data = [self.sim_time] + [0.0] * 3 + [1, 0, 0, 0] + relative_velocity self.send_data_to_server(data, send_meta_data=send_meta_data, receive_meta_data=actuator_joint_commands) def _reset_request_meta_data(self, set_simulation_name: bool = True): @@ -506,7 +504,7 @@ class MultiverseAPI(MultiverseClient): """ The wait time for the API request in seconds. """ - APIs_THAT_NEED_WAIT_TIME: List[API] = [API.ATTACH] + APIs_THAT_NEED_WAIT_TIME: List[API] = [API.ATTACH, API.DETACH] def __init__(self, name: str, port: int, simulation: str, is_prospection_world: bool = False, simulation_wait_time_factor: float = 1.0): diff --git a/test/test_multiverse.py b/test/test_multiverse.py index a015583d9..4e9e3ae70 100644 --- a/test/test_multiverse.py +++ b/test/test_multiverse.py @@ -185,7 +185,7 @@ def test_spawn_robot_with_actuators_directly_from_multiverse(self): robot_name = "tiago_dual" rdm = RobotDescriptionManager() rdm.load_description(robot_name) - self.multiverse.spawn_robot_with_controller(robot_name, Pose([-2, -2, 0.001])) + self.multiverse.spawn_robot_with_controller(robot_name, Pose()) def test_spawn_object(self): milk = self.spawn_milk([1, 1, 0.1]) @@ -370,7 +370,7 @@ def test_attach_with_robot(self): self.assert_poses_are_equal(milk_initial_pose, milk_pose) def test_get_object_contact_points(self): - for i in range(10): + for i in range(3): milk = self.spawn_milk([1, 1, 0.01], [0, -0.707, 0, 0.707]) contact_points = self.multiverse.get_object_contact_points(milk) self.assertIsInstance(contact_points, ContactPointsList) @@ -443,7 +443,7 @@ def spawn_robot(self, position: Optional[List[float]] = None, if self.multiverse.robot is not None: self.multiverse.robot.remove() robot = Object(robot_name, ObjectType.ROBOT, f"{robot_name}.urdf", - pose=Pose(position, [0, 0, 0, 1])) + pose=Pose(position, orientation)) else: robot = self.multiverse.robot robot.set_position(position) From 2e3c938b8a202837269e5b15dc6199d024ec143e Mon Sep 17 00:00:00 2001 From: Abdelrhman Bassiouny Date: Fri, 8 Nov 2024 20:06:50 +0100 Subject: [PATCH 31/91] [MultiverseFallschoolDemo] (WIP) Need to fix multiverse contact, and make a feedback loop on gripper closing. --- .../behavior_tree_config.py | 310 ++++++++++++++++ .../pycram_multiverse_demo/fallschool_demo.py | 6 +- demos/pycram_multiverse_demo/pr2.py | 301 ++++++++++++++++ .../pycram_multiverse_demo/testing_giskard.py | 144 ++++++++ demos/pycram_multiverse_demo/world_config.py | 333 ++++++++++++++++++ src/pycram/designators/location_designator.py | 40 ++- src/pycram/designators/motion_designator.py | 6 +- src/pycram/external_interfaces/giskard.py | 19 +- src/pycram/external_interfaces/ik.py | 2 +- src/pycram/pose_generator_and_validator.py | 12 +- .../process_modules/pr2_process_modules.py | 15 +- src/pycram/worlds/multiverse.py | 7 +- 12 files changed, 1166 insertions(+), 29 deletions(-) create mode 100644 demos/pycram_multiverse_demo/behavior_tree_config.py create mode 100644 demos/pycram_multiverse_demo/pr2.py create mode 100644 demos/pycram_multiverse_demo/testing_giskard.py create mode 100644 demos/pycram_multiverse_demo/world_config.py diff --git a/demos/pycram_multiverse_demo/behavior_tree_config.py b/demos/pycram_multiverse_demo/behavior_tree_config.py new file mode 100644 index 000000000..952764df8 --- /dev/null +++ b/demos/pycram_multiverse_demo/behavior_tree_config.py @@ -0,0 +1,310 @@ +from abc import ABC, abstractmethod +from typing import Optional + +from giskardpy.exceptions import SetupException +from giskardpy.god_map import god_map +from giskardpy.model.ros_msg_visualization import VisualizationMode +from giskardpy.tree.behaviors.tf_publisher import TfPublishingModes +from giskardpy.tree.branches.giskard_bt import GiskardBT +from giskardpy.tree.control_modes import ControlModes +from giskardpy.utils.utils import is_running_in_pytest + + +class BehaviorTreeConfig(ABC): + + def __init__(self, mode: ControlModes, control_loop_max_hz: float = 50, simulation_max_hz: Optional[float] = None): + """ + + :param mode: Defines the default setup of the behavior tree. + :param control_loop_max_hz: if mode == ControlModes.standalone: limits the simulation speed + if mode == ControlModes.open_loop: limits the control loop of the base tracker + if mode == ControlModes.close_loop: limits the control loop + """ + self._control_mode = mode + self.control_loop_max_hz = control_loop_max_hz + self.simulation_max_hz = simulation_max_hz + + @abstractmethod + def setup(self): + """ + Implement this method to configure the behavior tree using it's self. methods. + """ + + @property + def tree(self) -> GiskardBT: + return god_map.tree + + def _create_behavior_tree(self): + god_map.tree = GiskardBT(control_mode=self._control_mode) + + def set_defaults(self): + pass + + def set_tree_tick_rate(self, rate: float = 0.05): + """ + How often the tree ticks per second. + :param rate: in /s + """ + self.tree_tick_rate = rate + + def add_visualization_marker_publisher(self, + mode: VisualizationMode, + add_to_sync: Optional[bool] = None, + add_to_control_loop: Optional[bool] = None): + """ + + :param add_to_sync: Markers are published while waiting for a goal. + :param add_to_control_loop: Markers are published during the closed loop control sequence, this is slow. + :param use_decomposed_meshes: True: publish decomposed meshes used for collision avoidance, these likely only + available on the machine where Giskard is running. + False: use meshes defined in urdf. + """ + if add_to_sync: + self.tree.wait_for_goal.publish_state.add_visualization_marker_behavior(mode) + if add_to_control_loop: + self.tree.control_loop_branch.publish_state.add_visualization_marker_behavior(mode) + + def add_qp_data_publisher(self, publish_lb: bool = False, publish_ub: bool = False, + publish_lbA: bool = False, publish_ubA: bool = False, + publish_bE: bool = False, publish_Ax: bool = False, + publish_Ex: bool = False, publish_xdot: bool = False, + publish_weights: bool = False, publish_g: bool = False, + publish_debug: bool = False, add_to_base: bool = False): + """ + QP data is streamed and can be visualized in e.g. plotjuggler. Useful for debugging. + """ + self.add_evaluate_debug_expressions() + if god_map.is_open_loop(): + self.tree.execute_traj.base_closed_loop.publish_state.add_qp_data_publisher( + publish_lb=publish_lb, + publish_ub=publish_ub, + publish_lbA=publish_lbA, + publish_ubA=publish_ubA, + publish_bE=publish_bE, + publish_Ax=publish_Ax, + publish_Ex=publish_Ex, + publish_xdot=publish_xdot, + publish_weights=publish_weights, + publish_g=publish_g, + publish_debug=publish_debug) + else: + self.tree.control_loop_branch.publish_state.add_qp_data_publisher( + publish_lb=publish_lb, + publish_ub=publish_ub, + publish_lbA=publish_lbA, + publish_ubA=publish_ubA, + publish_bE=publish_bE, + publish_Ax=publish_Ax, + publish_Ex=publish_Ex, + publish_xdot=publish_xdot, + publish_weights=publish_weights, + publish_g=publish_g, + publish_debug=publish_debug) + + def add_trajectory_plotter(self, normalize_position: bool = False, wait: bool = False): + """ + Plots the generated trajectories. + :param normalize_position: Positions are centered around zero. + :param wait: True: Behavior tree waits for this plotter to finish. + False: Plot is generated in a separate thread to not slow down Giskard. + """ + self.tree.cleanup_control_loop.add_plot_trajectory(normalize_position, wait) + + def add_trajectory_visualizer(self): + self.tree.cleanup_control_loop.add_visualize_trajectory() + + def add_debug_trajectory_visualizer(self): + self.tree.cleanup_control_loop.add_debug_visualize_trajectory() + + def add_debug_trajectory_plotter(self, normalize_position: bool = False, wait: bool = False): + """ + Plots debug expressions defined in goals. + """ + self.add_evaluate_debug_expressions() + self.tree.cleanup_control_loop.add_plot_debug_trajectory(normalize_position=normalize_position, + wait=wait) + + def add_gantt_chart_plotter(self): + self.add_evaluate_debug_expressions() + self.tree.cleanup_control_loop.add_plot_gantt_chart() + + def add_goal_graph_plotter(self): + self.add_evaluate_debug_expressions() + self.tree.prepare_control_loop.add_plot_goal_graph() + + def add_debug_marker_publisher(self): + """ + Publishes debug expressions defined in goals. + """ + self.add_evaluate_debug_expressions() + self.tree.control_loop_branch.publish_state.add_debug_marker_publisher() + + def add_tf_publisher(self, include_prefix: bool = True, tf_topic: str = 'tf', + mode: TfPublishingModes = TfPublishingModes.attached_and_world_objects): + """ + Publishes tf for Giskard's internal state. + """ + self.tree.wait_for_goal.publish_state.add_tf_publisher(include_prefix=include_prefix, + tf_topic=tf_topic, + mode=mode) + if god_map.is_standalone(): + self.tree.control_loop_branch.publish_state.add_tf_publisher(include_prefix=include_prefix, + tf_topic=tf_topic, + mode=mode) + + def add_evaluate_debug_expressions(self): + self.tree.prepare_control_loop.add_compile_debug_expressions() + if god_map.is_closed_loop(): + self.tree.control_loop_branch.add_evaluate_debug_expressions(log_traj=False) + else: + self.tree.control_loop_branch.add_evaluate_debug_expressions(log_traj=True) + if god_map.is_open_loop(): + god_map.tree.execute_traj.prepare_base_control.add_compile_debug_expressions() + god_map.tree.execute_traj.base_closed_loop.add_evaluate_debug_expressions(log_traj=False) + + def add_js_publisher(self, topic_name: Optional[str] = None, include_prefix: bool = False): + """ + Publishes joint states for Giskard's internal state. + """ + god_map.tree.control_loop_branch.publish_state.add_joint_state_publisher(include_prefix=include_prefix, + topic_name=topic_name, + only_prismatic_and_revolute=True) + god_map.tree.wait_for_goal.publish_state.add_joint_state_publisher(include_prefix=include_prefix, + topic_name=topic_name, + only_prismatic_and_revolute=True) + + def add_free_variable_publisher(self, topic_name: Optional[str] = None, include_prefix: bool = False): + """ + Publishes joint states for Giskard's internal state. + """ + god_map.tree.control_loop_branch.publish_state.add_joint_state_publisher(include_prefix=include_prefix, + topic_name=topic_name, + only_prismatic_and_revolute=False) + god_map.tree.wait_for_goal.publish_state.add_joint_state_publisher(include_prefix=include_prefix, + topic_name=topic_name, + only_prismatic_and_revolute=False) + + +class StandAloneBTConfig(BehaviorTreeConfig): + def __init__(self, + debug_mode: bool = False, + publish_js: bool = False, + visualization_mode: VisualizationMode = VisualizationMode.VisualsFrameLocked, + publish_free_variables: bool = False, + publish_tf: bool = True, + include_prefix: bool = False, + simulation_max_hz: Optional[float] = None): + """ + The default behavior tree for Giskard in standalone mode. Make sure to set up the robot interface accordingly. + :param debug_mode: enable various debugging tools. + :param publish_js: publish current world state. + :param publish_tf: publish all link poses in tf. + :param simulation_max_hz: if not None, will limit the frequency of the simulation. + :param include_prefix: whether to include the robot name prefix when publishing joint states or tf + """ + self.include_prefix = include_prefix + self.visualization_mode = visualization_mode + if is_running_in_pytest(): + if god_map.is_in_github_workflow(): + publish_js = False + publish_tf = False + debug_mode = False + simulation_max_hz = None + super().__init__(ControlModes.standalone, simulation_max_hz=simulation_max_hz) + self.debug_mode = debug_mode + self.publish_js = publish_js + self.publish_free_variables = publish_free_variables + self.publish_tf = publish_tf + if publish_js and publish_free_variables: + raise SetupException('publish_js and publish_free_variables cannot be True at the same time.') + + def setup(self): + self.add_visualization_marker_publisher(add_to_sync=True, add_to_control_loop=True, + mode=self.visualization_mode) + if self.publish_tf: + self.add_tf_publisher(include_prefix=self.include_prefix, mode=TfPublishingModes.all) + self.add_gantt_chart_plotter() + self.add_goal_graph_plotter() + if self.debug_mode: + self.add_trajectory_plotter(wait=True) + # self.add_trajectory_visualizer() + # self.add_debug_trajectory_visualizer() + self.add_debug_trajectory_plotter(wait=True) + self.add_debug_marker_publisher() + # self.add_debug_marker_publisher() + if self.publish_js: + self.add_js_publisher(include_prefix=self.include_prefix) + if self.publish_free_variables: + self.add_free_variable_publisher() + + +class OpenLoopBTConfig(BehaviorTreeConfig): + def __init__(self, + debug_mode: bool = False, + control_loop_max_hz: float = 50, + visualization_mode: VisualizationMode = VisualizationMode.CollisionsDecomposed, + simulation_max_hz: Optional[float] = None): + """ + The default behavior tree for Giskard in open-loop mode. It will first plan the trajectory in simulation mode + and then publish it to connected joint trajectory followers. The base trajectory is tracked with a closed-loop + controller. + :param debug_mode: enable various debugging tools. + :param control_loop_max_hz: if not None, limits the frequency of the base trajectory controller. + """ + super().__init__(ControlModes.open_loop, control_loop_max_hz=control_loop_max_hz, + simulation_max_hz=simulation_max_hz) + if god_map.is_in_github_workflow(): + debug_mode = False + self.debug_mode = debug_mode + self.visualization_mode = visualization_mode + + def setup(self): + self.add_visualization_marker_publisher(add_to_sync=True, add_to_control_loop=True, + mode=self.visualization_mode) + self.add_gantt_chart_plotter() + self.add_goal_graph_plotter() + if self.debug_mode or True: + self.add_trajectory_plotter(wait=True) + self.add_debug_trajectory_plotter(wait=True) + self.add_debug_marker_publisher() + self.add_qp_data_publisher( + publish_debug=True, + publish_xdot=True, + # publish_lbA=True, + # publish_ubA=True + ) + + +class ClosedLoopBTConfig(BehaviorTreeConfig): + def __init__(self, debug_mode: bool = False, + control_loop_max_hz: float = 50, + visualization_mode: VisualizationMode = VisualizationMode.CollisionsDecomposed, + simulation_max_hz: Optional[float] = None): + """ + The default configuration for Giskard in closed loop mode. Make use to set up the robot interface accordingly. + :param debug_mode: If True, will publish debug data on topics. This will significantly slow down the control loop. + :param control_loop_max_hz: Limits the control loop frequency. If None, it will go as fast as possible. + """ + super().__init__(ControlModes.close_loop, control_loop_max_hz=control_loop_max_hz, + simulation_max_hz=simulation_max_hz) + if god_map.is_in_github_workflow(): + debug_mode = False + self.debug_mode = debug_mode + self.visualization_mode = visualization_mode + + def setup(self): + self.add_visualization_marker_publisher(add_to_sync=True, add_to_control_loop=False, + mode=self.visualization_mode) + # self.add_qp_data_publisher(publish_xdot=True, publish_lb=True, publish_ub=True) + self.add_gantt_chart_plotter() + self.add_goal_graph_plotter() + if self.debug_mode: + self.add_trajectory_plotter(wait=True) + self.add_debug_trajectory_plotter(wait=True) + self.add_debug_marker_publisher() + # self.add_qp_data_publisher( + # publish_debug=True, + # publish_xdot=True, + # # publish_lbA=True, + # # publish_ubA=True + # ) diff --git a/demos/pycram_multiverse_demo/fallschool_demo.py b/demos/pycram_multiverse_demo/fallschool_demo.py index 7dc2400c5..ea81e8df2 100644 --- a/demos/pycram_multiverse_demo/fallschool_demo.py +++ b/demos/pycram_multiverse_demo/fallschool_demo.py @@ -51,7 +51,7 @@ def move_and_detect(obj_type: ObjectType, pick_pose: Pose): with real_robot: - # Transport the milk + # Transport the milkMoveGripperMotion ParkArmsAction([Arms.BOTH]).resolve().perform() MoveTorsoAction([0.2]).resolve().perform() @@ -60,11 +60,15 @@ def move_and_detect(obj_type: ObjectType, pick_pose: Pose): LookAtAction(targets=[Pose(milk.get_position_as_list())]).resolve().perform() + # world.restore_physics_simulator_state(100) + try: milk_desig = DetectAction(BelieveObject(types=[milk.obj_type])).resolve().perform() except PerceptionObjectNotFound: milk_desig = DetectAction(BelieveObject(types=[milk.obj_type])).resolve().perform() + # world.save_state(100) + TransportAction(milk_desig, [Arms.LEFT], [Pose([2.4, 3, 1.02])]).resolve().perform() ParkArmsAction([Arms.BOTH]).resolve().perform() diff --git a/demos/pycram_multiverse_demo/pr2.py b/demos/pycram_multiverse_demo/pr2.py new file mode 100644 index 000000000..ae4cc1872 --- /dev/null +++ b/demos/pycram_multiverse_demo/pr2.py @@ -0,0 +1,301 @@ +from giskardpy.configs.giskard import CollisionAvoidanceConfig, RobotInterfaceConfig +from giskardpy.configs.world_config import WorldWithOmniDriveRobot +from giskardpy.data_types import Derivatives + + +class WorldWithPR2Config(WorldWithOmniDriveRobot): + def __init__(self, map_name: str = 'map', localization_joint_name: str = 'localization', + odom_link_name: str = 'odom_combined', drive_joint_name: str = 'brumbrum'): + super().__init__(map_name, localization_joint_name, odom_link_name, drive_joint_name) + + def setup(self): + super().setup() + self.set_joint_limits(limit_map={Derivatives.velocity: 2, + Derivatives.jerk: 60}, + joint_name='head_pan_joint') + self.set_joint_limits(limit_map={Derivatives.velocity: 4, + Derivatives.jerk: 120}, + joint_name='head_tilt_joint') + + + +class PR2StandaloneInterface(RobotInterfaceConfig): + drive_joint_name: str + + def __init__(self, drive_joint_name: str): + self.drive_joint_name = drive_joint_name + + def setup(self): + self.register_controlled_joints([ + 'torso_lift_joint', + 'head_pan_joint', + 'head_tilt_joint', + 'r_shoulder_pan_joint', + 'r_shoulder_lift_joint', + 'r_upper_arm_roll_joint', + 'r_forearm_roll_joint', + 'r_elbow_flex_joint', + 'r_wrist_flex_joint', + 'r_wrist_roll_joint', + 'l_shoulder_pan_joint', + 'l_shoulder_lift_joint', + 'l_upper_arm_roll_joint', + 'l_forearm_roll_joint', + 'l_elbow_flex_joint', + 'l_wrist_flex_joint', + 'l_wrist_roll_joint', + self.drive_joint_name, + ]) + + +class PR2JointTrajServerMujocoInterface(RobotInterfaceConfig): + map_name: str + localization_joint_name: str + odom_link_name: str + drive_joint_name: str + + def __init__(self, + map_name: str = 'map', + localization_joint_name: str = 'localization', + odom_link_name: str = 'odom_combined', + drive_joint_name: str = 'brumbrum'): + self.map_name = map_name + self.localization_joint_name = localization_joint_name + self.odom_link_name = odom_link_name + self.drive_joint_name = drive_joint_name + + def setup(self): + self.sync_6dof_joint_with_tf_frame(joint_name=self.localization_joint_name, + tf_parent_frame=self.map_name, + tf_child_frame=self.odom_link_name) + self.sync_joint_state_topic('/joint_states') + self.sync_odometry_topic('/pr2/base_footprint', self.drive_joint_name) + self.add_follow_joint_trajectory_server( + namespace='/pr2/whole_body_controller') + self.add_follow_joint_trajectory_server( + namespace='/pr2/l_gripper_l_finger_controller') + self.add_follow_joint_trajectory_server( + namespace='/pr2/r_gripper_l_finger_controller') + self.add_base_cmd_velocity(cmd_vel_topic='/pr2/cmd_vel', + track_only_velocity=True, + joint_name=self.drive_joint_name) + +class PR2JointTrajServerMultiverseInterface(RobotInterfaceConfig): + map_name: str + localization_joint_name: str + odom_link_name: str + drive_joint_name: str + + def __init__(self, + map_name: str = 'map', + localization_joint_name: str = 'localization', + odom_link_name: str = 'pr2', + drive_joint_name: str = 'brumbrum'): + self.map_name = map_name + self.localization_joint_name = localization_joint_name + self.odom_link_name = odom_link_name + self.drive_joint_name = drive_joint_name + + def setup(self): + # self.sync_6dof_joint_with_tf_frame(joint_name=self.localization_joint_name, + # tf_parent_frame=self.map_name, + # tf_child_frame=self.odom_link_name) + # self. + self.sync_joint_state_topic('/real/pr2/joint_states') + self.sync_odometry_topic('/odom', self.drive_joint_name) + self.add_follow_joint_trajectory_server( + namespace='/real/pr2/head_controller') + self.add_follow_joint_trajectory_server( + namespace='/real/pr2/torso_controller') + self.add_follow_joint_trajectory_server( + namespace='/real/pr2/left_arm_controller') + self.add_follow_joint_trajectory_server( + namespace='/real/pr2/right_arm_controller') + self.add_base_cmd_velocity(cmd_vel_topic='/cmd_vel', + track_only_velocity=True, + joint_name=self.drive_joint_name) + +class PR2VelocityMujocoInterface(RobotInterfaceConfig): + map_name: str + localization_joint_name: str + odom_link_name: str + drive_joint_name: str + + def __init__(self, + map_name: str = 'map', + localization_joint_name: str = 'localization', + odom_link_name: str = 'odom_combined', + drive_joint_name: str = 'brumbrum'): + self.map_name = map_name + self.localization_joint_name = localization_joint_name + self.odom_link_name = odom_link_name + self.drive_joint_name = drive_joint_name + + def setup(self): + self.sync_6dof_joint_with_tf_frame(joint_name=self.localization_joint_name, + tf_parent_frame=self.map_name, + tf_child_frame=self.odom_link_name) + self.sync_joint_state_topic('pr2/joint_states') + self.sync_odometry_topic('/pr2/base_footprint', self.drive_joint_name) + self.add_joint_velocity_controller(namespaces=[ + 'pr2/torso_lift_velocity_controller', + 'pr2/r_upper_arm_roll_velocity_controller', + 'pr2/r_shoulder_pan_velocity_controller', + 'pr2/r_shoulder_lift_velocity_controller', + 'pr2/r_forearm_roll_velocity_controller', + 'pr2/r_elbow_flex_velocity_controller', + 'pr2/r_wrist_flex_velocity_controller', + 'pr2/r_wrist_roll_velocity_controller', + 'pr2/l_upper_arm_roll_velocity_controller', + 'pr2/l_shoulder_pan_velocity_controller', + 'pr2/l_shoulder_lift_velocity_controller', + 'pr2/l_forearm_roll_velocity_controller', + 'pr2/l_elbow_flex_velocity_controller', + 'pr2/l_wrist_flex_velocity_controller', + 'pr2/l_wrist_roll_velocity_controller', + 'pr2/head_pan_velocity_controller', + 'pr2/head_tilt_velocity_controller', + ]) + + self.add_base_cmd_velocity(cmd_vel_topic='/pr2/cmd_vel', + joint_name=self.drive_joint_name) + + +class PR2VelocityIAIInterface(RobotInterfaceConfig): + map_name: str + localization_joint_name: str + odom_link_name: str + drive_joint_name: str + + def __init__(self, + map_name: str = 'map', + localization_joint_name: str = 'localization', + odom_link_name: str = 'odom_combined', + drive_joint_name: str = 'brumbrum'): + self.map_name = map_name + self.localization_joint_name = localization_joint_name + self.odom_link_name = odom_link_name + self.drive_joint_name = drive_joint_name + + def setup(self): + self.sync_6dof_joint_with_tf_frame(joint_name=self.localization_joint_name, + tf_parent_frame=self.map_name, + tf_child_frame=self.odom_link_name) + self.sync_joint_state_topic('/joint_states') + self.sync_odometry_topic('/robot_pose_ekf/odom_combined', self.drive_joint_name) + self.add_joint_velocity_group_controller(namespace='l_arm_joint_group_velocity_controller') + self.add_joint_velocity_group_controller(namespace='r_arm_joint_group_velocity_controller') + self.add_joint_position_controller(namespaces=[ + 'head_pan_position_controller', + 'head_tilt_position_controller', + ]) + + self.add_base_cmd_velocity(cmd_vel_topic='/base_controller/command', + joint_name=self.drive_joint_name) + + +class PR2CollisionAvoidance(CollisionAvoidanceConfig): + def __init__(self, drive_joint_name: str = 'brumbrum'): + super().__init__() + self.drive_joint_name = drive_joint_name + + def setup(self): + self.load_self_collision_matrix('package://giskardpy/self_collision_matrices/iai/pr2.srdf') + self.set_default_external_collision_avoidance(soft_threshold=0.1, + hard_threshold=0.0) + for joint_name in ['r_wrist_roll_joint', 'l_wrist_roll_joint']: + self.overwrite_external_collision_avoidance(joint_name, + number_of_repeller=4, + soft_threshold=0.05, + hard_threshold=0.0, + max_velocity=0.2) + for joint_name in ['r_wrist_flex_joint', 'l_wrist_flex_joint']: + self.overwrite_external_collision_avoidance(joint_name, + number_of_repeller=2, + soft_threshold=0.05, + hard_threshold=0.0, + max_velocity=0.2) + for joint_name in ['r_elbow_flex_joint', 'l_elbow_flex_joint']: + self.overwrite_external_collision_avoidance(joint_name, + soft_threshold=0.05, + hard_threshold=0.0) + for joint_name in ['r_forearm_roll_joint', 'l_forearm_roll_joint']: + self.overwrite_external_collision_avoidance(joint_name, + soft_threshold=0.025, + hard_threshold=0.0) + self.fix_joints_for_collision_avoidance([ + 'r_gripper_l_finger_joint', + 'l_gripper_l_finger_joint' + ]) + self.overwrite_external_collision_avoidance(self.drive_joint_name, + number_of_repeller=2, + soft_threshold=0.2, + hard_threshold=0.1) + + +class PR2JointTrajServerIAIInterface(RobotInterfaceConfig): + map_name: str + localization_joint_name: str + odom_link_name: str + drive_joint_name: str + + def __init__(self, + map_name: str = 'map', + localization_joint_name: str = 'localization', + odom_link_name: str = 'odom_combined', + drive_joint_name: str = 'brumbrum'): + self.map_name = map_name + self.localization_joint_name = localization_joint_name + self.odom_link_name = odom_link_name + self.drive_joint_name = drive_joint_name + + def setup(self): + self.sync_6dof_joint_with_tf_frame(joint_name=self.localization_joint_name, + tf_parent_frame=self.map_name, + tf_child_frame=self.odom_link_name) + self.sync_joint_state_topic('/joint_states') + self.sync_odometry_topic('/robot_pose_ekf/odom_combined', self.drive_joint_name) + fill_velocity_values = False + self.add_follow_joint_trajectory_server(namespace='/l_arm_controller', + fill_velocity_values=fill_velocity_values) + self.add_follow_joint_trajectory_server(namespace='/r_arm_controller', + fill_velocity_values=fill_velocity_values) + self.add_follow_joint_trajectory_server(namespace='/torso_controller', + fill_velocity_values=fill_velocity_values) + self.add_follow_joint_trajectory_server(namespace='/head_traj_controller', + fill_velocity_values=fill_velocity_values) + self.add_base_cmd_velocity(cmd_vel_topic='/base_controller/command', + track_only_velocity=True, + joint_name=self.drive_joint_name) + + +class PR2JointTrajServerUnrealInterface(RobotInterfaceConfig): + map_name: str + localization_joint_name: str + odom_link_name: str + drive_joint_name: str + + def __init__(self, + map_name: str = 'map', + localization_joint_name: str = 'localization', + odom_link_name: str = 'odom_combined', + drive_joint_name: str = 'brumbrum'): + self.map_name = map_name + self.localization_joint_name = localization_joint_name + self.odom_link_name = odom_link_name + self.drive_joint_name = drive_joint_name + + def setup(self): + self.sync_6dof_joint_with_tf_frame(joint_name=self.localization_joint_name, + tf_parent_frame=self.map_name, + tf_child_frame=self.odom_link_name) + self.sync_joint_state_topic('/joint_states') + self.sync_odometry_topic('/base_odometry/odom', self.drive_joint_name) + fill_velocity_values = False + self.add_follow_joint_trajectory_server(namespace='/whole_body_controller', + fill_velocity_values=fill_velocity_values) + self.add_follow_joint_trajectory_server(namespace='/head_traj_controller', + fill_velocity_values=fill_velocity_values) + self.add_base_cmd_velocity(cmd_vel_topic='/base_controller/command', + track_only_velocity=True, + joint_name=self.drive_joint_name) diff --git a/demos/pycram_multiverse_demo/testing_giskard.py b/demos/pycram_multiverse_demo/testing_giskard.py new file mode 100644 index 000000000..f5181efe4 --- /dev/null +++ b/demos/pycram_multiverse_demo/testing_giskard.py @@ -0,0 +1,144 @@ +import actionlib +from control_msgs.msg import GripperCommandGoal, GripperCommandAction +from giskard_msgs.msg import WorldResult + +from giskardpy.python_interface.old_python_interface import OldGiskardWrapper as GiskardWrapper +import rospy + +from geometry_msgs.msg import PoseStamped, Point, Quaternion + +from giskardpy.utils.logging import loginfo + + +def spawn_urdf(name: str, urdf_path: str, pose: PoseStamped) -> WorldResult: + """ + Spawns an URDF in giskard's belief state. + + :param name: Name of the URDF + :param urdf_path: Path to the URDF file + :param pose: Pose in which the URDF should be spawned + :return: A WorldResult message + """ + with open(urdf_path) as f: + urdf_string = f.read() + + return giskard.add_urdf(name, urdf_string, pose) + + +def get_pose_stamped(position, orientation): + pose_stamped = PoseStamped() + pose_stamped.header.frame_id = 'map' + pose_stamped.pose.position = Point(*position) + pose_stamped.pose.orientation = Quaternion(*orientation) + return pose_stamped + + +def park_arms(): + joint_goals = {'l_shoulder_pan_joint': 1.712, 'l_shoulder_lift_joint': -0.264, 'l_upper_arm_roll_joint': 1.38, + 'l_elbow_flex_joint': -2.12, 'l_forearm_roll_joint': 16.996, 'l_wrist_flex_joint': -0.073, + 'l_wrist_roll_joint': 0.0, 'r_shoulder_pan_joint': -1.712, 'r_shoulder_lift_joint': -0.256, + 'r_upper_arm_roll_joint': -1.463, 'r_elbow_flex_joint': -2.12, 'r_forearm_roll_joint': 1.766, + 'r_wrist_flex_joint': -0.07, 'r_wrist_roll_joint': 0.051} + giskard.set_joint_goal(joint_goals) + giskard.execute() + + +def get_urdf_string(urdf_path): + with open(urdf_path) as f: + return f.read() + + +def open_left_gripper(): + open_gripper("left") + + +def close_left_gripper(): + close_gripper("left") + + +def open_right_gripper(): + open_gripper("right") + + +def close_right_gripper(): + close_gripper("right") + + +def open_gripper(gripper: str): + move_gripper("open", gripper) + + +def close_gripper(gripper: str): + move_gripper("close", gripper) + + +def move_gripper(cmd: str, gripper: str): + def activate_callback(): + loginfo("Started gripper Movement") + + def done_callback(state, result): + loginfo(f"Reached goal: {result.reached_goal}") + + def feedback_callback(msg): + pass + + goal = GripperCommandGoal() + goal.command.position = 0.0 if cmd == "close" else 0.4 + goal.command.max_effort = 50.0 + if gripper == "right": + controller_topic = "/real/pr2/right_gripper_controller/gripper_cmd" + else: + controller_topic = "/real/pr2/left_gripper_controller/gripper_cmd" + client = actionlib.SimpleActionClient(controller_topic, GripperCommandAction) + loginfo("Waiting for action server") + client.wait_for_server() + client.send_goal(goal, active_cb=activate_callback, done_cb=done_callback, feedback_cb=feedback_callback) + wait = client.wait_for_result() + + +def move_base(pose_stamped: PoseStamped): + giskard.set_cart_goal(pose_stamped, 'base_link', 'map', add_monitor=False) + giskard.execute() + + +def move_left_arm_tool(pose_stamped: PoseStamped): + move_arm_tool(pose_stamped, 'left') + + +def move_right_arm_tool(pose_stamped: PoseStamped): + move_arm_tool(pose_stamped, 'right') + + +def move_arm_tool(pose_stamped: PoseStamped, arm: str): + tool_frame = 'l_gripper_tool_frame' if arm == 'left' else 'r_gripper_tool_frame' + giskard.set_cart_goal(pose_stamped, tool_frame, 'torso_lift_link', add_monitor=False) + giskard.execute() + + +if __name__ == '__main__': + rospy.init_node('test_giskard') + + multiverse_resources = '/home/bassioun/Documents/repos/Multiverse/multiverse/resources/' + cached_dir = multiverse_resources + 'cached/' + + giskard = GiskardWrapper() + + giskard.add_urdf('apartment', + get_urdf_string(cached_dir + 'apartment.urdf'), + get_pose_stamped([0, 0, 0], [0, 0, 0, 1])) + + park_arms() + + move_base(get_pose_stamped([1.17, 2.655, 0], [0.0, 0.0, -0.992, 0.123])) + + open_left_gripper() + + move_left_arm_tool(get_pose_stamped([0.47, 2.48, 1.04], [0, 0, 0, 1])) + + close_left_gripper() + + move_left_arm_tool(get_pose_stamped([0.5, 2.48, 1.04], [0, 0, 0, 1])) + + park_arms() + + move_base(get_pose_stamped([1.4, 3.5, 0], [0.0, 0.0, 0, 1])) diff --git a/demos/pycram_multiverse_demo/world_config.py b/demos/pycram_multiverse_demo/world_config.py new file mode 100644 index 000000000..6a177ac5a --- /dev/null +++ b/demos/pycram_multiverse_demo/world_config.py @@ -0,0 +1,333 @@ +from __future__ import annotations + +import abc +from abc import ABC +from typing import Dict, Optional, Union + +import numpy as np +import rospy +from numpy.typing import NDArray +from std_msgs.msg import ColorRGBA + +from giskardpy.god_map import god_map +from giskardpy.model.joints import FixedJoint, OmniDrive, DiffDrive, Joint6DOF, OneDofJoint +from giskardpy.model.links import Link +from giskardpy.model.utils import robot_name_from_urdf_string +from giskardpy.model.world import WorldTree +from giskardpy.data_types import my_string, PrefixName, Derivatives, derivative_map + + +class WorldConfig(ABC): + + def __init__(self): + god_map.world = WorldTree() + self.set_default_weights() + + @property + def world(self) -> WorldTree: + return god_map.world + + def set_defaults(self): + pass + + @abc.abstractmethod + def setup(self): + """ + Implement this method to configure the initial world using it's self. methods. + """ + + @property + def robot_group_name(self) -> str: + return god_map.world.robot_name + + def set_default_weights(self, + velocity_weight: float = 0.01, + acceleration_weight: float = 0, + jerk_weight: float = 0.01): + """ + The default values are set automatically, even if this function is not called. + A typical goal has a weight of 1, so the values in here should be sufficiently below that. + """ + god_map.world.update_default_weights({Derivatives.velocity: velocity_weight, + Derivatives.acceleration: acceleration_weight, + Derivatives.jerk: jerk_weight}) + + def set_weight(self, weight_map: derivative_map, joint_name: str, group_name: Optional[str] = None): + """ + Set weights for joints that are used by the qp controller. Don't change this unless you know what you are doing. + """ + joint_name = god_map.world.search_for_joint_name(joint_name, group_name) + joint = god_map.world.joints[joint_name] + if not isinstance(joint, OneDofJoint): + raise ValueError(f'Can\'t change weight because {joint_name} is not of type {str(OneDofJoint)}.') + free_variable = god_map.world.free_variables[joint.free_variable.name] + for derivative, weight in weight_map.items(): + free_variable.quadratic_weights[derivative] = weight + + def get_root_link_of_group(self, group_name: str) -> PrefixName: + return god_map.world.groups[group_name].root_link_name + + def set_joint_limits(self, limit_map: derivative_map, joint_name: my_string, group_name: Optional[str] = None): + """ + Set the joint limits for individual joints + :param limit_map: maps Derivatives to values, e.g. {Derivatives.velocity: 1, + Derivatives.acceleration: np.inf, + Derivatives.jerk: 30} + """ + joint_name = god_map.world.search_for_joint_name(joint_name, group_name) + joint = god_map.world.joints[joint_name] + if not isinstance(joint, OneDofJoint): + raise ValueError(f'Can\'t change limits because {joint_name} is not of type {str(OneDofJoint)}.') + free_variable = god_map.world.free_variables[joint.free_variable.name] + for derivative, limit in limit_map.items(): + free_variable.set_lower_limit(derivative, -limit) + free_variable.set_upper_limit(derivative, limit) + + def set_default_color(self, r: float, g: float, b: float, a: float): + """ + :param r: 0-1 + :param g: 0-1 + :param b: 0-1 + :param a: 0-1 + """ + god_map.world.default_link_color = ColorRGBA(r, g, b, a) + + def set_default_limits(self, new_limits: derivative_map): + """ + The default values will be set automatically, even if this function is not called. + :param new_limits: e.g. {Derivatives.velocity: 1, + Derivatives.acceleration: np.inf, + Derivatives.jerk: 30} + """ + god_map.world.update_default_limits(new_limits) + + def add_robot_urdf(self, + urdf: str, + group_name: str) -> str: + """ + Add a robot urdf to the world. + :param urdf: robot urdf as string, not the path + :param group_name: + """ + if group_name is None: + group_name = robot_name_from_urdf_string(urdf) + god_map.world.add_urdf(urdf=urdf, group_name=group_name, actuated=True) + return group_name + + def add_robot_from_parameter_server(self, + parameter_name: str = 'robot_description', + group_name: Optional[str] = None) -> str: + """ + Add a robot urdf from parameter server to Giskard. + :param parameter_name: + :param group_name: How to call the robot. If nothing is specified it will get the name it has in the urdf + """ + urdf = rospy.get_param(parameter_name) + return self.add_robot_urdf(urdf=urdf, group_name=group_name) + + def add_fixed_joint(self, parent_link: my_string, child_link: my_string, + homogenous_transform: Optional[NDArray] = None): + """ + Add a fixed joint to Giskard's world. Can be used to e.g. connect a non-mobile robot to the world frame. + :param parent_link: + :param child_link: + :param homogenous_transform: a 4x4 transformation matrix. + """ + if homogenous_transform is None: + homogenous_transform = np.eye(4) + parent_link = god_map.world.search_for_link_name(parent_link) + + child_link = PrefixName.from_string(child_link, set_none_if_no_slash=True) + joint_name = PrefixName(f'{parent_link}_{child_link}_fixed_joint', None) + joint = FixedJoint(name=joint_name, parent_link_name=parent_link, child_link_name=child_link, + parent_T_child=homogenous_transform) + god_map.world._add_joint(joint) + + def add_diff_drive_joint(self, + name: str, + parent_link_name: my_string, + child_link_name: my_string, + robot_group_name: Optional[str] = None, + translation_limits: Optional[derivative_map] = None, + rotation_limits: Optional[derivative_map] = None): + """ + Same as add_omni_drive_joint, but for a differential drive. + """ + joint_name = PrefixName(name, robot_group_name) + parent_link_name = PrefixName.from_string(parent_link_name, set_none_if_no_slash=True) + child_link_name = PrefixName.from_string(child_link_name, set_none_if_no_slash=True) + brumbrum_joint = DiffDrive(parent_link_name=parent_link_name, + child_link_name=child_link_name, + name=joint_name, + translation_limits=translation_limits, + rotation_limits=rotation_limits) + god_map.world._add_joint(brumbrum_joint) + god_map.world.deregister_group(robot_group_name) + god_map.world.register_group(robot_group_name, root_link_name=parent_link_name, actuated=True) + + def add_6dof_joint(self, parent_link: my_string, child_link: my_string, joint_name: my_string): + """ + Add a 6dof joint to Giskard's world. Generally used if you want Giskard to keep track of a tf transform, + e.g. for localization. + :param parent_link: + :param child_link: + """ + parent_link = god_map.world.search_for_link_name(parent_link) + child_link = PrefixName.from_string(child_link, set_none_if_no_slash=True) + joint_name = PrefixName.from_string(joint_name, set_none_if_no_slash=True) + joint = Joint6DOF(name=joint_name, parent_link_name=parent_link, child_link_name=child_link) + god_map.world._add_joint(joint) + + def add_empty_link(self, link_name: my_string): + """ + If you need a virtual link during your world building. + """ + link = Link(link_name) + god_map.world._add_link(link) + + def add_omni_drive_joint(self, + name: str, + parent_link_name: Union[str, PrefixName], + child_link_name: Union[str, PrefixName], + robot_group_name: Optional[str] = None, + translation_limits: Optional[derivative_map] = None, + rotation_limits: Optional[derivative_map] = None, + x_name: Optional[PrefixName] = None, + y_name: Optional[PrefixName] = None, + yaw_vel_name: Optional[PrefixName] = None): + """ + Use this to connect a robot urdf of a mobile robot to the world if it has an omni-directional drive. + :param parent_link_name: + :param child_link_name: + :param robot_group_name: set if there are multiple robots + :param name: Name of the new link. Has to be unique and may be required in other functions. + :param translation_limits: in m/s**3 + :param rotation_limits: in rad/s**3 + """ + joint_name = PrefixName(name, robot_group_name) + parent_link_name = PrefixName.from_string(parent_link_name, set_none_if_no_slash=True) + child_link_name = PrefixName.from_string(child_link_name, set_none_if_no_slash=True) + brumbrum_joint = OmniDrive(parent_link_name=parent_link_name, + child_link_name=child_link_name, + name=joint_name, + translation_limits=translation_limits, + rotation_limits=rotation_limits, + x_name=x_name, + y_name=y_name, + yaw_name=yaw_vel_name) + god_map.world._add_joint(brumbrum_joint) + god_map.world.deregister_group(robot_group_name) + god_map.world.register_group(robot_group_name, root_link_name=parent_link_name, actuated=True) + + +class EmptyWorld(WorldConfig): + def __init__(self): + super().__init__() + + def setup(self): + self._default_limits = { + Derivatives.velocity: 1, + } + self.set_default_weights(velocity_weight=1, acceleration_weight=1, jerk_weight=1) + self.set_default_limits(self._default_limits) + self.add_empty_link('map') + + +class WorldWithFixedRobot(WorldConfig): + def __init__(self, joint_limits: Dict[Derivatives, float] = None): + super().__init__() + self._joint_limits = joint_limits + + def setup(self): + self.set_default_limits(self._joint_limits) + self.add_robot_from_parameter_server() + + +class WorldWithOmniDriveRobot(WorldConfig): + map_name: str + localization_joint_name: str + odom_link_name: str + drive_joint_name: str + + def __init__(self, + map_name: str = 'map', + localization_joint_name: str = 'localization', + odom_link_name: str = 'odom', + drive_joint_name: str = 'brumbrum'): + super().__init__() + self.map_name = map_name + self.localization_joint_name = localization_joint_name + self.odom_link_name = odom_link_name + self.drive_joint_name = drive_joint_name + + def setup(self): + self.set_default_limits({Derivatives.velocity: 1, + Derivatives.acceleration: np.inf, + Derivatives.jerk: 30}) + self.add_empty_link(self.map_name) + self.add_empty_link(self.odom_link_name) + self.add_fixed_joint(parent_link=self.map_name, child_link=self.odom_link_name) + # self.add_6dof_joint(parent_link=self.map_name, child_link=self.odom_link_name, + # joint_name=self.localization_joint_name) + self.add_robot_from_parameter_server() + root_link_name = self.get_root_link_of_group(self.robot_group_name) + self.add_omni_drive_joint(name=self.drive_joint_name, + parent_link_name=self.odom_link_name, + child_link_name=root_link_name, + translation_limits={ + Derivatives.velocity: 0.2, + Derivatives.acceleration: 1, + Derivatives.jerk: 5, + }, + rotation_limits={ + Derivatives.velocity: 0.2, + Derivatives.acceleration: 1, + Derivatives.jerk: 5 + }, + robot_group_name=self.robot_group_name) + self.set_joint_limits(limit_map={Derivatives.velocity: 3, + Derivatives.jerk: 60}, + joint_name='head_pan_joint') + + +class WorldWithDiffDriveRobot(WorldConfig): + map_name: str + localization_joint_name: str + odom_link_name: str + drive_joint_name: str + + def __init__(self, + map_name: str = 'map', + localization_joint_name: str = 'localization', + odom_link_name: str = 'odom', + drive_joint_name: str = 'brumbrum'): + super().__init__() + self.map_name = map_name + self.localization_joint_name = localization_joint_name + self.odom_link_name = odom_link_name + self.drive_joint_name = drive_joint_name + + def setup(self): + self.set_default_limits({Derivatives.velocity: 1, + Derivatives.acceleration: np.inf, + Derivatives.jerk: 30}) + self.add_empty_link(self.map_name) + self.add_empty_link(self.odom_link_name) + self.add_6dof_joint(parent_link=self.map_name, child_link=self.odom_link_name, + joint_name=self.localization_joint_name) + self.add_robot_from_parameter_server() + root_link_name = self.get_root_link_of_group(self.robot_group_name) + self.add_diff_drive_joint(name=self.drive_joint_name, + parent_link_name=self.odom_link_name, + child_link_name=root_link_name, + translation_limits={ + Derivatives.velocity: 0.2, + Derivatives.acceleration: 1, + Derivatives.jerk: 5, + }, + rotation_limits={ + Derivatives.velocity: 0.2, + Derivatives.acceleration: 1, + Derivatives.jerk: 5 + }, + robot_group_name=self.robot_group_name) diff --git a/src/pycram/designators/location_designator.py b/src/pycram/designators/location_designator.py index 395a46c2f..7f529799e 100644 --- a/src/pycram/designators/location_designator.py +++ b/src/pycram/designators/location_designator.py @@ -6,7 +6,8 @@ from .object_designator import ObjectDesignatorDescription, ObjectPart from ..datastructures.world import World, UseProspectionWorld from ..local_transformer import LocalTransformer -from ..world_reasoning import link_pose_for_joint_config +from ..world_concepts.world_object import Object +from ..world_reasoning import link_pose_for_joint_config, contact from ..designator import DesignatorError, LocationDesignatorDescription from ..costmaps import OccupancyCostmap, VisibilityCostmap, SemanticCostmap, GaussianCostmap from ..datastructures.enums import JointType, Arms @@ -112,7 +113,8 @@ class Location(LocationDesignatorDescription.Location): def __init__(self, target: Union[Pose, ObjectDesignatorDescription.Object], reachable_for: Optional[ObjectDesignatorDescription.Object] = None, visible_for: Optional[ObjectDesignatorDescription.Object] = None, - reachable_arm: Optional[Arms] = None): + reachable_arm: Optional[Arms] = None, + check_collision_at_start: bool = False): """ Location designator that uses costmaps as base to calculate locations for complex constrains like reachable or visible. In case of reachable the resolved location contains a list of arms with which the location is reachable. @@ -121,12 +123,14 @@ def __init__(self, target: Union[Pose, ObjectDesignatorDescription.Object], :param reachable_for: Object for which the reachability should be calculated, usually a robot :param visible_for: Object for which the visibility should be calculated, usually a robot :param reachable_arm: An optional arm with which the target should be reached + :param check_collision_at_start: If True, the designator will check for collisions at the start pose. """ super().__init__() self.target: Union[Pose, ObjectDesignatorDescription.Object] = target self.reachable_for: ObjectDesignatorDescription.Object = reachable_for self.visible_for: ObjectDesignatorDescription.Object = visible_for self.reachable_arm: Optional[Arms] = reachable_arm + self.check_collision_at_start = check_collision_at_start def ground(self) -> Location: """ @@ -136,6 +140,24 @@ def ground(self) -> Location: """ return next(iter(self)) + @staticmethod + def check_for_collision(robot: Object, pose: Pose) -> bool: + """ + Check if the robot collides with any object in the world at the given pose. + + :param robot: The robot object + :param pose: The pose to check for collision + :return: True if the robot collides with any object, False otherwise + """ + robot.set_pose(pose) + floor = robot.world.get_object_by_name("floor") + for obj in robot.world.objects: + if obj in [robot, floor]: + continue + if contact(robot, obj): + return True + return False + def __iter__(self): """ Generates positions for a given set of constrains from a costmap and returns @@ -171,12 +193,16 @@ def __iter__(self): visible = VisibilityCostmap(min_height, max_height, 200, 0.02, Pose(target_pose.position_as_list())) final_map += visible + test_robot = None if self.visible_for or self.reachable_for: robot_object = self.visible_for.world_object if self.visible_for else self.reachable_for.world_object test_robot = World.current_world.get_prospection_object_for_object(robot_object) with UseProspectionWorld(): for maybe_pose in PoseGenerator(final_map, number_of_samples=600): + if self.check_collision_at_start and (test_robot is not None): + if self.check_for_collision(test_robot, maybe_pose): + continue res = True arms = None if self.visible_for: @@ -185,10 +211,14 @@ def __iter__(self): World.current_world) if self.reachable_for: hand_links = [] - for description in RobotDescription.current_robot_description.get_manipulator_chains(): - hand_links += description.end_effector.links + if self.reachable_arm is not None: + hand_links = RobotDescription.current_robot_description.get_arm_chain(self.reachable_arm).end_effector.links + else: + for description in RobotDescription.current_robot_description.get_manipulator_chains(): + hand_links += description.end_effector.links valid, arms = reachability_validator(maybe_pose, test_robot, target_pose, - allowed_collision={test_robot: hand_links}) + allowed_collision={test_robot: hand_links}, + arm=self.reachable_arm) if self.reachable_arm: res = res and valid and self.reachable_arm in arms else: diff --git a/src/pycram/designators/motion_designator.py b/src/pycram/designators/motion_designator.py index d23b0521b..a743744c0 100644 --- a/src/pycram/designators/motion_designator.py +++ b/src/pycram/designators/motion_designator.py @@ -5,9 +5,7 @@ from pycrap import PhysicalObject, Location from .object_designator import ObjectDesignatorDescription, ObjectPart, RealObject -from ..datastructures.enums import ObjectType, Arms, GripperState, ExecutionType, MovementType -from ..designator import ResolutionError -from ..orm.base import ProcessMetaData +from ..datastructures.enums import MovementType from ..failures import PerceptionObjectNotFound from ..process_module import ProcessModuleManager from ..orm.motion_designator import (MoveMotion as ORMMoveMotion, @@ -17,7 +15,7 @@ Motion as ORMMotionDesignator) from ..datastructures.enums import ObjectType, Arms, GripperState, ExecutionType, DetectionTechnique, DetectionState -from typing_extensions import Dict, Optional, get_type_hints, Type, Any +from typing_extensions import Dict, Optional, Type from ..datastructures.pose import Pose from ..tasktree import with_tree from ..designator import BaseMotion diff --git a/src/pycram/external_interfaces/giskard.py b/src/pycram/external_interfaces/giskard.py index 483dc66f9..c594660d6 100644 --- a/src/pycram/external_interfaces/giskard.py +++ b/src/pycram/external_interfaces/giskard.py @@ -119,11 +119,13 @@ def removing_of_objects() -> None: @init_giskard_interface -def sync_worlds() -> None: +def sync_worlds(projection: bool = False) -> None: """ Synchronizes the World and the Giskard belief state, this includes adding and removing objects to the Giskard belief state such that it matches the objects present in the World and moving the robot to the position it is currently at in the World. + + :param projection: Whether the sync in projection mode or reality. """ add_gripper_groups() world_object_names = set() @@ -136,10 +138,11 @@ def sync_worlds() -> None: obj.joints.values())) joint_config_filtered = {joint.name: joint_config[joint.name] for joint in non_fixed_joints} - giskard_wrapper.monitors.add_set_seed_configuration(joint_config_filtered, - RobotDescription.current_robot_description.name) - giskard_wrapper.monitors.add_set_seed_odometry(_pose_to_pose_stamped(obj.get_pose()), - RobotDescription.current_robot_description.name) + if projection: + giskard_wrapper.monitors.add_set_seed_configuration(joint_config_filtered, + RobotDescription.current_robot_description.name) + giskard_wrapper.monitors.add_set_seed_odometry(_pose_to_pose_stamped(obj.get_pose()), + RobotDescription.current_robot_description.name) giskard_object_names = set(giskard_wrapper.get_group_names()) robot_name = {RobotDescription.current_robot_description.name} if not world_object_names.union(robot_name).issubset(giskard_object_names): @@ -554,7 +557,7 @@ def projection_cartesian_goal(goal_pose: Pose, tip_link: str, root_link: str) -> :param root_link: The starting link of the chain which should be used to achieve this goal :return: MoveResult message for this goal """ - sync_worlds() + sync_worlds(projection=True) giskard_wrapper.set_cart_goal(_pose_to_pose_stamped(goal_pose), tip_link, root_link) return giskard_wrapper.projection() @@ -573,7 +576,7 @@ def projection_cartesian_goal_with_approach(approach_pose: Pose, goal_pose: Pose :param robot_base_link: The base link of the robot :return: A trajectory calculated to move the tip_link to the goal_pose """ - sync_worlds() + sync_worlds(projection=True) giskard_wrapper.allow_all_collisions() giskard_wrapper.set_cart_goal(_pose_to_pose_stamped(approach_pose), robot_base_link, "map") giskard_wrapper.projection() @@ -592,7 +595,7 @@ def projection_joint_goal(goal_poses: Dict[str, float], allow_collisions: bool = :param allow_collisions: If all collisions should be allowed for this goal :return: MoveResult message for this goal """ - sync_worlds() + sync_worlds(projection=True) if allow_collisions: giskard_wrapper.allow_all_collisions() giskard_wrapper.set_joint_goal(goal_poses) diff --git a/src/pycram/external_interfaces/ik.py b/src/pycram/external_interfaces/ik.py index 9510b2cb3..5a89a679f 100644 --- a/src/pycram/external_interfaces/ik.py +++ b/src/pycram/external_interfaces/ik.py @@ -171,7 +171,7 @@ def request_ik(target_pose: Pose, robot: Object, joints: List[str], gripper: str :param gripper: Name of the tool frame which should grasp, this should be at the end of the given joint chain :return: A Pose at which the robt should stand as well as a dictionary of joint values """ - if "/giskard" not in rosnode.get_node_names() or True: + if "/giskard" not in rosnode.get_node_names(): return robot.pose, request_kdl_ik(target_pose, robot, joints, gripper) return request_giskard_ik(target_pose, robot, gripper) diff --git a/src/pycram/pose_generator_and_validator.py b/src/pycram/pose_generator_and_validator.py index 1232ee5b0..abcbe6454 100644 --- a/src/pycram/pose_generator_and_validator.py +++ b/src/pycram/pose_generator_and_validator.py @@ -1,7 +1,8 @@ import numpy as np import tf -from typing_extensions import Tuple, List, Union, Dict, Iterable +from typing_extensions import Tuple, List, Union, Dict, Iterable, Optional +from .datastructures.enums import Arms from .costmaps import Costmap from .datastructures.pose import Pose, Transform from .datastructures.world import World @@ -161,7 +162,8 @@ def _in_contact(robot: Object, obj: Object, allowed_collision: Dict[Object, List def reachability_validator(pose: Pose, robot: Object, target: Union[Object, Pose], - allowed_collision: Dict[Object, List] = None) -> Tuple[bool, List]: + allowed_collision: Dict[Object, List] = None, + arm: Optional[Arms] = None) -> Tuple[bool, List]: """ This method validates if a target position is reachable for a pose candidate. This is done by asking the ik solver if there is a valid solution if the @@ -172,6 +174,7 @@ def reachability_validator(pose: Pose, :param robot: The robot object in the World for which the reachability should be validated. :param target: The target position or object instance which should be the target for reachability. :param allowed_collision: dict of objects with which the robot is allowed to collide each object correlates to a list of links of which this object consists + :param arm: The arm that should be used for the reachability check. If None all arms are checked. :return: True if the target is reachable for the robot and False in any other case. """ if type(target) == Object: @@ -180,7 +183,10 @@ def reachability_validator(pose: Pose, robot.set_pose(pose) # manipulator_descs = list( # filter(lambda chain: isinstance(chain[1], ManipulatorDescription), robot_description.chains.items())) - manipulator_descs = RobotDescription.current_robot_description.get_manipulator_chains() + if arm is not None: + manipulator_descs = [RobotDescription.current_robot_description.get_arm_chain(arm)] + else: + manipulator_descs = RobotDescription.current_robot_description.get_manipulator_chains() # TODO Make orientation adhere to grasping orientation res = False diff --git a/src/pycram/process_modules/pr2_process_modules.py b/src/pycram/process_modules/pr2_process_modules.py index ecb51f7fd..b6aa6791c 100644 --- a/src/pycram/process_modules/pr2_process_modules.py +++ b/src/pycram/process_modules/pr2_process_modules.py @@ -1,5 +1,4 @@ -from threading import Lock -from typing_extensions import Any, TYPE_CHECKING +from typing_extensions import TYPE_CHECKING import actionlib @@ -15,6 +14,7 @@ from ..utils import _apply_ik from ..local_transformer import LocalTransformer +from .. import world_reasoning as btr from ..designators.motion_designator import MoveMotion, LookingMotion, \ DetectingMotion, MoveTCPMotion, MoveArmJointsMotion, WorldStateDetectingMotion, MoveJointsMotion, \ MoveGripperMotion, OpeningMotion, ClosingMotion @@ -31,13 +31,17 @@ from ..designators.object_designator import ObjectDesignatorDescription try: - from ..worlds import Multiverse + from ..worlds.multiverse import Multiverse except ImportError: Multiverse = type(None) try: - from pr2_controllers_msgs.msg import Pr2GripperCommandGoal, Pr2GripperCommandAction, Pr2 from control_msgs.msg import GripperCommandGoal, GripperCommandAction +except ImportError: + logwarn("control_msgs import failed") + +try: + from pr2_controllers_msgs.msg import Pr2GripperCommandGoal, Pr2GripperCommandAction, Pr2 except ImportError: logdebug("Pr2GripperCommandGoal not found") @@ -285,7 +289,7 @@ def feedback_callback(msg): pass goal = GripperCommandGoal() - goal.command.position = 0.0 if designator.motion == "close" else 0.1 + goal.command.position = 0.0 if designator.motion == GripperState.CLOSE else 0.4 goal.command.max_effort = 50.0 if designator.gripper == "right": controller_topic = "/real/pr2/right_gripper_controller/gripper_cmd" @@ -298,7 +302,6 @@ def feedback_callback(msg): wait = client.wait_for_result() - class Pr2MoveGripperReal(ProcessModule): """ Opens or closes the gripper of the real PR2, gripper uses an action server for this instead of giskard diff --git a/src/pycram/worlds/multiverse.py b/src/pycram/worlds/multiverse.py index c69a90149..f65413bb2 100644 --- a/src/pycram/worlds/multiverse.py +++ b/src/pycram/worlds/multiverse.py @@ -545,7 +545,12 @@ def get_object_contact_points(self, obj: Object) -> ContactPointsList: """ Note: Currently Multiverse only gets one contact point per contact objects. """ - contact_bodies = self.api_requester.get_contact_bodies_of_object(obj) + if self.is_paused: + self.unpause_simulation() + contact_bodies = self.api_requester.get_contact_bodies_of_object(obj) + self.pause_simulation() + else: + contact_bodies = self.api_requester.get_contact_bodies_of_object(obj) contact_points = ContactPointsList([]) for body_name in contact_bodies: multiverse_contact_points = self.api_requester.get_contact_points_between_objects(obj.name, body_name) From 401306f07043140de9e8434faa5528f84ee53086 Mon Sep 17 00:00:00 2001 From: A_Bassiouny Date: Sun, 10 Nov 2024 19:57:14 +0100 Subject: [PATCH 32/91] [MultiverseFallschoolDemo] testing transport action. --- .../pycram_multiverse_demo/fallschool_demo.py | 33 ++++++++++--------- 1 file changed, 17 insertions(+), 16 deletions(-) diff --git a/demos/pycram_multiverse_demo/fallschool_demo.py b/demos/pycram_multiverse_demo/fallschool_demo.py index ea81e8df2..85266196b 100644 --- a/demos/pycram_multiverse_demo/fallschool_demo.py +++ b/demos/pycram_multiverse_demo/fallschool_demo.py @@ -5,6 +5,7 @@ from pycram.datastructures.dataclasses import Color from pycram.datastructures.enums import ObjectType, Arms from pycram.datastructures.pose import Pose +from pycram.datastructures.world import UseProspectionWorld, World from pycram.designators.action_designator import ParkArmsAction, MoveTorsoAction, TransportAction, NavigateAction, \ LookAtAction, DetectAction from pycram.designators.object_designator import BelieveObject @@ -31,10 +32,10 @@ def move_and_detect(obj_type: ObjectType, pick_pose: Pose): WorldStateUpdater(tf_topic="/tf", joint_state_topic="/real/pr2/joint_states", update_rate=timedelta(seconds=1), world=world) apartment = Object("apartment", ObjectType.ENVIRONMENT, f"apartment.urdf") -milk = Object("milk", ObjectType.MILK, f"milk.stl", pose=Pose([0.4, 2.6, 1.34], +milk = Object("milk", ObjectType.MILK, f"milk.xml", pose=Pose([0.4, 2.6, 1.34], [1, 0, 0, 0]), color=Color(1, 0, 0, 1)) -apartment.set_joint_position("fridge_door1_joint", 1.5707963267948966) +# apartment.set_joint_position("fridge_door1_joint", 1.5707963267948966) # milk.set_orientation(Pose(orientation=[1, 0, 0, 1])) # apartment.attach(milk, 'fridge_base') fridge_base_pose = apartment.get_link_pose("fridge_base") @@ -42,8 +43,8 @@ def move_and_detect(obj_type: ObjectType, pick_pose: Pose): fridge_base_pose.position.x += 0.16 fridge_base_pose.position.y += -0.1 milk.set_pose(fridge_base_pose, base=True) -print(milk.get_position_as_list()) -print(milk.get_orientation_as_list()) +# print(milk.get_position_as_list()) +# print(milk.get_orientation_as_list()) robot_desig = BelieveObject(names=[robot.name]) @@ -52,23 +53,23 @@ def move_and_detect(obj_type: ObjectType, pick_pose: Pose): with real_robot: # Transport the milkMoveGripperMotion - ParkArmsAction([Arms.BOTH]).resolve().perform() - - MoveTorsoAction([0.2]).resolve().perform() - - NavigateAction(target_locations=[Pose([1.1, 3.15, 0.01], quaternion_from_euler(0, 0, -2.9))]).resolve().perform() - - LookAtAction(targets=[Pose(milk.get_position_as_list())]).resolve().perform() + # ParkArmsAction([Arms.BOTH]).resolve().perform() + # + # MoveTorsoAction([0.2]).resolve().perform() + # + # NavigateAction(target_locations=[Pose([1.1, 3.15, 0.01], quaternion_from_euler(0, 0, -2.9))]).resolve().perform() + # + # LookAtAction(targets=[Pose(milk.get_position_as_list())]).resolve().perform() # world.restore_physics_simulator_state(100) - try: - milk_desig = DetectAction(BelieveObject(types=[milk.obj_type])).resolve().perform() - except PerceptionObjectNotFound: - milk_desig = DetectAction(BelieveObject(types=[milk.obj_type])).resolve().perform() + # try: + # milk_desig = DetectAction(BelieveObject(types=[milk.obj_type])).resolve().perform() + # except PerceptionObjectNotFound: + # milk_desig = DetectAction(BelieveObject(types=[milk.obj_type])).resolve().perform() # world.save_state(100) - + milk_desig = BelieveObject(names=[milk.name]) TransportAction(milk_desig, [Arms.LEFT], [Pose([2.4, 3, 1.02])]).resolve().perform() ParkArmsAction([Arms.BOTH]).resolve().perform() From c23dbb082f84ac15dfd2fac9e36738e173e8b055 Mon Sep 17 00:00:00 2001 From: A_Bassiouny Date: Sun, 10 Nov 2024 20:00:25 +0100 Subject: [PATCH 33/91] [Multiverse] corrected multiverse contact bodies in the case of multi link objects. Added a failure for api request failure. --- src/pycram/failures.py | 10 +++++-- src/pycram/worlds/multiverse.py | 26 +++++++++---------- .../multiverse_communication/clients.py | 22 +++++++++++++--- test/test_multiverse.py | 15 +++++++++++ 4 files changed, 55 insertions(+), 18 deletions(-) diff --git a/src/pycram/failures.py b/src/pycram/failures.py index 8a621536b..4e86640d0 100644 --- a/src/pycram/failures.py +++ b/src/pycram/failures.py @@ -1,10 +1,11 @@ +from __future__ import annotations from pathlib import Path from typing_extensions import TYPE_CHECKING, List if TYPE_CHECKING: from .world_concepts.world_object import Object - from .datastructures.enums import JointType + from .datastructures.enums import JointType, MultiverseAPIName class PlanFailure(Exception): @@ -435,6 +436,12 @@ def __init__(self, *args, **kwargs): super().__init__(*args, **kwargs) +class FailedAPIResponse(Exception): + def __init__(self, api_response: List[str], api_name: MultiverseAPIName, *args, **kwargs): + super().__init__(f"{api_name} api request with arguments {args} and keyword arguments {kwargs}" + f" failed with response {api_response}") + + """ The following exceptions are used in the PyCRAM framework to handle errors related to the world and the objects in it. They are usually related to a bug in the code or a misuse of the framework (e.g. logical errors in the code). @@ -505,4 +512,3 @@ def __init__(self, link_name: str): class LinkGeometryHasNoMesh(Exception): def __init__(self, link_name: str, geometry_type: str): super().__init__(f"Link {link_name} geometry with type {geometry_type} has no mesh.") - diff --git a/src/pycram/worlds/multiverse.py b/src/pycram/worlds/multiverse.py index f65413bb2..d4af76e3d 100644 --- a/src/pycram/worlds/multiverse.py +++ b/src/pycram/worlds/multiverse.py @@ -18,6 +18,7 @@ from ..datastructures.pose import Pose from ..datastructures.world import World from ..description import Link, Joint +from ..failures import FailedAPIResponse from ..object_descriptors.mjcf import ObjectDescription as MJCF, ObjectFactory, PrimitiveObjectFactory from ..object_descriptors.generic import ObjectDescription as GenericObjectDescription from ..robot_description import RobotDescription @@ -545,23 +546,22 @@ def get_object_contact_points(self, obj: Object) -> ContactPointsList: """ Note: Currently Multiverse only gets one contact point per contact objects. """ - if self.is_paused: - self.unpause_simulation() - contact_bodies = self.api_requester.get_contact_bodies_of_object(obj) - self.pause_simulation() - else: - contact_bodies = self.api_requester.get_contact_bodies_of_object(obj) + contact_bodies = self.api_requester.get_contact_bodies_of_object(obj) contact_points = ContactPointsList([]) for body_name in contact_bodies: - multiverse_contact_points = self.api_requester.get_contact_points_between_objects(obj.name, body_name) body_object, body_link = self.get_object_with_body_name(body_name) - if body_link is None: - logerr(f"Body link not found: {body_name}") - raise ValueError(f"Body link not found: {body_name}") - for point in multiverse_contact_points: + if body_object is None: + logerr(f"Body Object not found: {body_name}") + raise ValueError(f"Body Object not found: {body_name}") + multiverse_contact_points = self.api_requester.get_contact_points_between_objects(obj.name, + body_name) + if len(multiverse_contact_points) == 0: contact_points.append(ContactPoint(obj.root_link, body_link)) - contact_points[-1].normal_on_b = point.normal - contact_points[-1].position_on_b = point.position + else: + for point in multiverse_contact_points: + contact_points.append(ContactPoint(obj.root_link, body_link)) + contact_points[-1].normal_on_b = point.normal + contact_points[-1].position_on_b = point.position return contact_points def get_object_with_body_name(self, body_name: str) -> Tuple[Optional[Object], Optional[Link]]: diff --git a/src/pycram/worlds/multiverse_communication/clients.py b/src/pycram/worlds/multiverse_communication/clients.py index ae93e7843..9c68c603a 100644 --- a/src/pycram/worlds/multiverse_communication/clients.py +++ b/src/pycram/worlds/multiverse_communication/clients.py @@ -12,7 +12,8 @@ from ...datastructures.enums import (MultiverseAPIName as API, MultiverseBodyProperty as BodyProperty, MultiverseProperty as Property) from ...datastructures.pose import Pose -from ...ros.logging import logwarn +from ...failures import FailedAPIResponse +from ...ros.logging import logwarn, logerr from ...utils import wxyz_to_xyzw from ...world_concepts.constraints import Constraint from ...world_concepts.world_object import Object, Link @@ -772,7 +773,7 @@ def get_contact_bodies_of_object(self, obj: Object) -> List[str]: :param obj: The object. :return: The names of the bodies/objects that are in contact with the object. """ - return self._request_single_api_callback(API.GET_CONTACT_BODIES, obj.name) + return self._request_single_api_callback(API.GET_CONTACT_BODIES, obj.name, "with_children") def get_contact_points_of_object(self, obj: Object) -> List[MultiverseContactPoint]: """ @@ -839,12 +840,27 @@ def _request_apis_callbacks(self, api_data: Dict[API, List]) -> Dict[API, List[s for api_name, params in api_data.items(): self._add_api_request(api_name.value, *params) self._send_api_request() - responses = self._get_all_apis_responses() if self.wait: sleep(self.API_REQUEST_WAIT_TIME.total_seconds() * self.simulation_wait_time_factor) self.wait = False + responses = self._get_all_apis_responses() + self.validate_apis_response(api_data, responses) return responses + @staticmethod + def validate_apis_response(api_data: Dict[API, List], responses: Dict[API, List[str]]): + """ + Validate the responses from the multiverse server and raise error if an api request failed. + + :param api_data: The data of the api request which has the api name and the arguments. + :param responses: The responses of the given api requests. + :raises FailedAPIResponse: when one of the responses reports that the request failed. + """ + for api_name, response in responses.items(): + for val in response: + if 'failed' in val: + raise FailedAPIResponse(response, api_name, api_data[api_name]) + def _get_all_apis_responses(self) -> Dict[API, List[str]]: """ Get all the API responses from the server. diff --git a/test/test_multiverse.py b/test/test_multiverse.py index 4e9e3ae70..0a5bffa1c 100644 --- a/test/test_multiverse.py +++ b/test/test_multiverse.py @@ -388,6 +388,14 @@ def test_get_object_contact_points(self): self.assertTrue(contact_points[0].link_b.object, milk) self.tearDown() + def test_get_robot_contact_points(self): + robot = self.spawn_robot([0.9345829872370865, 1.9027591011850133, 0.0], + quaternion_from_euler(0, 0, 2.26).tolist(), + robot_name="pr2") + apartment = self.spawn_apartment() + contact_points = self.multiverse.get_contact_points_between_two_objects(robot, apartment) + self.assertTrue(len(contact_points) > 0) + def test_get_contact_points_between_two_objects(self): for i in range(3): milk = self.spawn_milk([1, 1, 0.01], [0, -0.707, 0, 0.707]) @@ -431,6 +439,13 @@ def spawn_milk(position: List, orientation: Optional[List] = None, frame="map") pose=Pose(position, orientation, frame=frame)) return milk + def spawn_apartment(self) -> Object: + if "apartment" not in self.multiverse.get_object_names(): + apartment = Object("apartment", ObjectType.ENVIRONMENT, f"apartment.urdf") + else: + apartment = self.multiverse.get_object_by_name("apartment") + return apartment + def spawn_robot(self, position: Optional[List[float]] = None, orientation: Optional[List[float]] = None, robot_name: Optional[str] = 'tiago_dual', From 4a7b00c45efd12faaeb7e9a090c16c5e10d26145 Mon Sep 17 00:00:00 2001 From: A_Bassiouny Date: Sun, 10 Nov 2024 20:01:40 +0100 Subject: [PATCH 34/91] [Designator] Added some debug logging. check reachability of only the specified arm. --- src/pycram/designators/action_designator.py | 2 +- src/pycram/designators/location_designator.py | 23 +++++++++++-------- 2 files changed, 15 insertions(+), 10 deletions(-) diff --git a/src/pycram/designators/action_designator.py b/src/pycram/designators/action_designator.py index 6e7b71feb..6ab346dc5 100644 --- a/src/pycram/designators/action_designator.py +++ b/src/pycram/designators/action_designator.py @@ -8,7 +8,7 @@ import numpy as np from sqlalchemy.orm import Session from tf import transformations -from typing_extensions import List, Union, Callable, Optional, Type +from typing_extensions import List, Union, Optional, Type from pycrap import PhysicalObject, Location from .location_designator import CostmapLocation diff --git a/src/pycram/designators/location_designator.py b/src/pycram/designators/location_designator.py index 7f529799e..82c0e7ac0 100644 --- a/src/pycram/designators/location_designator.py +++ b/src/pycram/designators/location_designator.py @@ -1,19 +1,19 @@ import dataclasses -import time from typing_extensions import List, Union, Iterable, Optional, Callable from .object_designator import ObjectDesignatorDescription, ObjectPart -from ..datastructures.world import World, UseProspectionWorld -from ..local_transformer import LocalTransformer -from ..world_concepts.world_object import Object -from ..world_reasoning import link_pose_for_joint_config, contact -from ..designator import DesignatorError, LocationDesignatorDescription from ..costmaps import OccupancyCostmap, VisibilityCostmap, SemanticCostmap, GaussianCostmap from ..datastructures.enums import JointType, Arms +from ..datastructures.pose import Pose +from ..datastructures.world import World, UseProspectionWorld +from ..designator import DesignatorError, LocationDesignatorDescription +from ..local_transformer import LocalTransformer from ..pose_generator_and_validator import PoseGenerator, visibility_validator, reachability_validator from ..robot_description import RobotDescription -from ..datastructures.pose import Pose +from ..ros.logging import logdebug +from ..world_concepts.world_object import Object +from ..world_reasoning import link_pose_for_joint_config, contact class Location(LocationDesignatorDescription): @@ -114,7 +114,7 @@ def __init__(self, target: Union[Pose, ObjectDesignatorDescription.Object], reachable_for: Optional[ObjectDesignatorDescription.Object] = None, visible_for: Optional[ObjectDesignatorDescription.Object] = None, reachable_arm: Optional[Arms] = None, - check_collision_at_start: bool = False): + check_collision_at_start: bool = True): """ Location designator that uses costmaps as base to calculate locations for complex constrains like reachable or visible. In case of reachable the resolved location contains a list of arms with which the location is reachable. @@ -155,7 +155,11 @@ def check_for_collision(robot: Object, pose: Pose) -> bool: if obj in [robot, floor]: continue if contact(robot, obj): + logdebug(f"Robot is in contact with {obj.name} in prospection: {obj.world.is_prospection_world}" + f"at position {pose.position_as_list()} and z_angle {pose.z_angle}") return True + logdebug(f"Robot is not in contact with {obj.name} in prospection: {obj.world.is_prospection_world}" + f"at position {pose.position_as_list()} and z_angle {pose.z_angle}") return False def __iter__(self): @@ -212,7 +216,8 @@ def __iter__(self): if self.reachable_for: hand_links = [] if self.reachable_arm is not None: - hand_links = RobotDescription.current_robot_description.get_arm_chain(self.reachable_arm).end_effector.links + hand_links = RobotDescription.current_robot_description.get_arm_chain( + self.reachable_arm).end_effector.links else: for description in RobotDescription.current_robot_description.get_manipulator_chains(): hand_links += description.end_effector.links From f41f5d7dc68b9361b763317aa9fd48be59d39a27 Mon Sep 17 00:00:00 2001 From: Abdelrhman Bassiouny Date: Mon, 11 Nov 2024 10:49:57 +0100 Subject: [PATCH 35/91] [Multiverse] Can get all contact links. --- src/pycram/worlds/multiverse.py | 25 ++++++++++++------- .../multiverse_communication/clients.py | 9 +++++++ 2 files changed, 25 insertions(+), 9 deletions(-) diff --git a/src/pycram/worlds/multiverse.py b/src/pycram/worlds/multiverse.py index d4af76e3d..f454a7e5b 100644 --- a/src/pycram/worlds/multiverse.py +++ b/src/pycram/worlds/multiverse.py @@ -553,15 +553,22 @@ def get_object_contact_points(self, obj: Object) -> ContactPointsList: if body_object is None: logerr(f"Body Object not found: {body_name}") raise ValueError(f"Body Object not found: {body_name}") - multiverse_contact_points = self.api_requester.get_contact_points_between_objects(obj.name, - body_name) - if len(multiverse_contact_points) == 0: - contact_points.append(ContactPoint(obj.root_link, body_link)) - else: - for point in multiverse_contact_points: - contact_points.append(ContactPoint(obj.root_link, body_link)) - contact_points[-1].normal_on_b = point.normal - contact_points[-1].position_on_b = point.position + # To get the links of obj that are in contact with body_link, we need to get the contact bodies of the + # body_link and check if they are in obj. + body_contact_bodies = self.api_requester.get_contact_bodies_of_link(body_link) + for obj_body in body_contact_bodies: + obj_body_object, obj_body_link = self.get_object_with_body_name(obj_body) + if obj_body_object.name != obj.name: + continue + multiverse_contact_points = self.api_requester.get_contact_points_between_objects(obj_body_link.name, + body_name) + if len(multiverse_contact_points) == 0: + contact_points.append(ContactPoint(obj_body_link, body_link)) + else: + for point in multiverse_contact_points: + contact_points.append(ContactPoint(obj_body_link, body_link)) + contact_points[-1].normal_on_b = point.normal + contact_points[-1].position_on_b = point.position return contact_points def get_object_with_body_name(self, body_name: str) -> Tuple[Optional[Object], Optional[Link]]: diff --git a/src/pycram/worlds/multiverse_communication/clients.py b/src/pycram/worlds/multiverse_communication/clients.py index 9c68c603a..1df579fc5 100644 --- a/src/pycram/worlds/multiverse_communication/clients.py +++ b/src/pycram/worlds/multiverse_communication/clients.py @@ -766,6 +766,15 @@ def _parse_constraint_effort(contact_effort: List[str]) -> Tuple[List[float], Li forces, torques = contact_effort[:3], contact_effort[3:] return forces, torques + def get_contact_bodies_of_link(self, link: Link) -> List[str]: + """ + Get the names of bodies/objects that are in contact with an object. + + :param link: The link to get the contact bodies for. + :return: The names of the bodies/objects that are in contact with the object. + """ + return self._request_single_api_callback(API.GET_CONTACT_BODIES, link.name) + def get_contact_bodies_of_object(self, obj: Object) -> List[str]: """ Get the names of bodies/objects that are in contact with an object. From 8528d0a4114317aed23b18ba3f42b22bd0e80433 Mon Sep 17 00:00:00 2001 From: Abdelrhman Bassiouny Date: Mon, 11 Nov 2024 10:51:07 +0100 Subject: [PATCH 36/91] [Contact] corrected the setting of in_contact variable by checking that all the links are in the allowed ones not just some or one of them. --- src/pycram/pose_generator_and_validator.py | 7 ++----- 1 file changed, 2 insertions(+), 5 deletions(-) diff --git a/src/pycram/pose_generator_and_validator.py b/src/pycram/pose_generator_and_validator.py index abcbe6454..2246fa7f7 100644 --- a/src/pycram/pose_generator_and_validator.py +++ b/src/pycram/pose_generator_and_validator.py @@ -151,11 +151,8 @@ def _in_contact(robot: Object, obj: Object, allowed_collision: Dict[Object, List allowed_links = allowed_collision[obj] if obj in allowed_collision.keys() else [] if in_contact: - for link in contact_links: - if link[0].name in allowed_robot_links or link[1].name in allowed_links: - in_contact = False - # TODO: in_contact is never set to True after it was set to False is that correct? - # TODO: If it is correct, then this loop should break after the first contact is found + if all(link[0].name in allowed_robot_links or link[1].name in allowed_links for link in contact_links): + in_contact = False return in_contact From 40ec4e26a086326a288c1a44b8fc6b071ba0fa72 Mon Sep 17 00:00:00 2001 From: A_Bassiouny Date: Mon, 11 Nov 2024 13:03:26 +0100 Subject: [PATCH 37/91] [NavigateAction] Add an option that asks if the joint states should be kept while navigating. --- .../behavior_tree_config.py | 310 ---------------- .../pycram_multiverse_demo/fallschool_demo.py | 6 + demos/pycram_multiverse_demo/pr2.py | 301 ---------------- demos/pycram_multiverse_demo/world_config.py | 333 ------------------ 4 files changed, 6 insertions(+), 944 deletions(-) delete mode 100644 demos/pycram_multiverse_demo/behavior_tree_config.py delete mode 100644 demos/pycram_multiverse_demo/pr2.py delete mode 100644 demos/pycram_multiverse_demo/world_config.py diff --git a/demos/pycram_multiverse_demo/behavior_tree_config.py b/demos/pycram_multiverse_demo/behavior_tree_config.py deleted file mode 100644 index 952764df8..000000000 --- a/demos/pycram_multiverse_demo/behavior_tree_config.py +++ /dev/null @@ -1,310 +0,0 @@ -from abc import ABC, abstractmethod -from typing import Optional - -from giskardpy.exceptions import SetupException -from giskardpy.god_map import god_map -from giskardpy.model.ros_msg_visualization import VisualizationMode -from giskardpy.tree.behaviors.tf_publisher import TfPublishingModes -from giskardpy.tree.branches.giskard_bt import GiskardBT -from giskardpy.tree.control_modes import ControlModes -from giskardpy.utils.utils import is_running_in_pytest - - -class BehaviorTreeConfig(ABC): - - def __init__(self, mode: ControlModes, control_loop_max_hz: float = 50, simulation_max_hz: Optional[float] = None): - """ - - :param mode: Defines the default setup of the behavior tree. - :param control_loop_max_hz: if mode == ControlModes.standalone: limits the simulation speed - if mode == ControlModes.open_loop: limits the control loop of the base tracker - if mode == ControlModes.close_loop: limits the control loop - """ - self._control_mode = mode - self.control_loop_max_hz = control_loop_max_hz - self.simulation_max_hz = simulation_max_hz - - @abstractmethod - def setup(self): - """ - Implement this method to configure the behavior tree using it's self. methods. - """ - - @property - def tree(self) -> GiskardBT: - return god_map.tree - - def _create_behavior_tree(self): - god_map.tree = GiskardBT(control_mode=self._control_mode) - - def set_defaults(self): - pass - - def set_tree_tick_rate(self, rate: float = 0.05): - """ - How often the tree ticks per second. - :param rate: in /s - """ - self.tree_tick_rate = rate - - def add_visualization_marker_publisher(self, - mode: VisualizationMode, - add_to_sync: Optional[bool] = None, - add_to_control_loop: Optional[bool] = None): - """ - - :param add_to_sync: Markers are published while waiting for a goal. - :param add_to_control_loop: Markers are published during the closed loop control sequence, this is slow. - :param use_decomposed_meshes: True: publish decomposed meshes used for collision avoidance, these likely only - available on the machine where Giskard is running. - False: use meshes defined in urdf. - """ - if add_to_sync: - self.tree.wait_for_goal.publish_state.add_visualization_marker_behavior(mode) - if add_to_control_loop: - self.tree.control_loop_branch.publish_state.add_visualization_marker_behavior(mode) - - def add_qp_data_publisher(self, publish_lb: bool = False, publish_ub: bool = False, - publish_lbA: bool = False, publish_ubA: bool = False, - publish_bE: bool = False, publish_Ax: bool = False, - publish_Ex: bool = False, publish_xdot: bool = False, - publish_weights: bool = False, publish_g: bool = False, - publish_debug: bool = False, add_to_base: bool = False): - """ - QP data is streamed and can be visualized in e.g. plotjuggler. Useful for debugging. - """ - self.add_evaluate_debug_expressions() - if god_map.is_open_loop(): - self.tree.execute_traj.base_closed_loop.publish_state.add_qp_data_publisher( - publish_lb=publish_lb, - publish_ub=publish_ub, - publish_lbA=publish_lbA, - publish_ubA=publish_ubA, - publish_bE=publish_bE, - publish_Ax=publish_Ax, - publish_Ex=publish_Ex, - publish_xdot=publish_xdot, - publish_weights=publish_weights, - publish_g=publish_g, - publish_debug=publish_debug) - else: - self.tree.control_loop_branch.publish_state.add_qp_data_publisher( - publish_lb=publish_lb, - publish_ub=publish_ub, - publish_lbA=publish_lbA, - publish_ubA=publish_ubA, - publish_bE=publish_bE, - publish_Ax=publish_Ax, - publish_Ex=publish_Ex, - publish_xdot=publish_xdot, - publish_weights=publish_weights, - publish_g=publish_g, - publish_debug=publish_debug) - - def add_trajectory_plotter(self, normalize_position: bool = False, wait: bool = False): - """ - Plots the generated trajectories. - :param normalize_position: Positions are centered around zero. - :param wait: True: Behavior tree waits for this plotter to finish. - False: Plot is generated in a separate thread to not slow down Giskard. - """ - self.tree.cleanup_control_loop.add_plot_trajectory(normalize_position, wait) - - def add_trajectory_visualizer(self): - self.tree.cleanup_control_loop.add_visualize_trajectory() - - def add_debug_trajectory_visualizer(self): - self.tree.cleanup_control_loop.add_debug_visualize_trajectory() - - def add_debug_trajectory_plotter(self, normalize_position: bool = False, wait: bool = False): - """ - Plots debug expressions defined in goals. - """ - self.add_evaluate_debug_expressions() - self.tree.cleanup_control_loop.add_plot_debug_trajectory(normalize_position=normalize_position, - wait=wait) - - def add_gantt_chart_plotter(self): - self.add_evaluate_debug_expressions() - self.tree.cleanup_control_loop.add_plot_gantt_chart() - - def add_goal_graph_plotter(self): - self.add_evaluate_debug_expressions() - self.tree.prepare_control_loop.add_plot_goal_graph() - - def add_debug_marker_publisher(self): - """ - Publishes debug expressions defined in goals. - """ - self.add_evaluate_debug_expressions() - self.tree.control_loop_branch.publish_state.add_debug_marker_publisher() - - def add_tf_publisher(self, include_prefix: bool = True, tf_topic: str = 'tf', - mode: TfPublishingModes = TfPublishingModes.attached_and_world_objects): - """ - Publishes tf for Giskard's internal state. - """ - self.tree.wait_for_goal.publish_state.add_tf_publisher(include_prefix=include_prefix, - tf_topic=tf_topic, - mode=mode) - if god_map.is_standalone(): - self.tree.control_loop_branch.publish_state.add_tf_publisher(include_prefix=include_prefix, - tf_topic=tf_topic, - mode=mode) - - def add_evaluate_debug_expressions(self): - self.tree.prepare_control_loop.add_compile_debug_expressions() - if god_map.is_closed_loop(): - self.tree.control_loop_branch.add_evaluate_debug_expressions(log_traj=False) - else: - self.tree.control_loop_branch.add_evaluate_debug_expressions(log_traj=True) - if god_map.is_open_loop(): - god_map.tree.execute_traj.prepare_base_control.add_compile_debug_expressions() - god_map.tree.execute_traj.base_closed_loop.add_evaluate_debug_expressions(log_traj=False) - - def add_js_publisher(self, topic_name: Optional[str] = None, include_prefix: bool = False): - """ - Publishes joint states for Giskard's internal state. - """ - god_map.tree.control_loop_branch.publish_state.add_joint_state_publisher(include_prefix=include_prefix, - topic_name=topic_name, - only_prismatic_and_revolute=True) - god_map.tree.wait_for_goal.publish_state.add_joint_state_publisher(include_prefix=include_prefix, - topic_name=topic_name, - only_prismatic_and_revolute=True) - - def add_free_variable_publisher(self, topic_name: Optional[str] = None, include_prefix: bool = False): - """ - Publishes joint states for Giskard's internal state. - """ - god_map.tree.control_loop_branch.publish_state.add_joint_state_publisher(include_prefix=include_prefix, - topic_name=topic_name, - only_prismatic_and_revolute=False) - god_map.tree.wait_for_goal.publish_state.add_joint_state_publisher(include_prefix=include_prefix, - topic_name=topic_name, - only_prismatic_and_revolute=False) - - -class StandAloneBTConfig(BehaviorTreeConfig): - def __init__(self, - debug_mode: bool = False, - publish_js: bool = False, - visualization_mode: VisualizationMode = VisualizationMode.VisualsFrameLocked, - publish_free_variables: bool = False, - publish_tf: bool = True, - include_prefix: bool = False, - simulation_max_hz: Optional[float] = None): - """ - The default behavior tree for Giskard in standalone mode. Make sure to set up the robot interface accordingly. - :param debug_mode: enable various debugging tools. - :param publish_js: publish current world state. - :param publish_tf: publish all link poses in tf. - :param simulation_max_hz: if not None, will limit the frequency of the simulation. - :param include_prefix: whether to include the robot name prefix when publishing joint states or tf - """ - self.include_prefix = include_prefix - self.visualization_mode = visualization_mode - if is_running_in_pytest(): - if god_map.is_in_github_workflow(): - publish_js = False - publish_tf = False - debug_mode = False - simulation_max_hz = None - super().__init__(ControlModes.standalone, simulation_max_hz=simulation_max_hz) - self.debug_mode = debug_mode - self.publish_js = publish_js - self.publish_free_variables = publish_free_variables - self.publish_tf = publish_tf - if publish_js and publish_free_variables: - raise SetupException('publish_js and publish_free_variables cannot be True at the same time.') - - def setup(self): - self.add_visualization_marker_publisher(add_to_sync=True, add_to_control_loop=True, - mode=self.visualization_mode) - if self.publish_tf: - self.add_tf_publisher(include_prefix=self.include_prefix, mode=TfPublishingModes.all) - self.add_gantt_chart_plotter() - self.add_goal_graph_plotter() - if self.debug_mode: - self.add_trajectory_plotter(wait=True) - # self.add_trajectory_visualizer() - # self.add_debug_trajectory_visualizer() - self.add_debug_trajectory_plotter(wait=True) - self.add_debug_marker_publisher() - # self.add_debug_marker_publisher() - if self.publish_js: - self.add_js_publisher(include_prefix=self.include_prefix) - if self.publish_free_variables: - self.add_free_variable_publisher() - - -class OpenLoopBTConfig(BehaviorTreeConfig): - def __init__(self, - debug_mode: bool = False, - control_loop_max_hz: float = 50, - visualization_mode: VisualizationMode = VisualizationMode.CollisionsDecomposed, - simulation_max_hz: Optional[float] = None): - """ - The default behavior tree for Giskard in open-loop mode. It will first plan the trajectory in simulation mode - and then publish it to connected joint trajectory followers. The base trajectory is tracked with a closed-loop - controller. - :param debug_mode: enable various debugging tools. - :param control_loop_max_hz: if not None, limits the frequency of the base trajectory controller. - """ - super().__init__(ControlModes.open_loop, control_loop_max_hz=control_loop_max_hz, - simulation_max_hz=simulation_max_hz) - if god_map.is_in_github_workflow(): - debug_mode = False - self.debug_mode = debug_mode - self.visualization_mode = visualization_mode - - def setup(self): - self.add_visualization_marker_publisher(add_to_sync=True, add_to_control_loop=True, - mode=self.visualization_mode) - self.add_gantt_chart_plotter() - self.add_goal_graph_plotter() - if self.debug_mode or True: - self.add_trajectory_plotter(wait=True) - self.add_debug_trajectory_plotter(wait=True) - self.add_debug_marker_publisher() - self.add_qp_data_publisher( - publish_debug=True, - publish_xdot=True, - # publish_lbA=True, - # publish_ubA=True - ) - - -class ClosedLoopBTConfig(BehaviorTreeConfig): - def __init__(self, debug_mode: bool = False, - control_loop_max_hz: float = 50, - visualization_mode: VisualizationMode = VisualizationMode.CollisionsDecomposed, - simulation_max_hz: Optional[float] = None): - """ - The default configuration for Giskard in closed loop mode. Make use to set up the robot interface accordingly. - :param debug_mode: If True, will publish debug data on topics. This will significantly slow down the control loop. - :param control_loop_max_hz: Limits the control loop frequency. If None, it will go as fast as possible. - """ - super().__init__(ControlModes.close_loop, control_loop_max_hz=control_loop_max_hz, - simulation_max_hz=simulation_max_hz) - if god_map.is_in_github_workflow(): - debug_mode = False - self.debug_mode = debug_mode - self.visualization_mode = visualization_mode - - def setup(self): - self.add_visualization_marker_publisher(add_to_sync=True, add_to_control_loop=False, - mode=self.visualization_mode) - # self.add_qp_data_publisher(publish_xdot=True, publish_lb=True, publish_ub=True) - self.add_gantt_chart_plotter() - self.add_goal_graph_plotter() - if self.debug_mode: - self.add_trajectory_plotter(wait=True) - self.add_debug_trajectory_plotter(wait=True) - self.add_debug_marker_publisher() - # self.add_qp_data_publisher( - # publish_debug=True, - # publish_xdot=True, - # # publish_lbA=True, - # # publish_ubA=True - # ) diff --git a/demos/pycram_multiverse_demo/fallschool_demo.py b/demos/pycram_multiverse_demo/fallschool_demo.py index 85266196b..169a024b3 100644 --- a/demos/pycram_multiverse_demo/fallschool_demo.py +++ b/demos/pycram_multiverse_demo/fallschool_demo.py @@ -1,5 +1,7 @@ +import logging from datetime import timedelta +import rospy from tf.transformations import quaternion_from_euler from pycram.datastructures.dataclasses import Color @@ -16,6 +18,10 @@ from pycram.worlds.multiverse import Multiverse +rospy_logger = logging.getLogger('rosout') +rospy_logger.setLevel(logging.DEBUG) + + @with_simulated_robot def move_and_detect(obj_type: ObjectType, pick_pose: Pose): NavigateAction(target_locations=[Pose([1.7, 2, 0])]).resolve().perform() diff --git a/demos/pycram_multiverse_demo/pr2.py b/demos/pycram_multiverse_demo/pr2.py deleted file mode 100644 index ae4cc1872..000000000 --- a/demos/pycram_multiverse_demo/pr2.py +++ /dev/null @@ -1,301 +0,0 @@ -from giskardpy.configs.giskard import CollisionAvoidanceConfig, RobotInterfaceConfig -from giskardpy.configs.world_config import WorldWithOmniDriveRobot -from giskardpy.data_types import Derivatives - - -class WorldWithPR2Config(WorldWithOmniDriveRobot): - def __init__(self, map_name: str = 'map', localization_joint_name: str = 'localization', - odom_link_name: str = 'odom_combined', drive_joint_name: str = 'brumbrum'): - super().__init__(map_name, localization_joint_name, odom_link_name, drive_joint_name) - - def setup(self): - super().setup() - self.set_joint_limits(limit_map={Derivatives.velocity: 2, - Derivatives.jerk: 60}, - joint_name='head_pan_joint') - self.set_joint_limits(limit_map={Derivatives.velocity: 4, - Derivatives.jerk: 120}, - joint_name='head_tilt_joint') - - - -class PR2StandaloneInterface(RobotInterfaceConfig): - drive_joint_name: str - - def __init__(self, drive_joint_name: str): - self.drive_joint_name = drive_joint_name - - def setup(self): - self.register_controlled_joints([ - 'torso_lift_joint', - 'head_pan_joint', - 'head_tilt_joint', - 'r_shoulder_pan_joint', - 'r_shoulder_lift_joint', - 'r_upper_arm_roll_joint', - 'r_forearm_roll_joint', - 'r_elbow_flex_joint', - 'r_wrist_flex_joint', - 'r_wrist_roll_joint', - 'l_shoulder_pan_joint', - 'l_shoulder_lift_joint', - 'l_upper_arm_roll_joint', - 'l_forearm_roll_joint', - 'l_elbow_flex_joint', - 'l_wrist_flex_joint', - 'l_wrist_roll_joint', - self.drive_joint_name, - ]) - - -class PR2JointTrajServerMujocoInterface(RobotInterfaceConfig): - map_name: str - localization_joint_name: str - odom_link_name: str - drive_joint_name: str - - def __init__(self, - map_name: str = 'map', - localization_joint_name: str = 'localization', - odom_link_name: str = 'odom_combined', - drive_joint_name: str = 'brumbrum'): - self.map_name = map_name - self.localization_joint_name = localization_joint_name - self.odom_link_name = odom_link_name - self.drive_joint_name = drive_joint_name - - def setup(self): - self.sync_6dof_joint_with_tf_frame(joint_name=self.localization_joint_name, - tf_parent_frame=self.map_name, - tf_child_frame=self.odom_link_name) - self.sync_joint_state_topic('/joint_states') - self.sync_odometry_topic('/pr2/base_footprint', self.drive_joint_name) - self.add_follow_joint_trajectory_server( - namespace='/pr2/whole_body_controller') - self.add_follow_joint_trajectory_server( - namespace='/pr2/l_gripper_l_finger_controller') - self.add_follow_joint_trajectory_server( - namespace='/pr2/r_gripper_l_finger_controller') - self.add_base_cmd_velocity(cmd_vel_topic='/pr2/cmd_vel', - track_only_velocity=True, - joint_name=self.drive_joint_name) - -class PR2JointTrajServerMultiverseInterface(RobotInterfaceConfig): - map_name: str - localization_joint_name: str - odom_link_name: str - drive_joint_name: str - - def __init__(self, - map_name: str = 'map', - localization_joint_name: str = 'localization', - odom_link_name: str = 'pr2', - drive_joint_name: str = 'brumbrum'): - self.map_name = map_name - self.localization_joint_name = localization_joint_name - self.odom_link_name = odom_link_name - self.drive_joint_name = drive_joint_name - - def setup(self): - # self.sync_6dof_joint_with_tf_frame(joint_name=self.localization_joint_name, - # tf_parent_frame=self.map_name, - # tf_child_frame=self.odom_link_name) - # self. - self.sync_joint_state_topic('/real/pr2/joint_states') - self.sync_odometry_topic('/odom', self.drive_joint_name) - self.add_follow_joint_trajectory_server( - namespace='/real/pr2/head_controller') - self.add_follow_joint_trajectory_server( - namespace='/real/pr2/torso_controller') - self.add_follow_joint_trajectory_server( - namespace='/real/pr2/left_arm_controller') - self.add_follow_joint_trajectory_server( - namespace='/real/pr2/right_arm_controller') - self.add_base_cmd_velocity(cmd_vel_topic='/cmd_vel', - track_only_velocity=True, - joint_name=self.drive_joint_name) - -class PR2VelocityMujocoInterface(RobotInterfaceConfig): - map_name: str - localization_joint_name: str - odom_link_name: str - drive_joint_name: str - - def __init__(self, - map_name: str = 'map', - localization_joint_name: str = 'localization', - odom_link_name: str = 'odom_combined', - drive_joint_name: str = 'brumbrum'): - self.map_name = map_name - self.localization_joint_name = localization_joint_name - self.odom_link_name = odom_link_name - self.drive_joint_name = drive_joint_name - - def setup(self): - self.sync_6dof_joint_with_tf_frame(joint_name=self.localization_joint_name, - tf_parent_frame=self.map_name, - tf_child_frame=self.odom_link_name) - self.sync_joint_state_topic('pr2/joint_states') - self.sync_odometry_topic('/pr2/base_footprint', self.drive_joint_name) - self.add_joint_velocity_controller(namespaces=[ - 'pr2/torso_lift_velocity_controller', - 'pr2/r_upper_arm_roll_velocity_controller', - 'pr2/r_shoulder_pan_velocity_controller', - 'pr2/r_shoulder_lift_velocity_controller', - 'pr2/r_forearm_roll_velocity_controller', - 'pr2/r_elbow_flex_velocity_controller', - 'pr2/r_wrist_flex_velocity_controller', - 'pr2/r_wrist_roll_velocity_controller', - 'pr2/l_upper_arm_roll_velocity_controller', - 'pr2/l_shoulder_pan_velocity_controller', - 'pr2/l_shoulder_lift_velocity_controller', - 'pr2/l_forearm_roll_velocity_controller', - 'pr2/l_elbow_flex_velocity_controller', - 'pr2/l_wrist_flex_velocity_controller', - 'pr2/l_wrist_roll_velocity_controller', - 'pr2/head_pan_velocity_controller', - 'pr2/head_tilt_velocity_controller', - ]) - - self.add_base_cmd_velocity(cmd_vel_topic='/pr2/cmd_vel', - joint_name=self.drive_joint_name) - - -class PR2VelocityIAIInterface(RobotInterfaceConfig): - map_name: str - localization_joint_name: str - odom_link_name: str - drive_joint_name: str - - def __init__(self, - map_name: str = 'map', - localization_joint_name: str = 'localization', - odom_link_name: str = 'odom_combined', - drive_joint_name: str = 'brumbrum'): - self.map_name = map_name - self.localization_joint_name = localization_joint_name - self.odom_link_name = odom_link_name - self.drive_joint_name = drive_joint_name - - def setup(self): - self.sync_6dof_joint_with_tf_frame(joint_name=self.localization_joint_name, - tf_parent_frame=self.map_name, - tf_child_frame=self.odom_link_name) - self.sync_joint_state_topic('/joint_states') - self.sync_odometry_topic('/robot_pose_ekf/odom_combined', self.drive_joint_name) - self.add_joint_velocity_group_controller(namespace='l_arm_joint_group_velocity_controller') - self.add_joint_velocity_group_controller(namespace='r_arm_joint_group_velocity_controller') - self.add_joint_position_controller(namespaces=[ - 'head_pan_position_controller', - 'head_tilt_position_controller', - ]) - - self.add_base_cmd_velocity(cmd_vel_topic='/base_controller/command', - joint_name=self.drive_joint_name) - - -class PR2CollisionAvoidance(CollisionAvoidanceConfig): - def __init__(self, drive_joint_name: str = 'brumbrum'): - super().__init__() - self.drive_joint_name = drive_joint_name - - def setup(self): - self.load_self_collision_matrix('package://giskardpy/self_collision_matrices/iai/pr2.srdf') - self.set_default_external_collision_avoidance(soft_threshold=0.1, - hard_threshold=0.0) - for joint_name in ['r_wrist_roll_joint', 'l_wrist_roll_joint']: - self.overwrite_external_collision_avoidance(joint_name, - number_of_repeller=4, - soft_threshold=0.05, - hard_threshold=0.0, - max_velocity=0.2) - for joint_name in ['r_wrist_flex_joint', 'l_wrist_flex_joint']: - self.overwrite_external_collision_avoidance(joint_name, - number_of_repeller=2, - soft_threshold=0.05, - hard_threshold=0.0, - max_velocity=0.2) - for joint_name in ['r_elbow_flex_joint', 'l_elbow_flex_joint']: - self.overwrite_external_collision_avoidance(joint_name, - soft_threshold=0.05, - hard_threshold=0.0) - for joint_name in ['r_forearm_roll_joint', 'l_forearm_roll_joint']: - self.overwrite_external_collision_avoidance(joint_name, - soft_threshold=0.025, - hard_threshold=0.0) - self.fix_joints_for_collision_avoidance([ - 'r_gripper_l_finger_joint', - 'l_gripper_l_finger_joint' - ]) - self.overwrite_external_collision_avoidance(self.drive_joint_name, - number_of_repeller=2, - soft_threshold=0.2, - hard_threshold=0.1) - - -class PR2JointTrajServerIAIInterface(RobotInterfaceConfig): - map_name: str - localization_joint_name: str - odom_link_name: str - drive_joint_name: str - - def __init__(self, - map_name: str = 'map', - localization_joint_name: str = 'localization', - odom_link_name: str = 'odom_combined', - drive_joint_name: str = 'brumbrum'): - self.map_name = map_name - self.localization_joint_name = localization_joint_name - self.odom_link_name = odom_link_name - self.drive_joint_name = drive_joint_name - - def setup(self): - self.sync_6dof_joint_with_tf_frame(joint_name=self.localization_joint_name, - tf_parent_frame=self.map_name, - tf_child_frame=self.odom_link_name) - self.sync_joint_state_topic('/joint_states') - self.sync_odometry_topic('/robot_pose_ekf/odom_combined', self.drive_joint_name) - fill_velocity_values = False - self.add_follow_joint_trajectory_server(namespace='/l_arm_controller', - fill_velocity_values=fill_velocity_values) - self.add_follow_joint_trajectory_server(namespace='/r_arm_controller', - fill_velocity_values=fill_velocity_values) - self.add_follow_joint_trajectory_server(namespace='/torso_controller', - fill_velocity_values=fill_velocity_values) - self.add_follow_joint_trajectory_server(namespace='/head_traj_controller', - fill_velocity_values=fill_velocity_values) - self.add_base_cmd_velocity(cmd_vel_topic='/base_controller/command', - track_only_velocity=True, - joint_name=self.drive_joint_name) - - -class PR2JointTrajServerUnrealInterface(RobotInterfaceConfig): - map_name: str - localization_joint_name: str - odom_link_name: str - drive_joint_name: str - - def __init__(self, - map_name: str = 'map', - localization_joint_name: str = 'localization', - odom_link_name: str = 'odom_combined', - drive_joint_name: str = 'brumbrum'): - self.map_name = map_name - self.localization_joint_name = localization_joint_name - self.odom_link_name = odom_link_name - self.drive_joint_name = drive_joint_name - - def setup(self): - self.sync_6dof_joint_with_tf_frame(joint_name=self.localization_joint_name, - tf_parent_frame=self.map_name, - tf_child_frame=self.odom_link_name) - self.sync_joint_state_topic('/joint_states') - self.sync_odometry_topic('/base_odometry/odom', self.drive_joint_name) - fill_velocity_values = False - self.add_follow_joint_trajectory_server(namespace='/whole_body_controller', - fill_velocity_values=fill_velocity_values) - self.add_follow_joint_trajectory_server(namespace='/head_traj_controller', - fill_velocity_values=fill_velocity_values) - self.add_base_cmd_velocity(cmd_vel_topic='/base_controller/command', - track_only_velocity=True, - joint_name=self.drive_joint_name) diff --git a/demos/pycram_multiverse_demo/world_config.py b/demos/pycram_multiverse_demo/world_config.py deleted file mode 100644 index 6a177ac5a..000000000 --- a/demos/pycram_multiverse_demo/world_config.py +++ /dev/null @@ -1,333 +0,0 @@ -from __future__ import annotations - -import abc -from abc import ABC -from typing import Dict, Optional, Union - -import numpy as np -import rospy -from numpy.typing import NDArray -from std_msgs.msg import ColorRGBA - -from giskardpy.god_map import god_map -from giskardpy.model.joints import FixedJoint, OmniDrive, DiffDrive, Joint6DOF, OneDofJoint -from giskardpy.model.links import Link -from giskardpy.model.utils import robot_name_from_urdf_string -from giskardpy.model.world import WorldTree -from giskardpy.data_types import my_string, PrefixName, Derivatives, derivative_map - - -class WorldConfig(ABC): - - def __init__(self): - god_map.world = WorldTree() - self.set_default_weights() - - @property - def world(self) -> WorldTree: - return god_map.world - - def set_defaults(self): - pass - - @abc.abstractmethod - def setup(self): - """ - Implement this method to configure the initial world using it's self. methods. - """ - - @property - def robot_group_name(self) -> str: - return god_map.world.robot_name - - def set_default_weights(self, - velocity_weight: float = 0.01, - acceleration_weight: float = 0, - jerk_weight: float = 0.01): - """ - The default values are set automatically, even if this function is not called. - A typical goal has a weight of 1, so the values in here should be sufficiently below that. - """ - god_map.world.update_default_weights({Derivatives.velocity: velocity_weight, - Derivatives.acceleration: acceleration_weight, - Derivatives.jerk: jerk_weight}) - - def set_weight(self, weight_map: derivative_map, joint_name: str, group_name: Optional[str] = None): - """ - Set weights for joints that are used by the qp controller. Don't change this unless you know what you are doing. - """ - joint_name = god_map.world.search_for_joint_name(joint_name, group_name) - joint = god_map.world.joints[joint_name] - if not isinstance(joint, OneDofJoint): - raise ValueError(f'Can\'t change weight because {joint_name} is not of type {str(OneDofJoint)}.') - free_variable = god_map.world.free_variables[joint.free_variable.name] - for derivative, weight in weight_map.items(): - free_variable.quadratic_weights[derivative] = weight - - def get_root_link_of_group(self, group_name: str) -> PrefixName: - return god_map.world.groups[group_name].root_link_name - - def set_joint_limits(self, limit_map: derivative_map, joint_name: my_string, group_name: Optional[str] = None): - """ - Set the joint limits for individual joints - :param limit_map: maps Derivatives to values, e.g. {Derivatives.velocity: 1, - Derivatives.acceleration: np.inf, - Derivatives.jerk: 30} - """ - joint_name = god_map.world.search_for_joint_name(joint_name, group_name) - joint = god_map.world.joints[joint_name] - if not isinstance(joint, OneDofJoint): - raise ValueError(f'Can\'t change limits because {joint_name} is not of type {str(OneDofJoint)}.') - free_variable = god_map.world.free_variables[joint.free_variable.name] - for derivative, limit in limit_map.items(): - free_variable.set_lower_limit(derivative, -limit) - free_variable.set_upper_limit(derivative, limit) - - def set_default_color(self, r: float, g: float, b: float, a: float): - """ - :param r: 0-1 - :param g: 0-1 - :param b: 0-1 - :param a: 0-1 - """ - god_map.world.default_link_color = ColorRGBA(r, g, b, a) - - def set_default_limits(self, new_limits: derivative_map): - """ - The default values will be set automatically, even if this function is not called. - :param new_limits: e.g. {Derivatives.velocity: 1, - Derivatives.acceleration: np.inf, - Derivatives.jerk: 30} - """ - god_map.world.update_default_limits(new_limits) - - def add_robot_urdf(self, - urdf: str, - group_name: str) -> str: - """ - Add a robot urdf to the world. - :param urdf: robot urdf as string, not the path - :param group_name: - """ - if group_name is None: - group_name = robot_name_from_urdf_string(urdf) - god_map.world.add_urdf(urdf=urdf, group_name=group_name, actuated=True) - return group_name - - def add_robot_from_parameter_server(self, - parameter_name: str = 'robot_description', - group_name: Optional[str] = None) -> str: - """ - Add a robot urdf from parameter server to Giskard. - :param parameter_name: - :param group_name: How to call the robot. If nothing is specified it will get the name it has in the urdf - """ - urdf = rospy.get_param(parameter_name) - return self.add_robot_urdf(urdf=urdf, group_name=group_name) - - def add_fixed_joint(self, parent_link: my_string, child_link: my_string, - homogenous_transform: Optional[NDArray] = None): - """ - Add a fixed joint to Giskard's world. Can be used to e.g. connect a non-mobile robot to the world frame. - :param parent_link: - :param child_link: - :param homogenous_transform: a 4x4 transformation matrix. - """ - if homogenous_transform is None: - homogenous_transform = np.eye(4) - parent_link = god_map.world.search_for_link_name(parent_link) - - child_link = PrefixName.from_string(child_link, set_none_if_no_slash=True) - joint_name = PrefixName(f'{parent_link}_{child_link}_fixed_joint', None) - joint = FixedJoint(name=joint_name, parent_link_name=parent_link, child_link_name=child_link, - parent_T_child=homogenous_transform) - god_map.world._add_joint(joint) - - def add_diff_drive_joint(self, - name: str, - parent_link_name: my_string, - child_link_name: my_string, - robot_group_name: Optional[str] = None, - translation_limits: Optional[derivative_map] = None, - rotation_limits: Optional[derivative_map] = None): - """ - Same as add_omni_drive_joint, but for a differential drive. - """ - joint_name = PrefixName(name, robot_group_name) - parent_link_name = PrefixName.from_string(parent_link_name, set_none_if_no_slash=True) - child_link_name = PrefixName.from_string(child_link_name, set_none_if_no_slash=True) - brumbrum_joint = DiffDrive(parent_link_name=parent_link_name, - child_link_name=child_link_name, - name=joint_name, - translation_limits=translation_limits, - rotation_limits=rotation_limits) - god_map.world._add_joint(brumbrum_joint) - god_map.world.deregister_group(robot_group_name) - god_map.world.register_group(robot_group_name, root_link_name=parent_link_name, actuated=True) - - def add_6dof_joint(self, parent_link: my_string, child_link: my_string, joint_name: my_string): - """ - Add a 6dof joint to Giskard's world. Generally used if you want Giskard to keep track of a tf transform, - e.g. for localization. - :param parent_link: - :param child_link: - """ - parent_link = god_map.world.search_for_link_name(parent_link) - child_link = PrefixName.from_string(child_link, set_none_if_no_slash=True) - joint_name = PrefixName.from_string(joint_name, set_none_if_no_slash=True) - joint = Joint6DOF(name=joint_name, parent_link_name=parent_link, child_link_name=child_link) - god_map.world._add_joint(joint) - - def add_empty_link(self, link_name: my_string): - """ - If you need a virtual link during your world building. - """ - link = Link(link_name) - god_map.world._add_link(link) - - def add_omni_drive_joint(self, - name: str, - parent_link_name: Union[str, PrefixName], - child_link_name: Union[str, PrefixName], - robot_group_name: Optional[str] = None, - translation_limits: Optional[derivative_map] = None, - rotation_limits: Optional[derivative_map] = None, - x_name: Optional[PrefixName] = None, - y_name: Optional[PrefixName] = None, - yaw_vel_name: Optional[PrefixName] = None): - """ - Use this to connect a robot urdf of a mobile robot to the world if it has an omni-directional drive. - :param parent_link_name: - :param child_link_name: - :param robot_group_name: set if there are multiple robots - :param name: Name of the new link. Has to be unique and may be required in other functions. - :param translation_limits: in m/s**3 - :param rotation_limits: in rad/s**3 - """ - joint_name = PrefixName(name, robot_group_name) - parent_link_name = PrefixName.from_string(parent_link_name, set_none_if_no_slash=True) - child_link_name = PrefixName.from_string(child_link_name, set_none_if_no_slash=True) - brumbrum_joint = OmniDrive(parent_link_name=parent_link_name, - child_link_name=child_link_name, - name=joint_name, - translation_limits=translation_limits, - rotation_limits=rotation_limits, - x_name=x_name, - y_name=y_name, - yaw_name=yaw_vel_name) - god_map.world._add_joint(brumbrum_joint) - god_map.world.deregister_group(robot_group_name) - god_map.world.register_group(robot_group_name, root_link_name=parent_link_name, actuated=True) - - -class EmptyWorld(WorldConfig): - def __init__(self): - super().__init__() - - def setup(self): - self._default_limits = { - Derivatives.velocity: 1, - } - self.set_default_weights(velocity_weight=1, acceleration_weight=1, jerk_weight=1) - self.set_default_limits(self._default_limits) - self.add_empty_link('map') - - -class WorldWithFixedRobot(WorldConfig): - def __init__(self, joint_limits: Dict[Derivatives, float] = None): - super().__init__() - self._joint_limits = joint_limits - - def setup(self): - self.set_default_limits(self._joint_limits) - self.add_robot_from_parameter_server() - - -class WorldWithOmniDriveRobot(WorldConfig): - map_name: str - localization_joint_name: str - odom_link_name: str - drive_joint_name: str - - def __init__(self, - map_name: str = 'map', - localization_joint_name: str = 'localization', - odom_link_name: str = 'odom', - drive_joint_name: str = 'brumbrum'): - super().__init__() - self.map_name = map_name - self.localization_joint_name = localization_joint_name - self.odom_link_name = odom_link_name - self.drive_joint_name = drive_joint_name - - def setup(self): - self.set_default_limits({Derivatives.velocity: 1, - Derivatives.acceleration: np.inf, - Derivatives.jerk: 30}) - self.add_empty_link(self.map_name) - self.add_empty_link(self.odom_link_name) - self.add_fixed_joint(parent_link=self.map_name, child_link=self.odom_link_name) - # self.add_6dof_joint(parent_link=self.map_name, child_link=self.odom_link_name, - # joint_name=self.localization_joint_name) - self.add_robot_from_parameter_server() - root_link_name = self.get_root_link_of_group(self.robot_group_name) - self.add_omni_drive_joint(name=self.drive_joint_name, - parent_link_name=self.odom_link_name, - child_link_name=root_link_name, - translation_limits={ - Derivatives.velocity: 0.2, - Derivatives.acceleration: 1, - Derivatives.jerk: 5, - }, - rotation_limits={ - Derivatives.velocity: 0.2, - Derivatives.acceleration: 1, - Derivatives.jerk: 5 - }, - robot_group_name=self.robot_group_name) - self.set_joint_limits(limit_map={Derivatives.velocity: 3, - Derivatives.jerk: 60}, - joint_name='head_pan_joint') - - -class WorldWithDiffDriveRobot(WorldConfig): - map_name: str - localization_joint_name: str - odom_link_name: str - drive_joint_name: str - - def __init__(self, - map_name: str = 'map', - localization_joint_name: str = 'localization', - odom_link_name: str = 'odom', - drive_joint_name: str = 'brumbrum'): - super().__init__() - self.map_name = map_name - self.localization_joint_name = localization_joint_name - self.odom_link_name = odom_link_name - self.drive_joint_name = drive_joint_name - - def setup(self): - self.set_default_limits({Derivatives.velocity: 1, - Derivatives.acceleration: np.inf, - Derivatives.jerk: 30}) - self.add_empty_link(self.map_name) - self.add_empty_link(self.odom_link_name) - self.add_6dof_joint(parent_link=self.map_name, child_link=self.odom_link_name, - joint_name=self.localization_joint_name) - self.add_robot_from_parameter_server() - root_link_name = self.get_root_link_of_group(self.robot_group_name) - self.add_diff_drive_joint(name=self.drive_joint_name, - parent_link_name=self.odom_link_name, - child_link_name=root_link_name, - translation_limits={ - Derivatives.velocity: 0.2, - Derivatives.acceleration: 1, - Derivatives.jerk: 5, - }, - rotation_limits={ - Derivatives.velocity: 0.2, - Derivatives.acceleration: 1, - Derivatives.jerk: 5 - }, - robot_group_name=self.robot_group_name) From e24a07145e62f9dee13f4998e43e864fb16d3bc4 Mon Sep 17 00:00:00 2001 From: Abdelrhman Bassiouny Date: Mon, 11 Nov 2024 17:59:09 +0100 Subject: [PATCH 38/91] [Multiverse] new version of multiverse contact points. --- src/pycram/datastructures/dataclasses.py | 13 +-- src/pycram/datastructures/enums.py | 1 + src/pycram/worlds/multiverse.py | 35 +++----- .../multiverse_communication/clients.py | 87 ++++++------------- test/test_multiverse.py | 3 - 5 files changed, 40 insertions(+), 99 deletions(-) diff --git a/src/pycram/datastructures/dataclasses.py b/src/pycram/datastructures/dataclasses.py index 5d3ba6bf7..f31dbcbb6 100644 --- a/src/pycram/datastructures/dataclasses.py +++ b/src/pycram/datastructures/dataclasses.py @@ -879,20 +879,13 @@ def intersected(self) -> bool: return self.distance >= 0 and self.body_name != "" -@dataclass -class MultiverseObjectContactData: - """ - A dataclass to store all the contact data returned from Multiverse for a single object. - """ - body_names: List[str] - data: List[MultiverseContactPoint] - - @dataclass class MultiverseContactPoint: """ - A dataclass to store the contact point returned from Multiverse. + A dataclass to store all the contact data returned from Multiverse for a single object. """ + body_1: str + body_2: str position: List[float] normal: List[float] diff --git a/src/pycram/datastructures/enums.py b/src/pycram/datastructures/enums.py index 0249d7f5e..6eb516405 100644 --- a/src/pycram/datastructures/enums.py +++ b/src/pycram/datastructures/enums.py @@ -246,6 +246,7 @@ class MultiverseAPIName(Enum): """ GET_CONTACT_POINTS = "get_contact_points" GET_CONTACT_BODIES = "get_contact_bodies" + GET_CONTACT_BODIES_AND_POINTS = "get_contact_bodies_and_points" GET_CONSTRAINT_EFFORT = "get_constraint_effort" ATTACH = "attach" DETACH = "detach" diff --git a/src/pycram/worlds/multiverse.py b/src/pycram/worlds/multiverse.py index f454a7e5b..1037101c9 100644 --- a/src/pycram/worlds/multiverse.py +++ b/src/pycram/worlds/multiverse.py @@ -18,11 +18,10 @@ from ..datastructures.pose import Pose from ..datastructures.world import World from ..description import Link, Joint -from ..failures import FailedAPIResponse -from ..object_descriptors.mjcf import ObjectDescription as MJCF, ObjectFactory, PrimitiveObjectFactory from ..object_descriptors.generic import ObjectDescription as GenericObjectDescription +from ..object_descriptors.mjcf import ObjectDescription as MJCF, PrimitiveObjectFactory from ..robot_description import RobotDescription -from ..ros.logging import logwarn, logerr +from ..ros.logging import logwarn from ..utils import RayTestUtils, wxyz_to_xyzw, xyzw_to_wxyz from ..validation.goal_validator import validate_object_pose, validate_multiple_joint_positions, \ validate_joint_position, validate_multiple_object_poses @@ -546,29 +545,15 @@ def get_object_contact_points(self, obj: Object) -> ContactPointsList: """ Note: Currently Multiverse only gets one contact point per contact objects. """ - contact_bodies = self.api_requester.get_contact_bodies_of_object(obj) + multiverse_contact_points = self.api_requester.get_contact_points(obj.name) contact_points = ContactPointsList([]) - for body_name in contact_bodies: - body_object, body_link = self.get_object_with_body_name(body_name) - if body_object is None: - logerr(f"Body Object not found: {body_name}") - raise ValueError(f"Body Object not found: {body_name}") - # To get the links of obj that are in contact with body_link, we need to get the contact bodies of the - # body_link and check if they are in obj. - body_contact_bodies = self.api_requester.get_contact_bodies_of_link(body_link) - for obj_body in body_contact_bodies: - obj_body_object, obj_body_link = self.get_object_with_body_name(obj_body) - if obj_body_object.name != obj.name: - continue - multiverse_contact_points = self.api_requester.get_contact_points_between_objects(obj_body_link.name, - body_name) - if len(multiverse_contact_points) == 0: - contact_points.append(ContactPoint(obj_body_link, body_link)) - else: - for point in multiverse_contact_points: - contact_points.append(ContactPoint(obj_body_link, body_link)) - contact_points[-1].normal_on_b = point.normal - contact_points[-1].position_on_b = point.position + for mcp in multiverse_contact_points: + body_object, body_link = self.get_object_with_body_name(mcp.body_2) + obj_link = obj.links[mcp.body_1] if mcp.body_1 in obj.links.keys() else obj.root_link + for point in multiverse_contact_points: + contact_points.append(ContactPoint(obj_link, body_link)) + contact_points[-1].normal_on_b = point.normal + contact_points[-1].position_on_b = point.position return contact_points def get_object_with_body_name(self, body_name: str) -> Tuple[Optional[Object], Optional[Link]]: diff --git a/src/pycram/worlds/multiverse_communication/clients.py b/src/pycram/worlds/multiverse_communication/clients.py index 1df579fc5..0e7a6120e 100644 --- a/src/pycram/worlds/multiverse_communication/clients.py +++ b/src/pycram/worlds/multiverse_communication/clients.py @@ -8,12 +8,11 @@ from .socket import MultiverseSocket, MultiverseMetaData from ...config.multiverse_conf import MultiverseConfig as Conf -from ...datastructures.dataclasses import RayResult, MultiverseObjectContactData, MultiverseContactPoint +from ...datastructures.dataclasses import RayResult, MultiverseContactPoint from ...datastructures.enums import (MultiverseAPIName as API, MultiverseBodyProperty as BodyProperty, MultiverseProperty as Property) from ...datastructures.pose import Pose from ...failures import FailedAPIResponse -from ...ros.logging import logwarn, logerr from ...utils import wxyz_to_xyzw from ...world_concepts.constraints import Constraint from ...world_concepts.world_object import Object, Link @@ -674,28 +673,16 @@ def get_resultant_force_and_torque_on_object(self, obj: Object) -> Tuple[List[fl effort = self._request_single_api_callback(API.GET_CONSTRAINT_EFFORT, obj.name) return self._parse_constraint_effort(effort) - def get_contact_data_of_object(self, obj: Object) -> MultiverseObjectContactData: + def get_contact_points_between_bodies(self, body_1_name: str, body_2_name: str) -> List[MultiverseContactPoint]: """ - Request the contact data of an object, this includes the object names, and the contact points. + Request the contact points between two bodies. - :param obj: The object. - :return: The contact data of the object as a MultiverseObjectContactData. - """ - api_response_data = self._get_contact_data_of_object(obj.name) - body_names = api_response_data[API.GET_CONTACT_BODIES] - points = self._parse_contact_points(api_response_data[API.GET_CONTACT_POINTS]) - return MultiverseObjectContactData(body_names, points) - - def get_contact_points_between_objects(self, obj1_name: str, obj2_name: str) -> List[MultiverseContactPoint]: - """ - Request the contact points between two objects. - - :param obj1_name: The name of the first object. - :param obj2_name: The name of the second object. - :return: The contact points between the objects as a list of MultiverseContactPoint. + :param body_1_name: The name of the first body. + :param body_2_name: The name of the second body. + :return: The contact points between the bodies as a list of MultiverseContactPoint. """ - points = self._request_single_api_callback(API.GET_CONTACT_POINTS, obj1_name, obj2_name) - return self._parse_contact_points(points) + points = self._request_single_api_callback(API.GET_CONTACT_POINTS, body_1_name, body_2_name) + return self._parse_contact_points(body_1_name, body_2_name, points) def get_objects_intersected_with_rays(self, from_positions: List[List[float]], to_positions: List[List[float]]) -> List[RayResult]: @@ -759,62 +746,40 @@ def _parse_constraint_effort(contact_effort: List[str]) -> Tuple[List[float], Li :return: The contact effort of the object as a list of floats. """ contact_effort = contact_effort[0].split() - if 'failed' in contact_effort: - logwarn("Failed to get contact effort") - return [0.0] * 3, [0.0]*3 contact_effort = list(map(float, contact_effort)) forces, torques = contact_effort[:3], contact_effort[3:] return forces, torques - def get_contact_bodies_of_link(self, link: Link) -> List[str]: - """ - Get the names of bodies/objects that are in contact with an object. - - :param link: The link to get the contact bodies for. - :return: The names of the bodies/objects that are in contact with the object. - """ - return self._request_single_api_callback(API.GET_CONTACT_BODIES, link.name) - - def get_contact_bodies_of_object(self, obj: Object) -> List[str]: - """ - Get the names of bodies/objects that are in contact with an object. - - :param obj: The object. - :return: The names of the bodies/objects that are in contact with the object. - """ - return self._request_single_api_callback(API.GET_CONTACT_BODIES, obj.name, "with_children") - - def get_contact_points_of_object(self, obj: Object) -> List[MultiverseContactPoint]: - """ - Get the 3d positions of the contacts of an object and the normal vectors at the contact points. - - :param obj: The object. - :return: The contact points of the object as a list of MultiverseContactPoint. - """ - api_response = self._request_single_api_callback(API.GET_CONTACT_POINTS, obj.name) - return self._parse_contact_points(api_response) - @staticmethod - def _parse_contact_points(contact_points: List[str]) -> List[MultiverseContactPoint]: + def _parse_contact_points(body_1, body_2, contact_points: List[str]) -> List[MultiverseContactPoint]: """ Parse the contact points of an object. + :param body_1: The name of the first body. + :param body_2: The name of the second body. :param contact_points: The contact points of the object as a list of strings. :return: The contact positions, and normal vectors as a list of MultiverseContactPoint. """ contact_point_data = [list(map(float, contact_point.split())) for contact_point in contact_points] - return [MultiverseContactPoint(point[:3], point[3:]) for point in contact_point_data] + return [MultiverseContactPoint(body_1, body_2, point[:3], point[3:]) for point in contact_point_data] - def _get_contact_data_of_object(self, object_name) -> Dict[API, List]: + def get_contact_points(self, body_name: str) -> List[MultiverseContactPoint]: """ - Request the names of bodies/objects that are in contact of an object, and the contact points. + Get the contact points of a body from the multiverse server. - :param object_name: The name of the object. - :return: The contact bodies, and contact points as a dictionary. + :param body_name: The name of the body. """ - return self._request_apis_callbacks({API.GET_CONTACT_BODIES: [object_name], - API.GET_CONTACT_POINTS: [object_name] - }) + contact_data = self._request_single_api_callback(API.GET_CONTACT_BODIES_AND_POINTS, + body_name, "with_children") + contact_points = [] + for contact_point in contact_data: + contact_point_data = list(contact_point.split()) + position_and_normal = list(map(float, contact_point_data[2].split())) + contact_points.append(MultiverseContactPoint(contact_point_data[0], + contact_point_data[1], + position_and_normal[:3], + position_and_normal[3:])) + return contact_points def pause_simulation(self) -> None: """ diff --git a/test/test_multiverse.py b/test/test_multiverse.py index 0a5bffa1c..399de5906 100644 --- a/test/test_multiverse.py +++ b/test/test_multiverse.py @@ -67,17 +67,14 @@ def test_load_generic_object(self): def test_save_and_restore_state(self): milk = self.spawn_milk([1, 1, 0.1]) robot = self.spawn_robot() - cup = self.spawn_cup([1, 2, 0.1]) if "apartment" not in self.multiverse.get_object_names(): apartment = Object("apartment", ObjectType.ENVIRONMENT, f"apartment.urdf") else: apartment = self.multiverse.get_object_by_name("apartment") apartment.set_joint_position("cabinet10_drawer1_joint", 0.1) robot.attach(milk) - milk.attach(cup) all_object_attachments = {obj: obj.attachments.copy() for obj in self.multiverse.objects} state_id = self.multiverse.save_state() - milk.detach(cup) robot_link = robot.root_link milk_link = milk.root_link cid = robot_link.constraint_ids[milk_link] From e8565cf42643e6b187c460c991ebac4c0f24fe21 Mon Sep 17 00:00:00 2001 From: Abdelrhman Bassiouny Date: Mon, 11 Nov 2024 19:30:49 +0100 Subject: [PATCH 39/91] [MultiverseFallschoolDemo] (WIP) picking up prepose and pose of arm need adjustment --- src/pycram/pose_generator_and_validator.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/pycram/pose_generator_and_validator.py b/src/pycram/pose_generator_and_validator.py index 2246fa7f7..480cdbe3e 100644 --- a/src/pycram/pose_generator_and_validator.py +++ b/src/pycram/pose_generator_and_validator.py @@ -191,7 +191,7 @@ def reachability_validator(pose: Pose, for description in manipulator_descs: retract_target_pose = LocalTransformer().transform_pose(target, robot.get_link_tf_frame( description.end_effector.tool_frame)) - retract_target_pose.position.x -= 0.07 # Care hard coded value copied from PlaceAction class + retract_target_pose.position.x -= 0.15 # Care hard coded value copied from PlaceAction class # retract_pose needs to be in world frame? retract_target_pose = LocalTransformer().transform_pose(retract_target_pose, "map") From 61549449f7587c41d5ea597b5581cb796a584379 Mon Sep 17 00:00:00 2001 From: Abdelrhman Bassiouny Date: Mon, 11 Nov 2024 19:31:06 +0100 Subject: [PATCH 40/91] [Multiverse] corrected multiverse contact points. --- src/pycram/worlds/multiverse.py | 7 +++---- src/pycram/worlds/multiverse_communication/clients.py | 2 +- 2 files changed, 4 insertions(+), 5 deletions(-) diff --git a/src/pycram/worlds/multiverse.py b/src/pycram/worlds/multiverse.py index 1037101c9..84f2e139a 100644 --- a/src/pycram/worlds/multiverse.py +++ b/src/pycram/worlds/multiverse.py @@ -550,10 +550,9 @@ def get_object_contact_points(self, obj: Object) -> ContactPointsList: for mcp in multiverse_contact_points: body_object, body_link = self.get_object_with_body_name(mcp.body_2) obj_link = obj.links[mcp.body_1] if mcp.body_1 in obj.links.keys() else obj.root_link - for point in multiverse_contact_points: - contact_points.append(ContactPoint(obj_link, body_link)) - contact_points[-1].normal_on_b = point.normal - contact_points[-1].position_on_b = point.position + contact_points.append(ContactPoint(obj_link, body_link)) + contact_points[-1].normal_on_b = mcp.normal + contact_points[-1].position_on_b = mcp.position return contact_points def get_object_with_body_name(self, body_name: str) -> Tuple[Optional[Object], Optional[Link]]: diff --git a/src/pycram/worlds/multiverse_communication/clients.py b/src/pycram/worlds/multiverse_communication/clients.py index 0e7a6120e..3dbccb2d9 100644 --- a/src/pycram/worlds/multiverse_communication/clients.py +++ b/src/pycram/worlds/multiverse_communication/clients.py @@ -774,7 +774,7 @@ def get_contact_points(self, body_name: str) -> List[MultiverseContactPoint]: contact_points = [] for contact_point in contact_data: contact_point_data = list(contact_point.split()) - position_and_normal = list(map(float, contact_point_data[2].split())) + position_and_normal = list(map(float, contact_point_data[2:])) contact_points.append(MultiverseContactPoint(contact_point_data[0], contact_point_data[1], position_and_normal[:3], From 31cd583c6270eea363dae0c5023ddbf1a569ec07 Mon Sep 17 00:00:00 2001 From: Abdelrhman Bassiouny Date: Tue, 12 Nov 2024 09:55:34 +0100 Subject: [PATCH 41/91] [ActionMotionDesignation] An option to specify movement types with MoveTCP. Navigate rotation then translation when transporting to avoid wierd arm pose. --- .../process_modules/pr2_process_modules.py | 16 ++++++++++++---- 1 file changed, 12 insertions(+), 4 deletions(-) diff --git a/src/pycram/process_modules/pr2_process_modules.py b/src/pycram/process_modules/pr2_process_modules.py index b6aa6791c..f5571f62e 100644 --- a/src/pycram/process_modules/pr2_process_modules.py +++ b/src/pycram/process_modules/pr2_process_modules.py @@ -238,13 +238,21 @@ class Pr2MoveTCPReal(ProcessModule): def _execute(self, designator: MoveTCPMotion) -> Any: lt = LocalTransformer() pose_in_map = lt.transform_pose(designator.target, "map") + tip_link = RobotDescription.current_robot_description.get_arm_chain(designator.arm).get_tool_frame() + root_link = "torso_lift_link" if designator.allow_gripper_collision: giskard.allow_gripper_collision(designator.arm.name.lower()) - giskard.achieve_cartesian_goal(pose_in_map, RobotDescription.current_robot_description.get_arm_chain( - designator.arm).get_tool_frame(), "torso_lift_link", - use_monitor=World.current_world.conf.use_giskard_monitor) - # robot_description.base_link) + + if designator.movement_type == MovementType.STRAIGHT_TRANSLATION: + giskard.achieve_straight_translation_goal(pose_in_map.position_as_list(), tip_link, root_link) + elif designator.movement_type == MovementType.STRAIGHT_CARTESIAN: + giskard.achieve_straight_cartesian_goal(pose_in_map, tip_link, root_link) + elif designator.movement_type == MovementType.TRANSLATION: + 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, + use_monitor=World.current_world.conf.use_giskard_monitor) class Pr2MoveArmJointsReal(ProcessModule): From 500610081b2f3ad0a02593fe767fb373ce2e99b1 Mon Sep 17 00:00:00 2001 From: A_Bassiouny Date: Tue, 12 Nov 2024 11:08:17 +0100 Subject: [PATCH 42/91] [ActionLocationDesignators] can ignore collisions with certain specified objects. add prepose distance variable to transportation and picking up. --- src/pycram/designators/action_designator.py | 2 +- src/pycram/designators/location_designator.py | 16 +++++++++++----- src/pycram/pose_generator_and_validator.py | 4 +++- 3 files changed, 15 insertions(+), 7 deletions(-) diff --git a/src/pycram/designators/action_designator.py b/src/pycram/designators/action_designator.py index 6ab346dc5..b3d6db6ec 100644 --- a/src/pycram/designators/action_designator.py +++ b/src/pycram/designators/action_designator.py @@ -423,7 +423,7 @@ def plan(self) -> None: robot_desig_resolved = BelieveObject(names=[RobotDescription.current_robot_description.name]).resolve() ParkArmsActionPerformable(Arms.BOTH).perform() pickup_loc = CostmapLocation(target=self.object_designator, reachable_for=robot_desig_resolved, - reachable_arm=self.arm) + reachable_arm=self.arm, prepose_distance=self.pickup_prepose_distance).resolve() # Tries to find a pick-up position for the robot that uses the given arm pickup_pose = None for pose in pickup_loc: diff --git a/src/pycram/designators/location_designator.py b/src/pycram/designators/location_designator.py index 82c0e7ac0..168d5dc62 100644 --- a/src/pycram/designators/location_designator.py +++ b/src/pycram/designators/location_designator.py @@ -114,7 +114,9 @@ def __init__(self, target: Union[Pose, ObjectDesignatorDescription.Object], reachable_for: Optional[ObjectDesignatorDescription.Object] = None, visible_for: Optional[ObjectDesignatorDescription.Object] = None, reachable_arm: Optional[Arms] = None, - check_collision_at_start: bool = True): + prepose_distance: float = 0.03, + check_collision_at_start: bool = True, + ignore_collision_with: Optional[List[Object]] = None): """ Location designator that uses costmaps as base to calculate locations for complex constrains like reachable or visible. In case of reachable the resolved location contains a list of arms with which the location is reachable. @@ -123,14 +125,18 @@ def __init__(self, target: Union[Pose, ObjectDesignatorDescription.Object], :param reachable_for: Object for which the reachability should be calculated, usually a robot :param visible_for: Object for which the visibility should be calculated, usually a robot :param reachable_arm: An optional arm with which the target should be reached + :param prepose_distance: Distance to the target pose where the robot should be checked for reachability. :param check_collision_at_start: If True, the designator will check for collisions at the start pose. + :param ignore_collision_with: List of objects that should be ignored for collision checking. """ super().__init__() self.target: Union[Pose, ObjectDesignatorDescription.Object] = target self.reachable_for: ObjectDesignatorDescription.Object = reachable_for self.visible_for: ObjectDesignatorDescription.Object = visible_for self.reachable_arm: Optional[Arms] = reachable_arm + self.prepose_distance = prepose_distance self.check_collision_at_start = check_collision_at_start + self.ignore_collision_with = ignore_collision_with if ignore_collision_with is not None else [] def ground(self) -> Location: """ @@ -140,8 +146,7 @@ def ground(self) -> Location: """ return next(iter(self)) - @staticmethod - def check_for_collision(robot: Object, pose: Pose) -> bool: + def check_for_collision(self, robot: Object, pose: Pose) -> bool: """ Check if the robot collides with any object in the world at the given pose. @@ -152,7 +157,7 @@ def check_for_collision(robot: Object, pose: Pose) -> bool: robot.set_pose(pose) floor = robot.world.get_object_by_name("floor") for obj in robot.world.objects: - if obj in [robot, floor]: + if obj in [robot, floor] + self.ignore_collision_with: continue if contact(robot, obj): logdebug(f"Robot is in contact with {obj.name} in prospection: {obj.world.is_prospection_world}" @@ -223,7 +228,8 @@ def __iter__(self): hand_links += description.end_effector.links valid, arms = reachability_validator(maybe_pose, test_robot, target_pose, allowed_collision={test_robot: hand_links}, - arm=self.reachable_arm) + arm=self.reachable_arm, + prepose_distance=self.prepose_distance) if self.reachable_arm: res = res and valid and self.reachable_arm in arms else: diff --git a/src/pycram/pose_generator_and_validator.py b/src/pycram/pose_generator_and_validator.py index 480cdbe3e..e87d87b47 100644 --- a/src/pycram/pose_generator_and_validator.py +++ b/src/pycram/pose_generator_and_validator.py @@ -159,6 +159,7 @@ def _in_contact(robot: Object, obj: Object, allowed_collision: Dict[Object, List def reachability_validator(pose: Pose, robot: Object, target: Union[Object, Pose], + prepose_distance: float = 0.07, allowed_collision: Dict[Object, List] = None, arm: Optional[Arms] = None) -> Tuple[bool, List]: """ @@ -170,6 +171,7 @@ def reachability_validator(pose: Pose, :param pose: The pose candidate for which the reachability should be validated :param robot: The robot object in the World for which the reachability should be validated. :param target: The target position or object instance which should be the target for reachability. + :param prepose_distance: The distance the robot should retract from the target position after/before reaching it. :param allowed_collision: dict of objects with which the robot is allowed to collide each object correlates to a list of links of which this object consists :param arm: The arm that should be used for the reachability check. If None all arms are checked. :return: True if the target is reachable for the robot and False in any other case. @@ -191,7 +193,7 @@ def reachability_validator(pose: Pose, for description in manipulator_descs: retract_target_pose = LocalTransformer().transform_pose(target, robot.get_link_tf_frame( description.end_effector.tool_frame)) - retract_target_pose.position.x -= 0.15 # Care hard coded value copied from PlaceAction class + retract_target_pose.position.x -= prepose_distance # Care hard coded value copied from PlaceAction class # retract_pose needs to be in world frame? retract_target_pose = LocalTransformer().transform_pose(retract_target_pose, "map") From 5f7fa4b3cfb2be295c436ab92f2c015f89917d89 Mon Sep 17 00:00:00 2001 From: A_Bassiouny Date: Tue, 12 Nov 2024 11:18:35 +0100 Subject: [PATCH 43/91] [ActionLocationDesignators] can ignore collisions with certain specified objects. add prepose distance variable to transportation and picking up. --- src/pycram/designators/location_designator.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/pycram/designators/location_designator.py b/src/pycram/designators/location_designator.py index 168d5dc62..ca2279650 100644 --- a/src/pycram/designators/location_designator.py +++ b/src/pycram/designators/location_designator.py @@ -157,7 +157,7 @@ def check_for_collision(self, robot: Object, pose: Pose) -> bool: robot.set_pose(pose) floor = robot.world.get_object_by_name("floor") for obj in robot.world.objects: - if obj in [robot, floor] + self.ignore_collision_with: + if obj in ([robot, floor] + self.ignore_collision_with): continue if contact(robot, obj): logdebug(f"Robot is in contact with {obj.name} in prospection: {obj.world.is_prospection_world}" From 2ff6352ff7fbe3dfc86cef65a687b2d12076d677 Mon Sep 17 00:00:00 2001 From: A_Bassiouny Date: Tue, 12 Nov 2024 11:27:18 +0100 Subject: [PATCH 44/91] [ActionLocationDesignators] can ignore collisions with certain specified objects. add prepose distance variable to transportation and picking up. --- src/pycram/designators/location_designator.py | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/src/pycram/designators/location_designator.py b/src/pycram/designators/location_designator.py index ca2279650..3e324f7ad 100644 --- a/src/pycram/designators/location_designator.py +++ b/src/pycram/designators/location_designator.py @@ -146,16 +146,20 @@ def ground(self) -> Location: """ return next(iter(self)) - def check_for_collision(self, robot: Object, pose: Pose) -> bool: + @staticmethod + def check_for_collision(robot: Object, pose: Pose, + ignore_collision_with: Optional[List[Object]] = None) -> bool: """ Check if the robot collides with any object in the world at the given pose. :param robot: The robot object :param pose: The pose to check for collision + :param ignore_collision_with: List of objects that should be ignored for collision checking. :return: True if the robot collides with any object, False otherwise """ robot.set_pose(pose) floor = robot.world.get_object_by_name("floor") + ignore_collision_with = ignore_collision_with if ignore_collision_with is not None else [] for obj in robot.world.objects: if obj in ([robot, floor] + self.ignore_collision_with): continue @@ -210,7 +214,7 @@ def __iter__(self): with UseProspectionWorld(): for maybe_pose in PoseGenerator(final_map, number_of_samples=600): if self.check_collision_at_start and (test_robot is not None): - if self.check_for_collision(test_robot, maybe_pose): + if self.check_for_collision(test_robot, maybe_pose, self.ignore_collision_with): continue res = True arms = None From 3602bb182b75a8f8b1c3b36160827ff75a4f608b Mon Sep 17 00:00:00 2001 From: A_Bassiouny Date: Tue, 12 Nov 2024 11:28:40 +0100 Subject: [PATCH 45/91] [ActionLocationDesignators] can ignore collisions with certain specified objects. add prepose distance variable to transportation and picking up. --- src/pycram/designators/location_designator.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/pycram/designators/location_designator.py b/src/pycram/designators/location_designator.py index 3e324f7ad..9bf27e948 100644 --- a/src/pycram/designators/location_designator.py +++ b/src/pycram/designators/location_designator.py @@ -161,7 +161,7 @@ def check_for_collision(robot: Object, pose: Pose, floor = robot.world.get_object_by_name("floor") ignore_collision_with = ignore_collision_with if ignore_collision_with is not None else [] for obj in robot.world.objects: - if obj in ([robot, floor] + self.ignore_collision_with): + if obj in ([robot, floor] + ignore_collision_with): continue if contact(robot, obj): logdebug(f"Robot is in contact with {obj.name} in prospection: {obj.world.is_prospection_world}" From 37973b68c8e69cb4c3a9b03824361a2677e7e629 Mon Sep 17 00:00:00 2001 From: A_Bassiouny Date: Tue, 12 Nov 2024 11:33:49 +0100 Subject: [PATCH 46/91] [ActionLocationDesignators] don't resolve costmap --- src/pycram/designators/action_designator.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/pycram/designators/action_designator.py b/src/pycram/designators/action_designator.py index b3d6db6ec..3931e9f7d 100644 --- a/src/pycram/designators/action_designator.py +++ b/src/pycram/designators/action_designator.py @@ -423,7 +423,7 @@ def plan(self) -> None: robot_desig_resolved = BelieveObject(names=[RobotDescription.current_robot_description.name]).resolve() ParkArmsActionPerformable(Arms.BOTH).perform() pickup_loc = CostmapLocation(target=self.object_designator, reachable_for=robot_desig_resolved, - reachable_arm=self.arm, prepose_distance=self.pickup_prepose_distance).resolve() + reachable_arm=self.arm, prepose_distance=self.pickup_prepose_distance) # Tries to find a pick-up position for the robot that uses the given arm pickup_pose = None for pose in pickup_loc: From 62b5d11e3b2c9bb42208af8ce2efd50524539e1a Mon Sep 17 00:00:00 2001 From: A_Bassiouny Date: Tue, 12 Nov 2024 11:44:07 +0100 Subject: [PATCH 47/91] [CostmapLocation] don't compare objects but object names when checking for collision. --- src/pycram/designators/location_designator.py | 11 ++++------- 1 file changed, 4 insertions(+), 7 deletions(-) diff --git a/src/pycram/designators/location_designator.py b/src/pycram/designators/location_designator.py index 9bf27e948..1b1722cf9 100644 --- a/src/pycram/designators/location_designator.py +++ b/src/pycram/designators/location_designator.py @@ -146,22 +146,19 @@ def ground(self) -> Location: """ return next(iter(self)) - @staticmethod - def check_for_collision(robot: Object, pose: Pose, - ignore_collision_with: Optional[List[Object]] = None) -> bool: + def check_for_collision(self, robot: Object, pose: Pose) -> bool: """ Check if the robot collides with any object in the world at the given pose. :param robot: The robot object :param pose: The pose to check for collision - :param ignore_collision_with: List of objects that should be ignored for collision checking. :return: True if the robot collides with any object, False otherwise """ robot.set_pose(pose) floor = robot.world.get_object_by_name("floor") - ignore_collision_with = ignore_collision_with if ignore_collision_with is not None else [] + ignore = [o.name for o in self.ignore_collision_with] for obj in robot.world.objects: - if obj in ([robot, floor] + ignore_collision_with): + if obj.name in ([robot.name, floor.name] + ignore): continue if contact(robot, obj): logdebug(f"Robot is in contact with {obj.name} in prospection: {obj.world.is_prospection_world}" @@ -214,7 +211,7 @@ def __iter__(self): with UseProspectionWorld(): for maybe_pose in PoseGenerator(final_map, number_of_samples=600): if self.check_collision_at_start and (test_robot is not None): - if self.check_for_collision(test_robot, maybe_pose, self.ignore_collision_with): + if self.check_for_collision(test_robot, maybe_pose): continue res = True arms = None From ff2381d134e476b97ee6d7461f3cf288fc6ca2b2 Mon Sep 17 00:00:00 2001 From: A_Bassiouny Date: Tue, 12 Nov 2024 11:54:16 +0100 Subject: [PATCH 48/91] [CostmapLocation] add ignored collisions to the allowed collisions of reachability validator. --- src/pycram/designators/location_designator.py | 6 +++++- src/pycram/pose_generator_and_validator.py | 3 ++- 2 files changed, 7 insertions(+), 2 deletions(-) diff --git a/src/pycram/designators/location_designator.py b/src/pycram/designators/location_designator.py index 1b1722cf9..eedcc601a 100644 --- a/src/pycram/designators/location_designator.py +++ b/src/pycram/designators/location_designator.py @@ -227,8 +227,12 @@ def __iter__(self): else: for description in RobotDescription.current_robot_description.get_manipulator_chains(): hand_links += description.end_effector.links + allowed_collision = {test_robot: hand_links} + ignore_collision_with = [World.current_world.get_prospection_object_for_object(o) for o in + self.ignore_collision_with] + allowed_collision.update({o: o.links for o in ignore_collision_with}) valid, arms = reachability_validator(maybe_pose, test_robot, target_pose, - allowed_collision={test_robot: hand_links}, + allowed_collision=allowed_collision, arm=self.reachable_arm, prepose_distance=self.prepose_distance) if self.reachable_arm: diff --git a/src/pycram/pose_generator_and_validator.py b/src/pycram/pose_generator_and_validator.py index e87d87b47..b3e09ef7b 100644 --- a/src/pycram/pose_generator_and_validator.py +++ b/src/pycram/pose_generator_and_validator.py @@ -172,7 +172,8 @@ def reachability_validator(pose: Pose, :param robot: The robot object in the World for which the reachability should be validated. :param target: The target position or object instance which should be the target for reachability. :param prepose_distance: The distance the robot should retract from the target position after/before reaching it. - :param allowed_collision: dict of objects with which the robot is allowed to collide each object correlates to a list of links of which this object consists + :param allowed_collision: dict of objects with which the robot is allowed to collide each object correlates + to a list of links of which this object consists :param arm: The arm that should be used for the reachability check. If None all arms are checked. :return: True if the target is reachable for the robot and False in any other case. """ From dbc190e4ab1d6b57c8e48cd4d6fffed231cabaaf Mon Sep 17 00:00:00 2001 From: A_Bassiouny Date: Tue, 12 Nov 2024 12:04:36 +0100 Subject: [PATCH 49/91] [CostmapLocation] use link names instead of link objects in allowed collisions of reachability validator. --- src/pycram/designators/location_designator.py | 7 +++---- 1 file changed, 3 insertions(+), 4 deletions(-) diff --git a/src/pycram/designators/location_designator.py b/src/pycram/designators/location_designator.py index eedcc601a..aec677779 100644 --- a/src/pycram/designators/location_designator.py +++ b/src/pycram/designators/location_designator.py @@ -207,7 +207,8 @@ def __iter__(self): if self.visible_for or self.reachable_for: robot_object = self.visible_for.world_object if self.visible_for else self.reachable_for.world_object test_robot = World.current_world.get_prospection_object_for_object(robot_object) - + self.ignore_collision_with = [World.current_world.get_prospection_object_for_object(o) for o in + self.ignore_collision_with] with UseProspectionWorld(): for maybe_pose in PoseGenerator(final_map, number_of_samples=600): if self.check_collision_at_start and (test_robot is not None): @@ -228,9 +229,7 @@ def __iter__(self): for description in RobotDescription.current_robot_description.get_manipulator_chains(): hand_links += description.end_effector.links allowed_collision = {test_robot: hand_links} - ignore_collision_with = [World.current_world.get_prospection_object_for_object(o) for o in - self.ignore_collision_with] - allowed_collision.update({o: o.links for o in ignore_collision_with}) + allowed_collision.update({o: o.link_names for o in self.ignore_collision_with}) valid, arms = reachability_validator(maybe_pose, test_robot, target_pose, allowed_collision=allowed_collision, arm=self.reachable_arm, From 4005d886aaa8fb4bf8c519032107a447835b353a Mon Sep 17 00:00:00 2001 From: A_Bassiouny Date: Tue, 12 Nov 2024 12:19:27 +0100 Subject: [PATCH 50/91] [Multiverse] add option to ignore contacts of attached objects when querying for contacts of the parent object. --- src/pycram/worlds/multiverse.py | 12 +++++++++++- 1 file changed, 11 insertions(+), 1 deletion(-) diff --git a/src/pycram/worlds/multiverse.py b/src/pycram/worlds/multiverse.py index 84f2e139a..5e0e4bba8 100644 --- a/src/pycram/worlds/multiverse.py +++ b/src/pycram/worlds/multiverse.py @@ -541,11 +541,21 @@ def remove_constraint(self, constraint_id) -> None: def perform_collision_detection(self) -> None: ... - def get_object_contact_points(self, obj: Object) -> ContactPointsList: + def get_object_contact_points(self, obj: Object, ignore_attached_objects: bool = True) -> ContactPointsList: """ Note: Currently Multiverse only gets one contact point per contact objects. + + :param obj: The object. + :param ignore_attached_objects: Whether to ignore the attached objects or not. + :return: The contact points of the object. """ multiverse_contact_points = self.api_requester.get_contact_points(obj.name) + if ignore_attached_objects: + attached_objects_link_names = [] + for att_obj in obj.attachments.keys(): + attached_objects_link_names.extend(att_obj.link_names) + multiverse_contact_points = [mcp for mcp in multiverse_contact_points if + mcp.body_1 not in attached_objects_link_names] contact_points = ContactPointsList([]) for mcp in multiverse_contact_points: body_object, body_link = self.get_object_with_body_name(mcp.body_2) From 20f6a2106a9abe9e96d4a4857cc11aa645b2b5c4 Mon Sep 17 00:00:00 2001 From: A_Bassiouny Date: Tue, 12 Nov 2024 12:41:37 +0100 Subject: [PATCH 51/91] [LocationDesignator] exclude hand endeffector links not hand links in reachability validator. --- demos/pycram_multiverse_demo/demo.py | 6 ++++-- src/pycram/designators/location_designator.py | 2 +- 2 files changed, 5 insertions(+), 3 deletions(-) diff --git a/demos/pycram_multiverse_demo/demo.py b/demos/pycram_multiverse_demo/demo.py index bf2a9cb98..ecf68165b 100644 --- a/demos/pycram_multiverse_demo/demo.py +++ b/demos/pycram_multiverse_demo/demo.py @@ -58,7 +58,8 @@ def move_and_detect(obj_type: ObjectType, pick_pose: Pose): NavigateAction([drawer_open_loc.pose]).resolve().perform() - OpenAction(object_designator_description=handle_desig, arms=[drawer_open_loc.arms[0]]).resolve().perform() + OpenAction(object_designator_description=handle_desig, + arms=[drawer_open_loc.arms[0]]).resolve().perform() spoon.detach(apartment) # Detect and pickup the spoon @@ -79,7 +80,8 @@ def move_and_detect(obj_type: ObjectType, pick_pose: Pose): # Find a pose to place the spoon, move and then place it spoon_target_pose = Pose([2.35, 2.6, 0.95], [0, 0, 0, 1]) - placing_loc = CostmapLocation(target=spoon_target_pose, reachable_for=robot_desig.resolve()).resolve() + placing_loc = CostmapLocation(target=spoon_target_pose, + reachable_for=robot_desig.resolve()).resolve() NavigateAction([placing_loc.pose]).resolve().perform() diff --git a/src/pycram/designators/location_designator.py b/src/pycram/designators/location_designator.py index aec677779..65e88a65d 100644 --- a/src/pycram/designators/location_designator.py +++ b/src/pycram/designators/location_designator.py @@ -316,7 +316,7 @@ def __iter__(self) -> Location: hand_links = [] for description in RobotDescription.current_robot_description.get_manipulator_chains(): - hand_links += description.links + hand_links += description.end_effector.links valid_init, arms_init = reachability_validator(maybe_pose, test_robot, init_pose, allowed_collision={test_robot: hand_links}) From f3f532725e748bae329acf2ba2434d3b2c0920a7 Mon Sep 17 00:00:00 2001 From: A_Bassiouny Date: Tue, 12 Nov 2024 12:42:17 +0100 Subject: [PATCH 52/91] [Multiverse] check if object has attachments in get contact to avoid unneeded processing. --- src/pycram/worlds/multiverse.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/pycram/worlds/multiverse.py b/src/pycram/worlds/multiverse.py index 5e0e4bba8..34e96e626 100644 --- a/src/pycram/worlds/multiverse.py +++ b/src/pycram/worlds/multiverse.py @@ -550,7 +550,7 @@ def get_object_contact_points(self, obj: Object, ignore_attached_objects: bool = :return: The contact points of the object. """ multiverse_contact_points = self.api_requester.get_contact_points(obj.name) - if ignore_attached_objects: + if ignore_attached_objects and len(obj.attachments) > 0: attached_objects_link_names = [] for att_obj in obj.attachments.keys(): attached_objects_link_names.extend(att_obj.link_names) From b0d4178396515caa35eb1cf275df555b23b4bf49 Mon Sep 17 00:00:00 2001 From: Abdelrhman Bassiouny Date: Tue, 12 Nov 2024 14:58:00 +0100 Subject: [PATCH 53/91] [Pr2ProcessModules] MoveTCP can include motion of base. --- src/pycram/process_modules/pr2_process_modules.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/pycram/process_modules/pr2_process_modules.py b/src/pycram/process_modules/pr2_process_modules.py index f5571f62e..a6860db22 100644 --- a/src/pycram/process_modules/pr2_process_modules.py +++ b/src/pycram/process_modules/pr2_process_modules.py @@ -239,7 +239,7 @@ def _execute(self, designator: MoveTCPMotion) -> Any: lt = LocalTransformer() pose_in_map = lt.transform_pose(designator.target, "map") tip_link = RobotDescription.current_robot_description.get_arm_chain(designator.arm).get_tool_frame() - root_link = "torso_lift_link" + root_link = RobotDescription.current_robot_description.base_link if designator.allow_gripper_collision: giskard.allow_gripper_collision(designator.arm.name.lower()) From 88274ae495ce1529087232caecbe9958628fb3a7 Mon Sep 17 00:00:00 2001 From: Abdelrhman Bassiouny Date: Tue, 12 Nov 2024 15:08:26 +0100 Subject: [PATCH 54/91] [Pr2ProcessModules] MoveTCP can include motion of base. --- src/pycram/process_modules/pr2_process_modules.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/pycram/process_modules/pr2_process_modules.py b/src/pycram/process_modules/pr2_process_modules.py index a6860db22..44534c44d 100644 --- a/src/pycram/process_modules/pr2_process_modules.py +++ b/src/pycram/process_modules/pr2_process_modules.py @@ -239,7 +239,7 @@ def _execute(self, designator: MoveTCPMotion) -> Any: lt = LocalTransformer() pose_in_map = lt.transform_pose(designator.target, "map") tip_link = RobotDescription.current_robot_description.get_arm_chain(designator.arm).get_tool_frame() - root_link = RobotDescription.current_robot_description.base_link + root_link = "map" if designator.allow_gripper_collision: giskard.allow_gripper_collision(designator.arm.name.lower()) From c0330b7bd81027333851a192ac54a9240e2269f7 Mon Sep 17 00:00:00 2001 From: Abdelrhman Bassiouny Date: Tue, 12 Nov 2024 15:44:16 +0100 Subject: [PATCH 55/91] [ActionDesignator] better prepose and approach technique for picking up milk. --- src/pycram/process_modules/pr2_process_modules.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/pycram/process_modules/pr2_process_modules.py b/src/pycram/process_modules/pr2_process_modules.py index 44534c44d..f35561b84 100644 --- a/src/pycram/process_modules/pr2_process_modules.py +++ b/src/pycram/process_modules/pr2_process_modules.py @@ -297,7 +297,7 @@ def feedback_callback(msg): pass goal = GripperCommandGoal() - goal.command.position = 0.0 if designator.motion == GripperState.CLOSE else 0.4 + goal.command.position = 0.0 if designator.motion == GripperState.CLOSE else 0.7 goal.command.max_effort = 50.0 if designator.gripper == "right": controller_topic = "/real/pr2/right_gripper_controller/gripper_cmd" From b174d05cf49fffde361986fffecaf813bd4d156f Mon Sep 17 00:00:00 2001 From: Abdelrhman Bassiouny Date: Tue, 12 Nov 2024 19:15:52 +0100 Subject: [PATCH 56/91] [MultiverseFallschoolDemo] The demo works. --- .../pycram_multiverse_demo/fallschool_demo.py | 36 ++++++++----------- src/pycram/designators/action_designator.py | 2 +- .../process_modules/pr2_process_modules.py | 2 +- 3 files changed, 17 insertions(+), 23 deletions(-) diff --git a/demos/pycram_multiverse_demo/fallschool_demo.py b/demos/pycram_multiverse_demo/fallschool_demo.py index 169a024b3..b1ec66f0a 100644 --- a/demos/pycram_multiverse_demo/fallschool_demo.py +++ b/demos/pycram_multiverse_demo/fallschool_demo.py @@ -41,16 +41,14 @@ def move_and_detect(obj_type: ObjectType, pick_pose: Pose): milk = Object("milk", ObjectType.MILK, f"milk.xml", pose=Pose([0.4, 2.6, 1.34], [1, 0, 0, 0]), color=Color(1, 0, 0, 1)) + # apartment.set_joint_position("fridge_door1_joint", 1.5707963267948966) -# milk.set_orientation(Pose(orientation=[1, 0, 0, 1])) -# apartment.attach(milk, 'fridge_base') + fridge_base_pose = apartment.get_link_pose("fridge_base") fridge_base_pose.position.z -= 0.12 fridge_base_pose.position.x += 0.16 fridge_base_pose.position.y += -0.1 milk.set_pose(fridge_base_pose, base=True) -# print(milk.get_position_as_list()) -# print(milk.get_orientation_as_list()) robot_desig = BelieveObject(names=[robot.name]) @@ -59,23 +57,19 @@ def move_and_detect(obj_type: ObjectType, pick_pose: Pose): with real_robot: # Transport the milkMoveGripperMotion - # ParkArmsAction([Arms.BOTH]).resolve().perform() - # - # MoveTorsoAction([0.2]).resolve().perform() - # - # NavigateAction(target_locations=[Pose([1.1, 3.15, 0.01], quaternion_from_euler(0, 0, -2.9))]).resolve().perform() - # - # LookAtAction(targets=[Pose(milk.get_position_as_list())]).resolve().perform() - - # world.restore_physics_simulator_state(100) - - # try: - # milk_desig = DetectAction(BelieveObject(types=[milk.obj_type])).resolve().perform() - # except PerceptionObjectNotFound: - # milk_desig = DetectAction(BelieveObject(types=[milk.obj_type])).resolve().perform() - - # world.save_state(100) - milk_desig = BelieveObject(names=[milk.name]) + ParkArmsAction([Arms.BOTH]).resolve().perform() + + MoveTorsoAction([0.2]).resolve().perform() + + NavigateAction(target_locations=[Pose([1.1, 3.15, 0.01], quaternion_from_euler(0, 0, -2.9))]).resolve().perform() + + LookAtAction(targets=[Pose(milk.get_position_as_list())]).resolve().perform() + + try: + milk_desig = DetectAction(BelieveObject(types=[milk.obj_type])).resolve().perform() + except PerceptionObjectNotFound: + milk_desig = DetectAction(BelieveObject(types=[milk.obj_type])).resolve().perform() + TransportAction(milk_desig, [Arms.LEFT], [Pose([2.4, 3, 1.02])]).resolve().perform() ParkArmsAction([Arms.BOTH]).resolve().perform() diff --git a/src/pycram/designators/action_designator.py b/src/pycram/designators/action_designator.py index 3931e9f7d..3b7f35609 100644 --- a/src/pycram/designators/action_designator.py +++ b/src/pycram/designators/action_designator.py @@ -367,7 +367,7 @@ def plan(self) -> None: World.robot.detach(self.object_designator.world_object) retract_pose = local_tf.transform_pose(target_diff, World.robot.get_link_tf_frame( RobotDescription.current_robot_description.get_arm_chain(self.arm).get_tool_frame())) - retract_pose.position.x -= 0.07 + retract_pose.position.x -= 0.03 MoveTCPMotion(retract_pose, self.arm).perform() diff --git a/src/pycram/process_modules/pr2_process_modules.py b/src/pycram/process_modules/pr2_process_modules.py index f35561b84..44534c44d 100644 --- a/src/pycram/process_modules/pr2_process_modules.py +++ b/src/pycram/process_modules/pr2_process_modules.py @@ -297,7 +297,7 @@ def feedback_callback(msg): pass goal = GripperCommandGoal() - goal.command.position = 0.0 if designator.motion == GripperState.CLOSE else 0.7 + goal.command.position = 0.0 if designator.motion == GripperState.CLOSE else 0.4 goal.command.max_effort = 50.0 if designator.gripper == "right": controller_topic = "/real/pr2/right_gripper_controller/gripper_cmd" From 5f7ba6d3b0315b440c47e4570852e5e214404419 Mon Sep 17 00:00:00 2001 From: Abdelrhman Bassiouny Date: Tue, 12 Nov 2024 19:16:19 +0100 Subject: [PATCH 57/91] [World] Save and Restore state from a file. --- config/multiverse_conf.py | 2 +- src/pycram/datastructures/world.py | 7 +++- src/pycram/datastructures/world_entity.py | 50 +++++++++++++++++++++-- 3 files changed, 52 insertions(+), 7 deletions(-) diff --git a/config/multiverse_conf.py b/config/multiverse_conf.py index fc4b67e78..5688fbb55 100644 --- a/config/multiverse_conf.py +++ b/config/multiverse_conf.py @@ -57,7 +57,7 @@ class MultiverseConfig(WorldConfig): The default description type for the objects. """ - use_physics_simulator_state: bool = True + use_physics_simulator_state: bool = False """ Whether to use the physics simulator state when restoring or saving the world state. """ diff --git a/src/pycram/datastructures/world.py b/src/pycram/datastructures/world.py index c4f3ed73f..aba76fe8b 100644 --- a/src/pycram/datastructures/world.py +++ b/src/pycram/datastructures/world.py @@ -1029,13 +1029,14 @@ def terminate_world_sync(self) -> None: self.world_sync.terminate = True self.world_sync.join() - def save_state(self, state_id: Optional[int] = None, use_same_id: bool = False) -> int: + def save_state(self, state_id: Optional[int] = None, use_same_id: bool = False, to_file: bool = False) -> int: """ Return the id of the saved state of the World. The saved state contains the states of all the objects and the state of the physics simulator. :param state_id: The id of the saved state. :param use_same_id: Whether to use the same current state id for the new saved state. + :param to_file: Whether to save the state to a file. :return: A unique id of the state """ @@ -1052,7 +1053,7 @@ def save_state(self, state_id: Optional[int] = None, use_same_id: bool = False) self._current_state = WorldState(self.object_states, sim_state_id) - return super().save_state(state_id) + return super().save_state(state_id, self.get_cache_dir() if to_file else None) @property def current_state(self) -> WorldState: @@ -1083,6 +1084,8 @@ def set_object_states_without_poses(self, states: Dict[str, ObjectState]) -> Non obj.set_attachments(obj_state.attachments) obj.link_states = obj_state.link_states obj.joint_states = obj_state.joint_states + if obj.name == self.robot.name and len(self.robot_virtual_joints) > 0: + obj.set_mobile_robot_pose(obj_state.pose) @property def object_states(self) -> Dict[str, ObjectState]: diff --git a/src/pycram/datastructures/world_entity.py b/src/pycram/datastructures/world_entity.py index 1e7c61e06..41c317fad 100644 --- a/src/pycram/datastructures/world_entity.py +++ b/src/pycram/datastructures/world_entity.py @@ -1,6 +1,8 @@ +import os +import pickle from abc import ABC, abstractmethod -from typing_extensions import TYPE_CHECKING, Dict +from typing_extensions import TYPE_CHECKING, Dict, Optional from .dataclasses import State @@ -14,6 +16,8 @@ class StateEntity: restore the state of the World. """ + SAVED_FILE_PREFIX = 'pycram_saved_state_' + def __init__(self): self._saved_states: Dict[int, State] = {} @@ -24,15 +28,49 @@ def saved_states(self) -> Dict[int, State]: """ return self._saved_states - def save_state(self, state_id: int) -> int: + def save_state(self, state_id: int, save_dir: Optional[str] = None) -> int: """ Saves the state of this entity with the given state id. :param state_id: The unique id of the state. + :param save_dir: The directory where the file should be saved. """ self._saved_states[state_id] = self.current_state + if save_dir is not None: + self.save_state_to_file(self.get_saved_file_path_of_state(save_dir, state_id), state_id) return state_id + def save_state_to_file(self, file_path: str, state_id: Optional[int] = None): + """ + Saves the state of this entity to a file. + + :param file_path: The path to the file. + :param state_id: The unique id of the state if saving a specific state not the current state. + """ + state_to_save = self.current_state if state_id is None else self.saved_states[state_id] + with open(file_path, 'wb') as file: + pickle.dump(state_to_save, file) + + def restore_state_from_file(self, save_dir: str, state_id: int) -> None: + """ + Restores the state of this entity from a file. + + :param save_dir: The directory where the file is saved. + :param state_id: The unique id of the state. + """ + with open(self.get_saved_file_path_of_state(save_dir, state_id), 'rb') as file: + self.current_state = pickle.load(file) + + def get_saved_file_path_of_state(self, save_dir: str, state_id: int) -> str: + """ + Gets the path of the file that stores the state with the given id. + + :param save_dir: The directory where the file should be saved. + :param state_id: The unique id of the state. + :return: The name of the file that stores the state with the given id. + """ + return os.path.join(save_dir, f'{self.SAVED_FILE_PREFIX}{state_id}.pkl') + @property @abstractmethod def current_state(self) -> State: @@ -51,13 +89,17 @@ def current_state(self, state: State) -> None: """ pass - def restore_state(self, state_id: int) -> None: + def restore_state(self, state_id: int, saved_file_dir: Optional[str] = None) -> None: """ Restores the state of this entity from a saved state using the given state id. :param state_id: The unique id of the state. + :param saved_file_dir: The directory where the file is saved. """ - self.current_state = self.saved_states[state_id] + if saved_file_dir is not None: + self.restore_state_from_file(saved_file_dir, state_id) + else: + self.current_state = self.saved_states[state_id] def remove_saved_states(self) -> None: """ From 9437e47b9617ce672fcebee3202a2c32e0654372 Mon Sep 17 00:00:00 2001 From: A_Bassiouny Date: Wed, 13 Nov 2024 00:46:08 +0100 Subject: [PATCH 58/91] [MultiverseFallschoolDemo] adjusting grasping pose and looking at pose. --- demos/pycram_multiverse_demo/fallschool_demo.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/demos/pycram_multiverse_demo/fallschool_demo.py b/demos/pycram_multiverse_demo/fallschool_demo.py index b1ec66f0a..1754fb0d8 100644 --- a/demos/pycram_multiverse_demo/fallschool_demo.py +++ b/demos/pycram_multiverse_demo/fallschool_demo.py @@ -35,7 +35,7 @@ def move_and_detect(obj_type: ObjectType, pick_pose: Pose): world = Multiverse() robot = Object('pr2', ObjectType.ROBOT, f'pr2.urdf', pose=Pose([1.3, 2.6, 0.01])) -WorldStateUpdater(tf_topic="/tf", joint_state_topic="/real/pr2/joint_states", update_rate=timedelta(seconds=1), +WorldStateUpdater(tf_topic="/tf", joint_state_topic="/real/pr2/joint_states", update_rate=timedelta(seconds=2), world=world) apartment = Object("apartment", ObjectType.ENVIRONMENT, f"apartment.urdf") milk = Object("milk", ObjectType.MILK, f"milk.xml", pose=Pose([0.4, 2.6, 1.34], @@ -61,7 +61,7 @@ def move_and_detect(obj_type: ObjectType, pick_pose: Pose): MoveTorsoAction([0.2]).resolve().perform() - NavigateAction(target_locations=[Pose([1.1, 3.15, 0.01], quaternion_from_euler(0, 0, -2.9))]).resolve().perform() + NavigateAction(target_locations=[Pose([1.4, 3.15, 0.01], quaternion_from_euler(0, 0, 3.14))]).resolve().perform() LookAtAction(targets=[Pose(milk.get_position_as_list())]).resolve().perform() From 6b3cd2ed2015efd603488c2fce61e846646bbf59 Mon Sep 17 00:00:00 2001 From: Abdelrhman Bassiouny Date: Wed, 13 Nov 2024 09:16:13 +0100 Subject: [PATCH 59/91] [PickupAction] Check that the object is in contact with the gripper after it closed. Also wait for gripper action has now a limited duration of 5 seconds. --- src/pycram/designators/action_designator.py | 1 + src/pycram/process_modules/pr2_process_modules.py | 6 ++++-- src/pycram/world_reasoning.py | 4 ++-- 3 files changed, 7 insertions(+), 4 deletions(-) diff --git a/src/pycram/designators/action_designator.py b/src/pycram/designators/action_designator.py index 3b7f35609..541cad1f5 100644 --- a/src/pycram/designators/action_designator.py +++ b/src/pycram/designators/action_designator.py @@ -23,6 +23,7 @@ from ..failures import ObjectUnfetchable, ReachabilityFailure from ..robot_description import RobotDescription from ..tasktree import with_tree +from ..world_reasoning import contact from owlready2 import Thing diff --git a/src/pycram/process_modules/pr2_process_modules.py b/src/pycram/process_modules/pr2_process_modules.py index 44534c44d..8384e5522 100644 --- a/src/pycram/process_modules/pr2_process_modules.py +++ b/src/pycram/process_modules/pr2_process_modules.py @@ -5,6 +5,7 @@ from .default_process_modules import DefaultDetectingReal, DefaultDetecting from .. import world_reasoning as btr import numpy as np +import rospy from .. import world_reasoning as btr from ..external_interfaces.move_base import query_pose_nav @@ -294,7 +295,7 @@ def done_callback(state, result): loginfo(f"Reached goal {designator.motion}: {result.reached_goal}") def feedback_callback(msg): - pass + loginfo(f"Gripper Action Feedback: {msg}") goal = GripperCommandGoal() goal.command.position = 0.0 if designator.motion == GripperState.CLOSE else 0.4 @@ -307,7 +308,8 @@ def feedback_callback(msg): loginfo("Waiting for action server") client.wait_for_server() client.send_goal(goal, active_cb=activate_callback, done_cb=done_callback, feedback_cb=feedback_callback) - wait = client.wait_for_result() + wait = client.wait_for_result(rospy.Duration.from_sec(5)) + # client.cancel_all_goals() class Pr2MoveGripperReal(ProcessModule): diff --git a/src/pycram/world_reasoning.py b/src/pycram/world_reasoning.py index 8fd0501b9..214aefb68 100644 --- a/src/pycram/world_reasoning.py +++ b/src/pycram/world_reasoning.py @@ -7,7 +7,7 @@ from .external_interfaces.ik import try_to_reach, try_to_reach_with_grasp from .robot_description import RobotDescription from .utils import RayTestUtils -from .world_concepts.world_object import Object +from .world_concepts.world_object import Object, Link from .config import world_conf as conf @@ -36,7 +36,7 @@ def stable(obj: Object) -> bool: def contact( object1: Object, object2: Object, - return_links: bool = False) -> Union[bool, Tuple[bool, List]]: + return_links: bool = False) -> Union[bool, Tuple[bool, List[Tuple[Link, Link]]]]: """ Checks if two objects are in contact or not. If the links should be returned then the output will also contain a list of tuples where the first element is the link name of 'object1' and the second element is the link name of From ccf743ef09b15d6990e8fab57d7b394c37463c03 Mon Sep 17 00:00:00 2001 From: A_Bassiouny Date: Wed, 13 Nov 2024 12:32:56 +0100 Subject: [PATCH 60/91] [MultiverseFallschoolDemo] try and redo actions Added mutex for multiverse writer client --- .../pycram_multiverse_demo/fallschool_demo.py | 9 +++---- src/pycram/designators/action_designator.py | 21 ++++++++++++--- src/pycram/failures.py | 5 ++++ .../process_modules/pr2_process_modules.py | 27 +++++++++---------- .../multiverse_communication/clients.py | 5 ++++ 5 files changed, 43 insertions(+), 24 deletions(-) diff --git a/demos/pycram_multiverse_demo/fallschool_demo.py b/demos/pycram_multiverse_demo/fallschool_demo.py index 1754fb0d8..e43b9cf6e 100644 --- a/demos/pycram_multiverse_demo/fallschool_demo.py +++ b/demos/pycram_multiverse_demo/fallschool_demo.py @@ -11,7 +11,7 @@ from pycram.designators.action_designator import ParkArmsAction, MoveTorsoAction, TransportAction, NavigateAction, \ LookAtAction, DetectAction from pycram.designators.object_designator import BelieveObject -from pycram.failures import PerceptionObjectNotFound +from pycram.failures import PerceptionObjectNotFound, NavigationGoalNotReachedError from pycram.process_module import simulated_robot, with_simulated_robot, real_robot from pycram.ros_utils.robot_state_updater import WorldStateUpdater from pycram.world_concepts.world_object import Object @@ -54,6 +54,7 @@ def move_and_detect(obj_type: ObjectType, pick_pose: Pose): robot_desig = BelieveObject(names=[robot.name]) apartment_desig = BelieveObject(names=[apartment.name]) + with real_robot: # Transport the milkMoveGripperMotion @@ -65,11 +66,9 @@ def move_and_detect(obj_type: ObjectType, pick_pose: Pose): LookAtAction(targets=[Pose(milk.get_position_as_list())]).resolve().perform() - try: - milk_desig = DetectAction(BelieveObject(types=[milk.obj_type])).resolve().perform() - except PerceptionObjectNotFound: - milk_desig = DetectAction(BelieveObject(types=[milk.obj_type])).resolve().perform() + milk_desig = DetectAction(BelieveObject(types=[milk.obj_type])).resolve().perform() + # milk_desig = BelieveObject(names=[milk.name]) TransportAction(milk_desig, [Arms.LEFT], [Pose([2.4, 3, 1.02])]).resolve().perform() ParkArmsAction([Arms.BOTH]).resolve().perform() diff --git a/src/pycram/designators/action_designator.py b/src/pycram/designators/action_designator.py index 541cad1f5..bc08b826f 100644 --- a/src/pycram/designators/action_designator.py +++ b/src/pycram/designators/action_designator.py @@ -20,7 +20,7 @@ VisibleProperty from ..knowledge.knowledge_engine import ReasoningInstance from ..local_transformer import LocalTransformer -from ..failures import ObjectUnfetchable, ReachabilityFailure +from ..failures import ObjectUnfetchable, ReachabilityFailure, NavigationGoalNotReachedError, PerceptionObjectNotFound from ..robot_description import RobotDescription from ..tasktree import with_tree from ..world_reasoning import contact @@ -46,6 +46,17 @@ from dataclasses import dataclass, field +def try_action(action, exception, num_retries=3): + current_retry = 0 + result = None + while current_retry < num_retries: + try: + result = action.perform() + break + except exception: + current_retry += 1 + return result + # ---------------------------------------------------------------------------- # ---------------- Performables ---------------------------------------------- # ---------------------------------------------------------------------------- @@ -392,7 +403,9 @@ class NavigateActionPerformable(ActionAbstract): @with_tree def plan(self) -> None: - MoveMotion(self.target_location, self.keep_joint_states).perform() + motion_action = MoveMotion(self.target_location, self.keep_joint_states) + return try_action(motion_action, NavigationGoalNotReachedError) + @dataclass @@ -495,8 +508,8 @@ class DetectActionPerformable(ActionAbstract): @with_tree def plan(self) -> None: - return DetectingMotion(technique=self.technique,state=self.state, object_designator_description=self.object_designator_description, - region=self.region).perform() + return try_action(DetectingMotion(technique=self.technique,state=self.state, object_designator_description=self.object_designator_description, + region=self.region), PerceptionObjectNotFound) @dataclass diff --git a/src/pycram/failures.py b/src/pycram/failures.py index 4e86640d0..956201fe5 100644 --- a/src/pycram/failures.py +++ b/src/pycram/failures.py @@ -436,6 +436,11 @@ def __init__(self, *args, **kwargs): super().__init__(*args, **kwargs) +class NavigationGoalNotReachedError(PlanFailure): + def __init__(self, current_pose, goal_pose): + super().__init__(f"Navigation goal not reached. Current pose: {current_pose}, goal pose: {goal_pose}") + + class FailedAPIResponse(Exception): def __init__(self, api_response: List[str], api_name: MultiverseAPIName, *args, **kwargs): super().__init__(f"{api_name} api request with arguments {args} and keyword arguments {kwargs}" diff --git a/src/pycram/process_modules/pr2_process_modules.py b/src/pycram/process_modules/pr2_process_modules.py index 8384e5522..ef4e18bc0 100644 --- a/src/pycram/process_modules/pr2_process_modules.py +++ b/src/pycram/process_modules/pr2_process_modules.py @@ -1,5 +1,4 @@ from typing_extensions import TYPE_CHECKING - import actionlib from .default_process_modules import DefaultDetectingReal, DefaultDetecting @@ -7,26 +6,23 @@ import numpy as np import rospy -from .. import world_reasoning as btr from ..external_interfaces.move_base import query_pose_nav -from ..process_module import ProcessModule, ProcessModuleManager -from ..external_interfaces.ik import request_ik -from ..ros.logging import logdebug -from ..utils import _apply_ik -from ..local_transformer import LocalTransformer - from .. import world_reasoning as btr +from ..datastructures.enums import JointType, ObjectType, Arms, ExecutionType, GripperState, MovementType +from ..datastructures.world import World from ..designators.motion_designator import MoveMotion, LookingMotion, \ DetectingMotion, MoveTCPMotion, MoveArmJointsMotion, WorldStateDetectingMotion, MoveJointsMotion, \ MoveGripperMotion, OpeningMotion, ClosingMotion -from ..robot_description import RobotDescription -from ..datastructures.world import World -from ..world_concepts.world_object import Object -from ..datastructures.pose import Pose -from ..datastructures.enums import JointType, ObjectType, Arms, ExecutionType, MovementType, GripperState from ..external_interfaces import giskard +from ..external_interfaces.ik import request_ik from ..external_interfaces.robokudo import * -from ..ros.logging import loginfo, logwarn, logdebug +from ..failures import NavigationGoalNotReachedError +from ..local_transformer import LocalTransformer +from ..process_module import ProcessModule, ProcessModuleManager +from ..robot_description import RobotDescription +from ..ros.logging import logdebug +from ..utils import _apply_ik +from ..world_concepts.world_object import Object if TYPE_CHECKING: from ..designators.object_designator import ObjectDesignatorDescription @@ -204,7 +200,8 @@ class Pr2NavigationReal(ProcessModule): def _execute(self, designator: MoveMotion) -> Any: logdebug(f"Sending goal to movebase to Move the robot") query_pose_nav(designator.target) - + if not World.current_world.robot.pose.almost_equal(designator.target, 0.05, 3): + raise NavigationGoalNotReachedError(World.current_world.robot.pose, designator.target) class Pr2MoveHeadReal(ProcessModule): """ diff --git a/src/pycram/worlds/multiverse_communication/clients.py b/src/pycram/worlds/multiverse_communication/clients.py index 3dbccb2d9..44316dfb0 100644 --- a/src/pycram/worlds/multiverse_communication/clients.py +++ b/src/pycram/worlds/multiverse_communication/clients.py @@ -2,6 +2,8 @@ import logging import os import threading +from threading import RLock + from time import time, sleep from typing_extensions import List, Dict, Tuple, Optional, Callable, Union @@ -319,6 +321,7 @@ def __init__(self, name: str, port: int, simulation: Optional[str] = None, """ super().__init__(name, port, is_prospection_world, simulation_wait_time_factor=simulation_wait_time_factor) self.simulation = simulation + self.lock = RLock() def spawn_robot_with_actuators(self, robot_name: str, actuator_joint_commands: Optional[Dict[str, List[str]]] = None) -> None: @@ -466,12 +469,14 @@ def send_data_to_server(self, data: List, :param set_simulation_name: Whether to set the simulation name to the value of self.simulation. :return: The response from the server. """ + self.lock.acquire() self._reset_request_meta_data(set_simulation_name=set_simulation_name) if send_meta_data: self.request_meta_data["send"] = send_meta_data if receive_meta_data: self.request_meta_data["receive"] = receive_meta_data self.send_and_receive_meta_data() + self.lock.release() self.send_data = data self.send_and_receive_data() return self.response_meta_data From 1555dbc96c5473222a499cc9b7300db1c5d04698 Mon Sep 17 00:00:00 2001 From: A_Bassiouny Date: Wed, 13 Nov 2024 12:46:14 +0100 Subject: [PATCH 61/91] [MultiverseFallschoolDemo] try and redo motions --- .../pycram_multiverse_demo/fallschool_demo.py | 22 +++++++++---------- src/pycram/designators/motion_designator.py | 17 +++++++++++--- src/pycram/failures.py | 5 +++++ .../process_modules/pr2_process_modules.py | 4 +++- 4 files changed, 33 insertions(+), 15 deletions(-) diff --git a/demos/pycram_multiverse_demo/fallschool_demo.py b/demos/pycram_multiverse_demo/fallschool_demo.py index e43b9cf6e..79e5ca165 100644 --- a/demos/pycram_multiverse_demo/fallschool_demo.py +++ b/demos/pycram_multiverse_demo/fallschool_demo.py @@ -58,17 +58,17 @@ def move_and_detect(obj_type: ObjectType, pick_pose: Pose): with real_robot: # Transport the milkMoveGripperMotion - ParkArmsAction([Arms.BOTH]).resolve().perform() - - MoveTorsoAction([0.2]).resolve().perform() - - NavigateAction(target_locations=[Pose([1.4, 3.15, 0.01], quaternion_from_euler(0, 0, 3.14))]).resolve().perform() - - LookAtAction(targets=[Pose(milk.get_position_as_list())]).resolve().perform() - - milk_desig = DetectAction(BelieveObject(types=[milk.obj_type])).resolve().perform() - - # milk_desig = BelieveObject(names=[milk.name]) + # ParkArmsAction([Arms.BOTH]).resolve().perform() + # + # MoveTorsoAction([0.2]).resolve().perform() + # + # NavigateAction(target_locations=[Pose([1.4, 3.15, 0.01], quaternion_from_euler(0, 0, 3.14))]).resolve().perform() + # + # LookAtAction(targets=[Pose(milk.get_position_as_list())]).resolve().perform() + # + # milk_desig = DetectAction(BelieveObject(types=[milk.obj_type])).resolve().perform() + + milk_desig = BelieveObject(names=[milk.name]) TransportAction(milk_desig, [Arms.LEFT], [Pose([2.4, 3, 1.02])]).resolve().perform() ParkArmsAction([Arms.BOTH]).resolve().perform() diff --git a/src/pycram/designators/motion_designator.py b/src/pycram/designators/motion_designator.py index a743744c0..33b97d52a 100644 --- a/src/pycram/designators/motion_designator.py +++ b/src/pycram/designators/motion_designator.py @@ -1,4 +1,3 @@ -from abc import ABC, abstractmethod from dataclasses import dataclass from sqlalchemy.orm import Session @@ -6,7 +5,7 @@ from pycrap import PhysicalObject, Location from .object_designator import ObjectDesignatorDescription, ObjectPart, RealObject from ..datastructures.enums import MovementType -from ..failures import PerceptionObjectNotFound +from ..failures import PerceptionObjectNotFound, ToolPoseNotReachedError from ..process_module import ProcessModuleManager from ..orm.motion_designator import (MoveMotion as ORMMoveMotion, MoveTCPMotion as ORMMoveTCPMotion, LookingMotion as ORMLookingMotion, @@ -22,6 +21,18 @@ from ..external_interfaces.robokudo import robokudo_found +def try_motion(motion, motion_designator_instance, exception, num_retries=3): + current_retry = 0 + result = None + while current_retry < num_retries: + try: + result = motion.execute(motion_designator_instance) + break + except exception: + current_retry += 1 + return result + + @dataclass class MoveMotion(BaseMotion): """ @@ -82,7 +93,7 @@ class MoveTCPMotion(BaseMotion): @with_tree def perform(self): pm_manager = ProcessModuleManager.get_manager() - return pm_manager.move_tcp().execute(self) + try_motion(pm_manager.move_tcp(), self, ToolPoseNotReachedError) def to_sql(self) -> ORMMoveTCPMotion: return ORMMoveTCPMotion(self.arm, self.allow_gripper_collision) diff --git a/src/pycram/failures.py b/src/pycram/failures.py index 956201fe5..ef878f07a 100644 --- a/src/pycram/failures.py +++ b/src/pycram/failures.py @@ -441,6 +441,11 @@ def __init__(self, current_pose, goal_pose): super().__init__(f"Navigation goal not reached. Current pose: {current_pose}, goal pose: {goal_pose}") +class ToolPoseNotReachedError(PlanFailure): + def __init__(self, current_pose, goal_pose): + super().__init__(f"Tool pose not reached. Current pose: {current_pose}, goal pose: {goal_pose}") + + class FailedAPIResponse(Exception): def __init__(self, api_response: List[str], api_name: MultiverseAPIName, *args, **kwargs): super().__init__(f"{api_name} api request with arguments {args} and keyword arguments {kwargs}" diff --git a/src/pycram/process_modules/pr2_process_modules.py b/src/pycram/process_modules/pr2_process_modules.py index ef4e18bc0..3a1083617 100644 --- a/src/pycram/process_modules/pr2_process_modules.py +++ b/src/pycram/process_modules/pr2_process_modules.py @@ -16,7 +16,7 @@ from ..external_interfaces import giskard from ..external_interfaces.ik import request_ik from ..external_interfaces.robokudo import * -from ..failures import NavigationGoalNotReachedError +from ..failures import NavigationGoalNotReachedError, ToolPoseNotReachedError from ..local_transformer import LocalTransformer from ..process_module import ProcessModule, ProcessModuleManager from ..robot_description import RobotDescription @@ -251,6 +251,8 @@ def _execute(self, designator: MoveTCPMotion) -> Any: elif designator.movement_type == MovementType.CARTESIAN: giskard.achieve_cartesian_goal(pose_in_map, tip_link, root_link, use_monitor=World.current_world.conf.use_giskard_monitor) + 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) class Pr2MoveArmJointsReal(ProcessModule): From 6e9110b32521e1b5f7e0d11d8af76df2829ab784 Mon Sep 17 00:00:00 2001 From: A_Bassiouny Date: Wed, 13 Nov 2024 12:46:29 +0100 Subject: [PATCH 62/91] [MultiverseFallschoolDemo] try and redo motions --- .../pycram_multiverse_demo/fallschool_demo.py | 21 +++++++++---------- 1 file changed, 10 insertions(+), 11 deletions(-) diff --git a/demos/pycram_multiverse_demo/fallschool_demo.py b/demos/pycram_multiverse_demo/fallschool_demo.py index 79e5ca165..659d19f99 100644 --- a/demos/pycram_multiverse_demo/fallschool_demo.py +++ b/demos/pycram_multiverse_demo/fallschool_demo.py @@ -58,17 +58,16 @@ def move_and_detect(obj_type: ObjectType, pick_pose: Pose): with real_robot: # Transport the milkMoveGripperMotion - # ParkArmsAction([Arms.BOTH]).resolve().perform() - # - # MoveTorsoAction([0.2]).resolve().perform() - # - # NavigateAction(target_locations=[Pose([1.4, 3.15, 0.01], quaternion_from_euler(0, 0, 3.14))]).resolve().perform() - # - # LookAtAction(targets=[Pose(milk.get_position_as_list())]).resolve().perform() - # - # milk_desig = DetectAction(BelieveObject(types=[milk.obj_type])).resolve().perform() - - milk_desig = BelieveObject(names=[milk.name]) + ParkArmsAction([Arms.BOTH]).resolve().perform() + + MoveTorsoAction([0.2]).resolve().perform() + + NavigateAction(target_locations=[Pose([1.4, 3.15, 0.01], quaternion_from_euler(0, 0, 3.14))]).resolve().perform() + + LookAtAction(targets=[Pose(milk.get_position_as_list())]).resolve().perform() + + milk_desig = DetectAction(BelieveObject(types=[milk.obj_type])).resolve().perform() + TransportAction(milk_desig, [Arms.LEFT], [Pose([2.4, 3, 1.02])]).resolve().perform() ParkArmsAction([Arms.BOTH]).resolve().perform() From e3e9f95e7b8d7c637a75caecb9cb68e677e13af1 Mon Sep 17 00:00:00 2001 From: A_Bassiouny Date: Wed, 13 Nov 2024 16:32:17 +0100 Subject: [PATCH 63/91] [MultiverseFallschoolDemo] increase number of retries --- src/pycram/designators/motion_designator.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/pycram/designators/motion_designator.py b/src/pycram/designators/motion_designator.py index 33b97d52a..2f06d755c 100644 --- a/src/pycram/designators/motion_designator.py +++ b/src/pycram/designators/motion_designator.py @@ -21,7 +21,7 @@ from ..external_interfaces.robokudo import robokudo_found -def try_motion(motion, motion_designator_instance, exception, num_retries=3): +def try_motion(motion, motion_designator_instance, exception, num_retries=4): current_retry = 0 result = None while current_retry < num_retries: From a296104ba3145cc5444937ce5985ecf402a2c4f2 Mon Sep 17 00:00:00 2001 From: A_Bassiouny Date: Wed, 13 Nov 2024 19:02:10 +0100 Subject: [PATCH 64/91] [MultiverseFallschoolDemo] add mutex at every send and receive in multiverse. --- demos/pycram_multiverse_demo/fallschool_demo.py | 1 - src/pycram/designators/motion_designator.py | 2 +- src/pycram/worlds/multiverse_communication/clients.py | 6 +++++- 3 files changed, 6 insertions(+), 3 deletions(-) diff --git a/demos/pycram_multiverse_demo/fallschool_demo.py b/demos/pycram_multiverse_demo/fallschool_demo.py index 659d19f99..644582187 100644 --- a/demos/pycram_multiverse_demo/fallschool_demo.py +++ b/demos/pycram_multiverse_demo/fallschool_demo.py @@ -11,7 +11,6 @@ from pycram.designators.action_designator import ParkArmsAction, MoveTorsoAction, TransportAction, NavigateAction, \ LookAtAction, DetectAction from pycram.designators.object_designator import BelieveObject -from pycram.failures import PerceptionObjectNotFound, NavigationGoalNotReachedError from pycram.process_module import simulated_robot, with_simulated_robot, real_robot from pycram.ros_utils.robot_state_updater import WorldStateUpdater from pycram.world_concepts.world_object import Object diff --git a/src/pycram/designators/motion_designator.py b/src/pycram/designators/motion_designator.py index 2f06d755c..33b97d52a 100644 --- a/src/pycram/designators/motion_designator.py +++ b/src/pycram/designators/motion_designator.py @@ -21,7 +21,7 @@ from ..external_interfaces.robokudo import robokudo_found -def try_motion(motion, motion_designator_instance, exception, num_retries=4): +def try_motion(motion, motion_designator_instance, exception, num_retries=3): current_retry = 0 result = None while current_retry < num_retries: diff --git a/src/pycram/worlds/multiverse_communication/clients.py b/src/pycram/worlds/multiverse_communication/clients.py index 44316dfb0..cf9d5fff9 100644 --- a/src/pycram/worlds/multiverse_communication/clients.py +++ b/src/pycram/worlds/multiverse_communication/clients.py @@ -435,6 +435,7 @@ def send_multiple_body_data_to_server(self, body_data: Dict[str, Dict[Property, :param body_data: The data to be sent for multiple bodies. :return: The response from the server. """ + self.lock.acquire() send_meta_data = {body_name: list(map(str, data.keys())) for body_name, data in body_data.items()} response_meta_data = self.send_meta_data_and_get_response(send_meta_data) body_names = list(response_meta_data["send"].keys()) @@ -442,6 +443,7 @@ def send_multiple_body_data_to_server(self, body_data: Dict[str, Dict[Property, for value in data] self.send_data = [self.sim_time, *flattened_data] self.send_and_receive_data() + self.lock.release() return self.response_meta_data def send_meta_data_and_get_response(self, send_meta_data: Dict) -> Dict: @@ -451,9 +453,11 @@ def send_meta_data_and_get_response(self, send_meta_data: Dict) -> Dict: :param send_meta_data: The metadata to be sent. :return: The response from the server. """ + self.lock.acquire() self._reset_request_meta_data() self.request_meta_data["send"] = send_meta_data self.send_and_receive_meta_data() + self.lock.release() return self.response_meta_data def send_data_to_server(self, data: List, @@ -476,9 +480,9 @@ def send_data_to_server(self, data: List, if receive_meta_data: self.request_meta_data["receive"] = receive_meta_data self.send_and_receive_meta_data() - self.lock.release() self.send_data = data self.send_and_receive_data() + self.lock.release() return self.response_meta_data From e3a8521a67015d2bcea5d5c6731f7b75c65618d9 Mon Sep 17 00:00:00 2001 From: Abdelrhman Bassiouny Date: Thu, 14 Nov 2024 09:12:10 +0100 Subject: [PATCH 65/91] [Giskard] Move straight caretesian can allow gripper colllision. corrected adding of gripper groups --- src/pycram/process_modules/pr2_process_modules.py | 1 + 1 file changed, 1 insertion(+) diff --git a/src/pycram/process_modules/pr2_process_modules.py b/src/pycram/process_modules/pr2_process_modules.py index 3a1083617..1e04d2924 100644 --- a/src/pycram/process_modules/pr2_process_modules.py +++ b/src/pycram/process_modules/pr2_process_modules.py @@ -250,6 +250,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, use_monitor=World.current_world.conf.use_giskard_monitor) 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) From 6bcd33ec8c85d7c61df07c5138803ee268177b06 Mon Sep 17 00:00:00 2001 From: Abdelrhman Bassiouny Date: Fri, 15 Nov 2024 13:23:57 +0100 Subject: [PATCH 66/91] [MultiverseBulletWorld] added an option in bullet_world to use multiverse for real world simulation. --- .../pycram_multiverse_demo/fallschool_demo.py | 18 ++++++++++++++++-- src/pycram/worlds/bullet_world.py | 18 +++++++++++++++++- src/pycram/worlds/multiverse.py | 19 +++++++++---------- 3 files changed, 42 insertions(+), 13 deletions(-) diff --git a/demos/pycram_multiverse_demo/fallschool_demo.py b/demos/pycram_multiverse_demo/fallschool_demo.py index 644582187..ad66d203b 100644 --- a/demos/pycram_multiverse_demo/fallschool_demo.py +++ b/demos/pycram_multiverse_demo/fallschool_demo.py @@ -14,7 +14,9 @@ from pycram.process_module import simulated_robot, with_simulated_robot, real_robot from pycram.ros_utils.robot_state_updater import WorldStateUpdater from pycram.world_concepts.world_object import Object +from pycram.worlds.bullet_world import BulletWorld from pycram.worlds.multiverse import Multiverse +from pycram.ros_utils.viz_marker_publisher import VizMarkerPublisher rospy_logger = logging.getLogger('rosout') @@ -32,12 +34,22 @@ def move_and_detect(obj_type: ObjectType, pick_pose: Pose): return object_desig -world = Multiverse() +use_bullet_world = False + +if use_bullet_world: + world = BulletWorld(use_multiverse_for_real_world_simulation=True) + vis_publisher = VizMarkerPublisher() + milk_path = "milk.stl" +else: + world = Multiverse() + vis_publisher = None + milk_path = "milk.xml" + robot = Object('pr2', ObjectType.ROBOT, f'pr2.urdf', pose=Pose([1.3, 2.6, 0.01])) WorldStateUpdater(tf_topic="/tf", joint_state_topic="/real/pr2/joint_states", update_rate=timedelta(seconds=2), world=world) apartment = Object("apartment", ObjectType.ENVIRONMENT, f"apartment.urdf") -milk = Object("milk", ObjectType.MILK, f"milk.xml", pose=Pose([0.4, 2.6, 1.34], +milk = Object("milk", ObjectType.MILK, milk_path, pose=Pose([0.4, 2.6, 1.34], [1, 0, 0, 0]), color=Color(1, 0, 0, 1)) @@ -71,4 +83,6 @@ def move_and_detect(obj_type: ObjectType, pick_pose: Pose): ParkArmsAction([Arms.BOTH]).resolve().perform() +if vis_publisher is not None: + vis_publisher._stop_publishing() world.exit() diff --git a/src/pycram/worlds/bullet_world.py b/src/pycram/worlds/bullet_world.py index 3c875c708..aaaf23f4f 100755 --- a/src/pycram/worlds/bullet_world.py +++ b/src/pycram/worlds/bullet_world.py @@ -36,7 +36,8 @@ class is the main interface to the Bullet Physics Engine and should be used to s manipulate the Bullet World. """ - def __init__(self, mode: WorldMode = WorldMode.DIRECT, is_prospection_world: bool = False): + def __init__(self, mode: WorldMode = WorldMode.DIRECT, is_prospection_world: bool = False, + use_multiverse_for_real_world_simulation: bool = False): """ Creates a new simulation, the type decides of the simulation should be a rendered window or just run in the background. There can only be one rendered simulation. @@ -44,9 +45,13 @@ def __init__(self, mode: WorldMode = WorldMode.DIRECT, is_prospection_world: boo :param mode: Can either be "GUI" for rendered window or "DIRECT" for non-rendered. The default is "GUI" :param is_prospection_world: For internal usage, decides if this BulletWorld should be used as a shadow world. + :param use_multiverse_for_real_world_simulation: Whether to use the Multiverse for real world simulation. """ super().__init__(mode=mode, is_prospection_world=is_prospection_world) + if use_multiverse_for_real_world_simulation: + self.add_multiverse_resources() + # This disables file caching from PyBullet, since this would also cache # files that can not be loaded p.setPhysicsEngineParameter(enableFileCaching=0) @@ -62,6 +67,17 @@ def __init__(self, mode: WorldMode = WorldMode.DIRECT, is_prospection_world: boo _ = Object("floor", Floor, "plane.urdf", world=self) + def add_multiverse_resources(self): + """ + Add the Multiverse resources to the start of the data directories of the BulletWorld such they are searched + first for the resources because the pycram objects need to have the same description as the Multiverse objects. + """ + try: + from .multiverse import Multiverse + Multiverse.make_sure_multiverse_resources_are_added(self.conf.clear_cache_at_start) + except ImportError: + logwarn("Multiverse is not installed, please install it to use the Multiverse.") + def _init_world(self, mode: WorldMode): self._gui_thread: Gui = Gui(self, mode) self._gui_thread.start() diff --git a/src/pycram/worlds/multiverse.py b/src/pycram/worlds/multiverse.py index 34e96e626..783e1a5da 100644 --- a/src/pycram/worlds/multiverse.py +++ b/src/pycram/worlds/multiverse.py @@ -54,26 +54,24 @@ class Multiverse(World): Add the MJCF description extension to the extension to description type mapping for the objects. """ - def __init__(self, mode: Optional[WorldMode] = WorldMode.DIRECT, - is_prospection: Optional[bool] = False, + def __init__(self, is_prospection: Optional[bool] = False, clear_cache: bool = False): """ Initialize the Multiverse Socket and the PyCram World. - :param mode: The mode of the world (DIRECT or GUI). :param is_prospection: Whether the world is prospection or not. :param clear_cache: Whether to clear the cache or not. """ self.latest_save_id: Optional[int] = None self.saved_simulator_states: Dict = {} - self._make_sure_multiverse_resources_are_added(clear_cache=clear_cache) + self.make_sure_multiverse_resources_are_added(clear_cache=clear_cache) self.simulation = self.conf.prospection_world_prefix if is_prospection else "belief_state" self.client_manager = MultiverseClientManager(self.conf.simulation_wait_time_factor) self._init_clients(is_prospection=is_prospection) - World.__init__(self, mode, is_prospection) + World.__init__(self, WorldMode.DIRECT, is_prospection) self._init_constraint_and_object_id_name_map_collections() @@ -117,18 +115,19 @@ def _init_constraint_and_object_id_name_map_collections(self): def _init_world(self, mode: WorldMode): pass - def _make_sure_multiverse_resources_are_added(self, clear_cache: bool = False): + @classmethod + def make_sure_multiverse_resources_are_added(cls, clear_cache: bool = False) -> None: """ Add the multiverse resources to the pycram world resources, and change the data directory and cache manager. :param clear_cache: Whether to clear the cache or not. """ - if not self.added_multiverse_resources: + if not cls.added_multiverse_resources: if clear_cache: World.cache_manager.clear_cache() - World.add_resource_path(self.conf.resources_path, prepend=True) - World.change_cache_dir_path(self.conf.resources_path) - self.added_multiverse_resources = True + World.add_resource_path(cls.conf.resources_path, prepend=True) + World.change_cache_dir_path(cls.conf.resources_path) + cls.added_multiverse_resources = True def remove_multiverse_resources(self): """ From 119824c4799928095e434fa79ed5c71fcd90c538 Mon Sep 17 00:00:00 2001 From: Abdelrhman Bassiouny Date: Tue, 19 Nov 2024 18:01:37 +0100 Subject: [PATCH 67/91] [BugFixes] Fixed issues after merge with dev, ORM is still not working. --- .../pycram_multiverse_demo/fallschool_demo.py | 2 +- src/pycram/datastructures/world.py | 46 +++++++++++++++++-- src/pycram/designators/action_designator.py | 20 ++++++-- src/pycram/failures.py | 7 ++- .../robot_descriptions/pr2_description.py | 2 +- src/pycram/world_concepts/world_object.py | 2 - src/pycram/worlds/bullet_world.py | 8 ++-- src/pycram/worlds/multiverse.py | 8 ++-- .../test_location_designator.py | 3 +- test/test_goal_validator.py | 2 +- 10 files changed, 77 insertions(+), 23 deletions(-) diff --git a/demos/pycram_multiverse_demo/fallschool_demo.py b/demos/pycram_multiverse_demo/fallschool_demo.py index ad66d203b..7828b0a2f 100644 --- a/demos/pycram_multiverse_demo/fallschool_demo.py +++ b/demos/pycram_multiverse_demo/fallschool_demo.py @@ -50,7 +50,7 @@ def move_and_detect(obj_type: ObjectType, pick_pose: Pose): world=world) apartment = Object("apartment", ObjectType.ENVIRONMENT, f"apartment.urdf") milk = Object("milk", ObjectType.MILK, milk_path, pose=Pose([0.4, 2.6, 1.34], - [1, 0, 0, 0]), + [1, 0, 0, 0]), color=Color(1, 0, 0, 1)) # apartment.set_joint_position("fridge_door1_joint", 1.5707963267948966) diff --git a/src/pycram/datastructures/world.py b/src/pycram/datastructures/world.py index aba76fe8b..224052110 100644 --- a/src/pycram/datastructures/world.py +++ b/src/pycram/datastructures/world.py @@ -522,8 +522,16 @@ def remove_constraint(self, constraint_id) -> None: """ pass - @abstractmethod def get_joint_position(self, joint: Joint) -> float: + """ + Wrapper for :meth:`_get_joint_position` that return 0.0 for a joint if it is in the ignore joints list. + """ + if joint.object.obj_type == ObjectType.ROBOT and joint.name in self.robot_description.ignore_joints: + return 0.0 + return self._get_joint_position(joint) + + @abstractmethod + def _get_joint_position(self, joint: Joint) -> float: """ Get the position of a joint of an articulated object @@ -775,9 +783,18 @@ def get_closest_points_between_objects(self, object_a: Object, object_b: Object, """ raise NotImplementedError + def reset_joint_position(self, joint: Joint, joint_position: float) -> bool: + """ + Wrapper around :meth:`_reset_joint_position` that checks if the joint should be ignored. + """ + if joint.object.obj_type == ObjectType.ROBOT and self.robot_description.ignore_joints: + if joint.name in self.robot_description.ignore_joints: + return True + return self._reset_joint_position(joint, joint_position) + @validate_joint_position @abstractmethod - def reset_joint_position(self, joint: Joint, joint_position: float) -> bool: + def _reset_joint_position(self, joint: Joint, joint_position: float) -> bool: """ Reset the joint position instantly without physics simulation @@ -791,9 +808,20 @@ def reset_joint_position(self, joint: Joint, joint_position: float) -> bool: """ pass + def set_multiple_joint_positions(self, joint_positions: Dict[Joint, float]) -> bool: + """ + Wrapper around :meth:`_set_multiple_joint_positions` that checks if any of the joints should be ignored. + """ + filtered_joint_positions = copy(joint_positions) + for joint, position in joint_positions.items(): + if joint.object.obj_type == ObjectType.ROBOT and self.robot_description.ignore_joints: + if joint.name in self.robot_description.ignore_joints: + filtered_joint_positions.pop(joint) + return self._set_multiple_joint_positions(filtered_joint_positions) + @validate_multiple_joint_positions @abstractmethod - def set_multiple_joint_positions(self, joint_positions: Dict[Joint, float]) -> bool: + def _set_multiple_joint_positions(self, joint_positions: Dict[Joint, float]) -> bool: """ Set the positions of multiple joints of an articulated object. @@ -806,8 +834,18 @@ def set_multiple_joint_positions(self, joint_positions: Dict[Joint, float]) -> b """ pass - @abstractmethod def get_multiple_joint_positions(self, joints: List[Joint]) -> Dict[str, float]: + """ + Wrapper around :meth:`_get_multiple_joint_positions` that checks if any of the joints should be ignored. + """ + filtered_joints = [joint for joint in joints if joint.object.obj_type != ObjectType.ROBOT or + joint.name not in self.robot_description.ignore_joints] + joint_positions = self._get_multiple_joint_positions(filtered_joints) + joint_positions.update({joint.name: 0.0 for joint in joints if joint not in filtered_joints}) + return joint_positions + + @abstractmethod + def _get_multiple_joint_positions(self, joints: List[Joint]) -> Dict[str, float]: """ Get the positions of multiple joints of an articulated object. diff --git a/src/pycram/designators/action_designator.py b/src/pycram/designators/action_designator.py index bc08b826f..8c071ce11 100644 --- a/src/pycram/designators/action_designator.py +++ b/src/pycram/designators/action_designator.py @@ -20,7 +20,8 @@ VisibleProperty from ..knowledge.knowledge_engine import ReasoningInstance from ..local_transformer import LocalTransformer -from ..failures import ObjectUnfetchable, ReachabilityFailure, NavigationGoalNotReachedError, PerceptionObjectNotFound +from ..failures import ObjectUnfetchable, ReachabilityFailure, NavigationGoalNotReachedError, PerceptionObjectNotFound, \ + ObjectNotGraspedError from ..robot_description import RobotDescription from ..tasktree import with_tree from ..world_reasoning import contact @@ -305,7 +306,6 @@ def plan(self) -> None: oTg = object.local_transformer.transform_pose(adjusted_oTm, gripper_frame) oTg.pose.position.x -= self.prepose_distance # in x since this is how the gripper is oriented - oTg.pose.position.y -= 0.01 prepose = object.local_transformer.transform_pose(oTg, "map") # Perform the motion with the prepose and open gripper @@ -315,10 +315,22 @@ def plan(self) -> None: # Perform the motion with the adjusted pose -> actual grasp and close gripper + oTg = object.local_transformer.transform_pose(adjusted_oTm, gripper_frame) + adjusted_oTm = object.local_transformer.transform_pose(oTg, "map") World.current_world.add_vis_axis(adjusted_oTm) - MoveTCPMotion(adjusted_oTm, self.arm, allow_gripper_collision=True).perform() - adjusted_oTm.pose.position.z += 0.03 + MoveTCPMotion(adjusted_oTm, self.arm, allow_gripper_collision=True, + movement_type=MovementType.STRAIGHT_CARTESIAN).perform() + MoveGripperMotion(motion=GripperState.CLOSE, gripper=self.arm).perform() + + # Make sure the object is in contact with the gripper + in_contact, contact_links = contact(object, robot, return_links=True) + if not in_contact or not any([link.name in arm_chain.end_effector.links + for _, link in contact_links]): + # TODO: Would be better to check for contact with both fingers + # (maybe introduce left and right finger links in the end effector description) + raise ObjectNotGraspedError(object, self.arm) + tool_frame = RobotDescription.current_robot_description.get_arm_chain(self.arm).get_tool_frame() robot.attach(object, tool_frame) diff --git a/src/pycram/failures.py b/src/pycram/failures.py index ef878f07a..7513bad81 100644 --- a/src/pycram/failures.py +++ b/src/pycram/failures.py @@ -5,7 +5,7 @@ if TYPE_CHECKING: from .world_concepts.world_object import Object - from .datastructures.enums import JointType, MultiverseAPIName + from .datastructures.enums import JointType, MultiverseAPIName, Arms class PlanFailure(Exception): @@ -307,6 +307,11 @@ def __init__(self, *args, **kwargs): super().__init__(*args, **kwargs) +class ObjectNotGraspedError(Grasping): + def __init__(self, obj: Object, arm: Arms, *args, **kwargs): + super().__init__(f"object {obj.name} was not grasped by arm {arm}", *args, **kwargs) + + class Looking(Task): """""" diff --git a/src/pycram/robot_descriptions/pr2_description.py b/src/pycram/robot_descriptions/pr2_description.py index 1a6b534ba..d0ff58b44 100644 --- a/src/pycram/robot_descriptions/pr2_description.py +++ b/src/pycram/robot_descriptions/pr2_description.py @@ -14,7 +14,7 @@ "torso_lift_joint", filename, virtual_mobile_base_joints=VirtualMobileBaseJoints(), mjcf_path=mjcf_filename, - ignore_joints=['torso_lift_motor_screw_joint','r_gripper_motor_slider_joint', + ignore_joints=['torso_lift_motor_screw_joint', 'r_gripper_motor_slider_joint', 'r_gripper_motor_screw_joint', 'r_gripper_joint', 'l_gripper_motor_slider_joint', 'l_gripper_motor_screw_joint', 'l_gripper_joint']) diff --git a/src/pycram/world_concepts/world_object.py b/src/pycram/world_concepts/world_object.py index a54fe8852..37c64d12b 100644 --- a/src/pycram/world_concepts/world_object.py +++ b/src/pycram/world_concepts/world_object.py @@ -446,8 +446,6 @@ def joint_names(self) -> List[str]: The names of the joints as a list. """ joint_names = self.world.get_object_joint_names(self) - if self.obj_type == ObjectType.ROBOT and self.robot_description.ignore_joints: - joint_names = [joint for joint in joint_names if joint not in self.robot_description.ignore_joints] return joint_names def get_link(self, link_name: str) -> ObjectDescription.Link: diff --git a/src/pycram/worlds/bullet_world.py b/src/pycram/worlds/bullet_world.py index aaaf23f4f..dee26a369 100755 --- a/src/pycram/worlds/bullet_world.py +++ b/src/pycram/worlds/bullet_world.py @@ -152,7 +152,7 @@ def add_constraint(self, constraint: Constraint) -> int: def remove_constraint(self, constraint_id): p.removeConstraint(constraint_id, physicsClientId=self.id) - def get_joint_position(self, joint: ObjectDescription.Joint) -> float: + def _get_joint_position(self, joint: ObjectDescription.Joint) -> float: return p.getJointState(joint.object_id, joint.id, physicsClientId=self.id)[0] def get_object_joint_names(self, obj: Object) -> List[str]: @@ -233,17 +233,17 @@ def parse_points_list_to_args(self, point: List) -> Dict: "lateral_friction_2": LateralFriction(point[12], point[13])} @validate_multiple_joint_positions - def set_multiple_joint_positions(self, joint_positions: Dict[Joint, float]) -> bool: + def _set_multiple_joint_positions(self, joint_positions: Dict[Joint, float]) -> bool: for joint, joint_position in joint_positions.items(): self.reset_joint_position(joint, joint_position) return True @validate_joint_position - def reset_joint_position(self, joint: Joint, joint_position: float) -> bool: + def _reset_joint_position(self, joint: Joint, joint_position: float) -> bool: p.resetJointState(joint.object_id, joint.id, joint_position, physicsClientId=self.id) return True - def get_multiple_joint_positions(self, joints: List[Joint]) -> Dict[str, float]: + def _get_multiple_joint_positions(self, joints: List[Joint]) -> Dict[str, float]: return {joint.name: self.get_joint_position(joint) for joint in joints} @validate_multiple_object_poses diff --git a/src/pycram/worlds/multiverse.py b/src/pycram/worlds/multiverse.py index 783e1a5da..ba6c63e9a 100644 --- a/src/pycram/worlds/multiverse.py +++ b/src/pycram/worlds/multiverse.py @@ -285,7 +285,7 @@ def get_multiple_link_orientations(self, links: List[Link]) -> Dict[str, List[fl return self.reader.get_multiple_body_orientations([link.name for link in links]) @validate_joint_position - def reset_joint_position(self, joint: Joint, joint_position: float) -> bool: + def _reset_joint_position(self, joint: Joint, joint_position: float) -> bool: if not self.is_paused and self.conf.use_controller and self.joint_has_actuator(joint): self._reset_joint_position_using_controller(joint, joint_position) else: @@ -306,7 +306,7 @@ def _reset_joint_position_using_controller(self, joint: Joint, joint_position: f return True @validate_multiple_joint_positions - def set_multiple_joint_positions(self, joint_positions: Dict[Joint, float]) -> bool: + def _set_multiple_joint_positions(self, joint_positions: Dict[Joint, float]) -> bool: """ Set the positions of multiple joints in the simulator. Also check if the joint is controlled by an actuator and use the controller to set the joint position if the joint is controlled. @@ -360,13 +360,13 @@ def _set_multiple_joint_positions_using_controller(self, joint_positions: Dict[J self.joint_controller.send_multiple_body_data_to_server(controlled_joints_data) return True - def get_joint_position(self, joint: Joint) -> Optional[float]: + def _get_joint_position(self, joint: Joint) -> Optional[float]: joint_position_name = self.get_joint_position_name(joint) data = self.reader.get_body_data(joint.name, [joint_position_name]) if data is not None: return data[joint_position_name.value][0] - def get_multiple_joint_positions(self, joints: List[Joint]) -> Optional[Dict[str, float]]: + def _get_multiple_joint_positions(self, joints: List[Joint]) -> Optional[Dict[str, float]]: joint_names = [joint.name for joint in joints] data = self.reader.get_multiple_body_data(joint_names, {joint.name: [self.get_joint_position_name(joint)] for joint in joints}) diff --git a/test/test_designator/test_location_designator.py b/test/test_designator/test_location_designator.py index 9a30f3563..e92803ae8 100644 --- a/test/test_designator/test_location_designator.py +++ b/test/test_designator/test_location_designator.py @@ -27,7 +27,8 @@ def test_reachability_pose(self): def test_visibility(self): object_desig = ObjectDesignatorDescription(names=["milk"]) robot_desig = ObjectDesignatorDescription(names=[RobotDescription.current_robot_description.name]) - location_desig = CostmapLocation(object_desig.resolve(), visible_for=robot_desig.resolve()) + location_desig = CostmapLocation(object_desig.resolve(), visible_for=robot_desig.resolve(), + check_collision_at_start=False) location = location_desig.resolve() self.assertTrue(len(location.pose.position_as_list()) == 3) self.assertTrue(len(location.pose.orientation_as_list()) == 4) diff --git a/test/test_goal_validator.py b/test/test_goal_validator.py index ad390137b..8f29cf7b3 100644 --- a/test/test_goal_validator.py +++ b/test/test_goal_validator.py @@ -88,7 +88,7 @@ def test_single_revolute_joint_position_goal(self): self.validate_revolute_joint_position_goal(goal_validator, JointType.REVOLUTE) def validate_revolute_joint_position_goal(self, goal_validator, joint_type: Optional[JointType] = None): - goal_joint_position = -np.pi / 4 + goal_joint_position = -np.pi / 8 joint_name = 'l_shoulder_lift_joint' if joint_type is not None: goal_validator.register_goal(goal_joint_position, joint_type, joint_name) From e67e671df22af7c49f8a27f586ae4e721efb5452 Mon Sep 17 00:00:00 2001 From: Abdelrhman Bassiouny Date: Wed, 20 Nov 2024 09:39:03 +0100 Subject: [PATCH 68/91] [FixORM] added new paramters to orm classes to match actions/motions. --- test/test_task_tree.py | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/test/test_task_tree.py b/test/test_task_tree.py index d59b4dbf2..a3a1a1cd6 100644 --- a/test/test_task_tree.py +++ b/test/test_task_tree.py @@ -35,8 +35,8 @@ def test_tree_creation(self): # self.tearDownBulletWorld() tt = pycram.tasktree.task_tree - self.assertEqual(15, len(tt.root)) - self.assertEqual(10, len(tt.root.leaves)) + self.assertEqual(16, len(tt.root)) + self.assertEqual(11, len(tt.root.leaves)) # check that all nodes succeeded for node in anytree.PreOrderIter(tt.root): @@ -74,8 +74,8 @@ def test_simulated_tree(self): self.plan() tt = pycram.tasktree.task_tree - self.assertEqual(15, len(tt.root)) - self.assertEqual(10, len(tt.root.leaves)) + self.assertEqual(16, len(tt.root)) + self.assertEqual(11, len(tt.root.leaves)) self.assertEqual(len(pycram.tasktree.task_tree), 1) From 40d53cd06c3fc32f9f5853ce21552f910132b5f1 Mon Sep 17 00:00:00 2001 From: Abdelrhman Bassiouny Date: Tue, 26 Nov 2024 18:13:26 +0100 Subject: [PATCH 69/91] [CheckforPickedObject] a function to check if an object is currently picked by robot. --- src/pycram/designators/location_designator.py | 10 +++++---- src/pycram/world_reasoning.py | 21 +++++++++++++++++++ 2 files changed, 27 insertions(+), 4 deletions(-) diff --git a/src/pycram/designators/location_designator.py b/src/pycram/designators/location_designator.py index 65e88a65d..9c8e22353 100644 --- a/src/pycram/designators/location_designator.py +++ b/src/pycram/designators/location_designator.py @@ -12,8 +12,8 @@ from ..pose_generator_and_validator import PoseGenerator, visibility_validator, reachability_validator from ..robot_description import RobotDescription from ..ros.logging import logdebug -from ..world_concepts.world_object import Object -from ..world_reasoning import link_pose_for_joint_config, contact +from ..world_concepts.world_object import Object, Link +from ..world_reasoning import link_pose_for_joint_config, contact, is_a_picked_object class Location(LocationDesignatorDescription): @@ -160,7 +160,8 @@ def check_for_collision(self, robot: Object, pose: Pose) -> bool: for obj in robot.world.objects: if obj.name in ([robot.name, floor.name] + ignore): continue - if contact(robot, obj): + in_contact, contact_links = contact(robot, obj, return_links=True) + if in_contact and not is_a_picked_object(robot, obj, [links[0] for links in contact_links]): logdebug(f"Robot is in contact with {obj.name} in prospection: {obj.world.is_prospection_world}" f"at position {pose.position_as_list()} and z_angle {pose.z_angle}") return True @@ -206,7 +207,8 @@ def __iter__(self): test_robot = None if self.visible_for or self.reachable_for: robot_object = self.visible_for.world_object if self.visible_for else self.reachable_for.world_object - test_robot = World.current_world.get_prospection_object_for_object(robot_object) + # test_robot = World.current_world.get_prospection_object_for_object(robot_object) + test_robot = robot_object self.ignore_collision_with = [World.current_world.get_prospection_object_for_object(o) for o in self.ignore_collision_with] with UseProspectionWorld(): diff --git a/src/pycram/world_reasoning.py b/src/pycram/world_reasoning.py index 214aefb68..670333d9c 100644 --- a/src/pycram/world_reasoning.py +++ b/src/pycram/world_reasoning.py @@ -62,6 +62,27 @@ def contact( return objects_are_in_contact +def is_a_picked_object(robot: Object, obj: Object, robot_contact_links: List[Link]) -> bool: + """ + Check if the object is picked by the robot. + + :param robot: The robot object + :param obj: The object to check if it is picked + :param robot_contact_links: The links of the robot that are in contact with the object + :return: True if the object is picked by the robot, False otherwise + """ + picked_object = False + if obj in robot.attachments: + arm_chains = RobotDescription.current_robot_description.get_manipulator_chains() + for chain in arm_chains: + gripper_links = chain.end_effector.links + if (robot.attachments[obj].parent_link.name in gripper_links + and all(link.name in gripper_links for link in robot_contact_links)): + picked_object = True + continue + return picked_object + + def get_visible_objects( camera_pose: Pose, front_facing_axis: Optional[List[float]] = None, From 2a2676aff2cfab3fb031bddee9bb44d3361493cd Mon Sep 17 00:00:00 2001 From: Abdelrhman Bassiouny Date: Wed, 27 Nov 2024 16:25:25 +0100 Subject: [PATCH 70/91] [JupyterTests] (WIP) cram_plan_tutorial has problems. --- src/pycram/datastructures/dataclasses.py | 10 +++++ src/pycram/datastructures/enums.py | 1 + src/pycram/datastructures/world.py | 13 +++++-- src/pycram/designators/location_designator.py | 4 +- src/pycram/pose_generator_and_validator.py | 2 +- src/pycram/worlds/multiverse.py | 3 +- .../multiverse_communication/clients.py | 37 ++++++++++++++++--- 7 files changed, 57 insertions(+), 13 deletions(-) diff --git a/src/pycram/datastructures/dataclasses.py b/src/pycram/datastructures/dataclasses.py index f31dbcbb6..87288c6b3 100644 --- a/src/pycram/datastructures/dataclasses.py +++ b/src/pycram/datastructures/dataclasses.py @@ -1,5 +1,6 @@ from __future__ import annotations +import os from abc import ABC, abstractmethod from copy import deepcopy, copy from dataclasses import dataclass, fields @@ -850,6 +851,15 @@ def get_axes(self) -> Dict[str, Point]: return {getattr(self, field.name).name: getattr(self, field.name).axes for field in fields(self)} +@dataclass +class MultiverseBoundingBox: + """ + Dataclass for storing the bounding box of a body in the Multiverse simulation. + """ + min_point: List[float] + max_point: List[float] + + @dataclass class MultiverseMetaData: """Meta data for the Multiverse Client, the simulation_name should be non-empty and unique for each simulation""" diff --git a/src/pycram/datastructures/enums.py b/src/pycram/datastructures/enums.py index 6eb516405..aced79bc1 100644 --- a/src/pycram/datastructures/enums.py +++ b/src/pycram/datastructures/enums.py @@ -248,6 +248,7 @@ class MultiverseAPIName(Enum): GET_CONTACT_BODIES = "get_contact_bodies" GET_CONTACT_BODIES_AND_POINTS = "get_contact_bodies_and_points" GET_CONSTRAINT_EFFORT = "get_constraint_effort" + GET_BOUNDING_BOX = "get_bounding_box" ATTACH = "attach" DETACH = "detach" GET_RAYS = "get_rays" diff --git a/src/pycram/datastructures/world.py b/src/pycram/datastructures/world.py index 224052110..cd6a5b5a2 100644 --- a/src/pycram/datastructures/world.py +++ b/src/pycram/datastructures/world.py @@ -80,7 +80,8 @@ class World(StateEntity, ABC): The ontology of this world. """ - def __init__(self, mode: WorldMode, is_prospection_world: bool = False, clear_cache: bool = False): + def __init__(self, mode: WorldMode = WorldMode.DIRECT, is_prospection_world: bool = False, + clear_cache: bool = False): """ Create a new simulation, the mode decides if the simulation should be a rendered window or just run in the background. There can only be one rendered simulation. @@ -301,8 +302,7 @@ def _init_prospection_world(self): if self.is_prospection_world: # then no need to add another prospection world self.prospection_world = None else: - self.prospection_world: World = self.__class__(WorldMode.DIRECT, - True) + self.prospection_world: World = self.__class__(is_prospection_world=True) def _sync_prospection_world(self): """ @@ -1659,6 +1659,13 @@ def _simulator_object_remover(self, remover_func: Callable, *args, **kwargs) -> remover_func(*args, **kwargs) self.update_simulator_state_id_in_original_state() + def update_original_state(self) -> int: + """ + Update the original state of the world. + :return: The id of the updated original state. + """ + return self.save_state(self.original_state_id, use_same_id=True) + def update_simulator_state_id_in_original_state(self, use_same_id: bool = False) -> None: """ Update the simulator state id in the original state if use_physics_simulator_state is True in the configuration. diff --git a/src/pycram/designators/location_designator.py b/src/pycram/designators/location_designator.py index 9c8e22353..02ce91037 100644 --- a/src/pycram/designators/location_designator.py +++ b/src/pycram/designators/location_designator.py @@ -207,8 +207,8 @@ def __iter__(self): test_robot = None if self.visible_for or self.reachable_for: robot_object = self.visible_for.world_object if self.visible_for else self.reachable_for.world_object - # test_robot = World.current_world.get_prospection_object_for_object(robot_object) - test_robot = robot_object + test_robot = World.current_world.get_prospection_object_for_object(robot_object) + # test_robot = robot_object self.ignore_collision_with = [World.current_world.get_prospection_object_for_object(o) for o in self.ignore_collision_with] with UseProspectionWorld(): diff --git a/src/pycram/pose_generator_and_validator.py b/src/pycram/pose_generator_and_validator.py index b3e09ef7b..6c7e31340 100644 --- a/src/pycram/pose_generator_and_validator.py +++ b/src/pycram/pose_generator_and_validator.py @@ -57,7 +57,7 @@ def __iter__(self) -> Iterable: """ # Determines how many positions should be sampled from the costmap - if self.number_of_samples == -1: + if self.number_of_samples == -1 or self.number_of_samples > self.costmap.map.flatten().shape[0]: self.number_of_samples = self.costmap.map.flatten().shape[0] indices = np.argpartition(self.costmap.map.flatten(), -self.number_of_samples)[-self.number_of_samples:] indices = np.dstack(np.unravel_index(indices, self.costmap.map.shape)).reshape(self.number_of_samples, 2) diff --git a/src/pycram/worlds/multiverse.py b/src/pycram/worlds/multiverse.py index ba6c63e9a..6e5230ca4 100644 --- a/src/pycram/worlds/multiverse.py +++ b/src/pycram/worlds/multiverse.py @@ -11,8 +11,7 @@ from .multiverse_communication.client_manager import MultiverseClientManager from .multiverse_communication.clients import MultiverseController, MultiverseReader, MultiverseWriter, MultiverseAPI from ..config.multiverse_conf import MultiverseConfig -from ..datastructures.dataclasses import AxisAlignedBoundingBox, Color, ContactPointsList, ContactPoint, \ - MultiverseContactPoint, MeshVisualShape +from ..datastructures.dataclasses import Color, ContactPointsList, ContactPoint from ..datastructures.enums import WorldMode, JointType, ObjectType, MultiverseBodyProperty, MultiverseJointPosition, \ MultiverseJointCMD from ..datastructures.pose import Pose diff --git a/src/pycram/worlds/multiverse_communication/clients.py b/src/pycram/worlds/multiverse_communication/clients.py index cf9d5fff9..5d1aa0742 100644 --- a/src/pycram/worlds/multiverse_communication/clients.py +++ b/src/pycram/worlds/multiverse_communication/clients.py @@ -10,7 +10,7 @@ from .socket import MultiverseSocket, MultiverseMetaData from ...config.multiverse_conf import MultiverseConfig as Conf -from ...datastructures.dataclasses import RayResult, MultiverseContactPoint +from ...datastructures.dataclasses import RayResult, MultiverseContactPoint, MultiverseBoundingBox from ...datastructures.enums import (MultiverseAPIName as API, MultiverseBodyProperty as BodyProperty, MultiverseProperty as Property) from ...datastructures.pose import Pose @@ -435,16 +435,17 @@ def send_multiple_body_data_to_server(self, body_data: Dict[str, Dict[Property, :param body_data: The data to be sent for multiple bodies. :return: The response from the server. """ - self.lock.acquire() send_meta_data = {body_name: list(map(str, data.keys())) for body_name, data in body_data.items()} response_meta_data = self.send_meta_data_and_get_response(send_meta_data) body_names = list(response_meta_data["send"].keys()) flattened_data = [value for body_name in body_names for data in body_data[body_name].values() for value in data] + self.lock.acquire() self.send_data = [self.sim_time, *flattened_data] self.send_and_receive_data() + response_meta_data = self.response_meta_data self.lock.release() - return self.response_meta_data + return response_meta_data def send_meta_data_and_get_response(self, send_meta_data: Dict) -> Dict: """ @@ -457,8 +458,9 @@ def send_meta_data_and_get_response(self, send_meta_data: Dict) -> Dict: self._reset_request_meta_data() self.request_meta_data["send"] = send_meta_data self.send_and_receive_meta_data() + response_meta_data = self.response_meta_data self.lock.release() - return self.response_meta_data + return response_meta_data def send_data_to_server(self, data: List, send_meta_data: Optional[Dict] = None, @@ -482,8 +484,9 @@ def send_data_to_server(self, data: List, self.send_and_receive_meta_data() self.send_data = data self.send_and_receive_data() + response_meta_data = self.response_meta_data self.lock.release() - return self.response_meta_data + return response_meta_data class MultiverseController(MultiverseWriter): @@ -532,6 +535,30 @@ def __init__(self, name: str, port: int, simulation: str, is_prospection_world: self.simulation = simulation self.wait: bool = False # Whether to wait after sending the API request. + def get_body_bounding_box(self, body_name: str, + with_children: bool = False) -> Union[MultiverseBoundingBox, List[MultiverseBoundingBox]]: + """ + Get the body bounding box from the multiverse server, they are with respect to the body's frame. + """ + bounding_boxes_data = self._get_bounding_box(body_name, with_children) + multiverse_bounding_boxes = [] + for bounding_box in bounding_boxes_data: + min_point, max_point = bounding_box[:3], bounding_box[3:] + multiverse_bounding_boxes.append(MultiverseBoundingBox(min_point, max_point)) + if with_children: + return multiverse_bounding_boxes + return multiverse_bounding_boxes[0] + + def _get_bounding_box(self, body_name: str, with_children: bool = False) -> List[List[float]]: + """ + Get the body bounding box from the multiverse server. + """ + params = [body_name] + if with_children: + params.append("with_children") + response = self._request_single_api_callback(API.GET_BOUNDING_BOX, *params)[0] + return [list(map(float, bounding_box.split())) for bounding_box in response] + def save(self, save_name: str, save_directory: Optional[str] = None) -> str: """ Save the current state of the simulation. From 95ab050711c063893ffacecfee7b70af2e3d7027 Mon Sep 17 00:00:00 2001 From: A_Bassiouny Date: Wed, 27 Nov 2024 23:08:38 +0100 Subject: [PATCH 71/91] [JupyterTests] Fixed cram_plan_tutorial.md. --- examples/cram_plan_tutorial.md | 206 +++++++----------- examples/location_designator.md | 2 +- src/pycram/costmaps.py | 28 ++- src/pycram/designators/location_designator.py | 23 +- src/pycram/world_concepts/world_object.py | 10 +- 5 files changed, 117 insertions(+), 152 deletions(-) diff --git a/examples/cram_plan_tutorial.md b/examples/cram_plan_tutorial.md index b09c6a8ad..542552075 100644 --- a/examples/cram_plan_tutorial.md +++ b/examples/cram_plan_tutorial.md @@ -36,18 +36,26 @@ import pycrap np.random.seed(4) -world = BulletWorld() +use_multiverse = False +viz_marker_publisher = None +if use_multiverse: + try: + from pycram.worlds.multiverse import Multiverse + world = Multiverse() + except ImportError: + Multiverse = None + raise ImportError("Multiverse not available") +else: + world = BulletWorld() + from pycram.ros_utils.viz_marker_publisher import VizMarkerPublisher + viz_marker_publisher = VizMarkerPublisher() + robot = Object("pr2", pycrap.Robot, "pr2.urdf") robot_desig = ObjectDesignatorDescription(names=['pr2']).resolve() -apartment = Object("apartment", pycrap.Apartment, "apartment.urdf", pose=Pose([-1.5, -2.5, 0])) +apartment = Object("apartment", pycrap.Apartment, "apartment.urdf") apartment_desig = ObjectDesignatorDescription(names=['apartment']).resolve() -table_top = apartment.get_link_position("cooktop") -# milk = Object("milk", "milk", "milk.stl", position=[table_top[0]-0.15, table_top[1], table_top[2]]) -# milk.set_position(position=milk.get_position(), base=True) -# cereal = Object("cereal", "cereal", "breakfast_cereal.stl", position=table_top) -# cereal.set_position(position=[table_top[0]-0.1, table_top[1] + 0.5, table_top[2]], base=True) -# milk_desig = ObjectDesignator(ObjectDesignatorDescription(name="milk", type="milk")) -# cereal_desig = ObjectDesignator(ObjectDesignatorDescription(name="cereal", type="cereal")) +table_top_name = "stove" if use_multiverse else "cooktop" +table_top = apartment.get_link_position(table_top_name) ``` ```python @@ -68,11 +76,8 @@ def get_n_random_positions(pose_list, n=4, dist=0.5, random=True): for i in range(len(positions) - 1): pos_idx = np.random.choice(all_indices) if random else all_indices[i] diff = np.absolute(np.linalg.norm(n_positions - positions[pos_idx], axis=1)) - # print(diff) min_diff = np.min(diff) - # print(min_diff) if min_diff >= dist: - # print("found") n_positions[found_count, :] = positions[pos_idx] found_indices.append(pos_idx) found_count += 1 @@ -80,31 +85,22 @@ def get_n_random_positions(pose_list, n=4, dist=0.5, random=True): if found_count == n: break found_poses = [pose_list[i] for i in found_indices] - # found_positions = [positions[i] for i in found_indices] - # for i in range(len(found_positions)): - # print(found_poses[i][0]) - # print(found_positions[i]) - # assert np.allclose(found_positions[i],found_poses[i][0]) - # for i in range(len(found_poses)): - # for j in range(i+1,len(found_poses)): - # pos1 = np.array(found_poses[i][0]) - # pos2 = np.array(found_poses[j][0]) - # diff = np.absolute(np.linalg.norm(pos1 - pos2)) - # print(diff) - # assert diff >= dist return found_poses - - - ``` ```python +from tf.transformations import quaternion_from_euler import pycrap from pycram.costmaps import SemanticCostmap from pycram.pose_generator_and_validator import PoseGenerator -scm = SemanticCostmap(apartment, "island_countertop") -poses_list = list(PoseGenerator(scm, number_of_samples=-1)) +counter_name = "counter_stove_sink" if use_multiverse else "island_countertop" +counter_link = apartment.get_link(counter_name) +counter_bounds = counter_link.get_axis_aligned_bounding_box() +scm = SemanticCostmap(apartment, counter_name) +# take only 6 cms from edges as viable region +edges_cm = scm.get_edges_map(0.06, horizontal_only=True) +poses_list = list(PoseGenerator(edges_cm, number_of_samples=-1)) poses_list.sort(reverse=True, key=lambda x: np.linalg.norm(x.position_as_list())) object_poses = get_n_random_positions(poses_list) object_names = ["bowl", "breakfast_cereal", "spoon"] @@ -112,11 +108,18 @@ object_types = [pycrap.Bowl, pycrap.Cereal, pycrap.Spoon] objects = {} object_desig = {} for obj_name, obj_type, obj_pose in zip(object_names, object_types, object_poses): + if obj_pose.position.x > counter_link.position.x: + z_angle = np.pi + else: + z_angle = 0 + orientation = quaternion_from_euler(0, 0, z_angle).tolist() objects[obj_name] = Object(obj_name, obj_type, obj_name + ".stl", - pose=Pose([obj_pose.position.x, obj_pose.position.y, table_top.z])) + pose=Pose([obj_pose.position.x, obj_pose.position.y, table_top.z], + orientation)) objects[obj_name].move_base_to_origin_pose() objects[obj_name].original_pose = objects[obj_name].pose object_desig[obj_name] = ObjectDesignatorDescription(names=[obj_name], types=[obj_type]).resolve() +world.update_original_state() ``` If You want to visualize all apartment frames @@ -138,12 +141,13 @@ Finally, we create a plan where the robot parks his arms, walks to the kitchen c execute the plan. ```python +import pycrap from pycram.external_interfaces.ik import IKError from pycram.datastructures.enums import Grasp @pycram.tasktree.with_tree -def plan(obj, obj_desig, torso=0.2, place="countertop"): +def plan(obj_desig: ObjectDesignatorDescription.Object, torso=0.2, place=counter_name): world.reset_world() with simulated_robot: ParkArmsActionPerformable(Arms.BOTH).perform() @@ -156,26 +160,46 @@ def plan(obj, obj_desig, torso=0.2, place="countertop"): ParkArmsActionPerformable(Arms.BOTH).perform() good_torsos.append(torso) picked_up_arm = pose.reachable_arms[0] - PickUpActionPerformable(object_designator=obj_desig, arm=pose.reachable_arms[0], grasp=Grasp.FRONT, + grasp = Grasp.TOP if issubclass(obj_desig.world_object.obj_type, pycrap.Spoon) else Grasp.FRONT + PickUpActionPerformable(object_designator=obj_desig, arm=pose.reachable_arms[0], grasp=grasp, prepose_distance=0.03).perform() ParkArmsActionPerformable(Arms.BOTH).perform() - scm = SemanticCostmapLocation(place, apartment_desig, obj_desig) - scm = iter(scm) - pose_island = None - for i in range(np.random.randint(5, 15)): - pose_island = next(scm) - print(pose_island) - - - place_location = CostmapLocation(target=pose_island.pose, reachable_for=robot_desig, - reachable_arm=picked_up_arm) - pose = place_location.resolve() - - NavigateActionPerformable(pose.pose, True).perform() - - PlaceActionPerformable(object_designator=obj_desig, target_location=pose_island.pose, + scm = SemanticCostmapLocation(place, apartment_desig, obj_desig, horizontal_edges_only=True, edge_size_in_meters=0.08) + scm_iter = iter(scm) + n_retries = 0 + found = False + while not found: + try: + print(f"Trying {n_retries} to find a place") + if n_retries == 0: + pose_island = next(scm_iter) + cost_map_size = len(np.where(scm.sem_costmap.map > 0)[0]) + print(f"cost_map_size: {cost_map_size}") + else: + for _ in range(np.random.randint(100, 110)): + pose_island = next(scm_iter) + print("found pose_island") + if pose_island.pose.position.x > counter_link.position.x: + z_angle = np.pi + else: + z_angle = 0 + orientation = quaternion_from_euler(0, 0, z_angle).tolist() + pose_island.pose = Pose(pose_island.pose.position_as_list(), orientation) + pose_island.pose.position.z += 0.07 + print(pose_island.pose.position) + place_location = CostmapLocation(target=pose_island.pose, reachable_for=robot_desig, reachable_arm=picked_up_arm) + pose = place_location.resolve() + NavigateActionPerformable(pose.pose, True).perform() + PlaceActionPerformable(object_designator=obj_desig, target_location=pose_island.pose, arm=picked_up_arm).perform() + found = True + except (StopIteration, IKError) as e: + print("Retrying") + print(e) + n_retries += 1 + if n_retries > 3: + raise StopIteration("No place found") ParkArmsActionPerformable(Arms.BOTH).perform() @@ -183,15 +207,16 @@ def plan(obj, obj_desig, torso=0.2, place="countertop"): good_torsos = [] for obj_name in object_names: done = False - torso = 0.25 if len(good_torsos) == 0 else good_torsos[-1] + torso = 0.3 if len(good_torsos) == 0 else good_torsos[-1] while not done: try: - plan(objects[obj_name], object_desig[obj_name], torso=torso, place="island_countertop") + plan(object_desig[obj_name], torso=torso, place=counter_name) done = True + print(f"Object {obj_name} placed") objects[obj_name].original_pose = objects[obj_name].pose + world.update_original_state() except (StopIteration, IKError) as e: print(type(e)) - print(e) print("no solution") torso += 0.05 if torso > 0.3: @@ -203,84 +228,7 @@ Now we get the task tree from its module and render it. Rendering can be done wi anytree package. We will use ascii rendering here for ease of displaying. ```python -tt = pycram.tasktree.task_tree -print(anytree.RenderTree(tt, style=anytree.render.AsciiStyle())) -``` - -As we see every task in the plan got recorded correctly. It is noticeable that the tree begins with a NoOperation node. -This is done because several, not connected, plans that get executed after each other should still appear in the task -tree. Hence, a NoOperation node is the root of any tree. If we re-execute the plan we would see them appear in the same -tree even though they are not connected. - -```python -world.reset_world() -plan(objects["bowl"], object_desig["bowl"], torso=0.25) -print(anytree.RenderTree(tt, style=anytree.render.AsciiStyle())) -``` - -Projecting a plan in a new environment with its own task tree that only exists while the projected plan is running can -be done with the ``with`` keyword. When this is done, both the bullet world and task tree are saved and new, freshly -reset objects are available. At the end of a with block the old state is restored. The root for such things is then -called ``simulation()``. - -```python -with pycram.tasktree.SimulatedTaskTree() as stt: - print(anytree.RenderTree(pycram.tasktree.task_tree, style=anytree.render.AsciiStyle())) -print(anytree.RenderTree(pycram.tasktree.task_tree, style=anytree.render.AsciiStyle())) -``` - -Task tree can be manipulated with ordinary anytree manipulation. If we for example want to discard the second plan, we -would write: - -```python -tt.root.children = (tt.root.children[0],) -print(anytree.RenderTree(tt, style=anytree.render.AsciiStyle())) -``` - -We can now re-execute this (modified) plan by executing the leaf in pre-ordering iteration using the anytree -functionality. This will not append the re-execution to the task tree. - -```python -world.reset_world() -with simulated_robot: - [node.action.perform() for node in tt.root.leaves] -print(anytree.RenderTree(pycram.tasktree.task_tree, style=anytree.render.AsciiStyle())) -``` - -Nodes in the task tree contain additional information about the status and time of a task. - -```python -print(pycram.tasktree.task_tree.children[0]) -``` - -The task tree can also be reset to an empty one by invoking: - -```python -pycram.tasktree.task_tree.reset_tree() -print(anytree.RenderTree(pycram.tasktree.task_tree, style=anytree.render.AsciiStyle())) -``` - -If a plan fails using the PlanFailure exception, the plan will stop and raise the respective error. -Additionally, the error will be logged in the node of the TaskTree. First let's create a simple failing plan and execute it. - -```python -@pycram.tasktree.with_tree -def failing_plan(): - raise pycram.failures.PlanFailure("Oopsie!") - -try: - failing_plan() -except pycram.failures.PlanFailure as e: - print(e) -``` - -We can now investigate the nodes of the tree, and we will see that the tree indeed contains a failed task. - -```python -print(anytree.RenderTree(pycram.tasktree.task_tree, style=anytree.render.AsciiStyle())) -print(pycram.tasktree.task_tree.children[0]) -``` - -```python +if viz_marker_publisher is not None: + viz_marker_publisher._stop_publishing() world.exit() ``` diff --git a/examples/location_designator.md b/examples/location_designator.md index 58a0dac45..02a2a608a 100644 --- a/examples/location_designator.md +++ b/examples/location_designator.md @@ -139,7 +139,7 @@ from pycram.designators.object_designator import BelieveObject kitchen_desig = BelieveObject(names=["apartment"]).resolve() milk_desig = BelieveObject(names=["milk"]).resolve() -location_description = SemanticCostmapLocation(urdf_link_name="island_countertop", +location_description = SemanticCostmapLocation(link_name="island_countertop", part_of=kitchen_desig, for_object=milk_desig) diff --git a/src/pycram/costmaps.py b/src/pycram/costmaps.py index 724fbe267..7ab4c48c3 100644 --- a/src/pycram/costmaps.py +++ b/src/pycram/costmaps.py @@ -747,21 +747,21 @@ class SemanticCostmap(Costmap): table surface. """ - def __init__(self, object, urdf_link_name, size=100, resolution=0.02, world=None): + def __init__(self, object, link_name, resolution=0.02, world=None): """ Creates a semantic costmap for the given parameter. The semantic costmap will be on top of the link of the given Object. :param object: The object of which the link is a part - :param urdf_link_name: The link name, as stated in the URDF - :param resolution: Resolution of the final costmap + :param link_name: The link name, as stated in the description of the object + :param resolution: Resolution of the final costmap (how much meters one pixel represents) :param world: The World from which the costmap should be created """ self.world: World = world if world else World.current_world self.object: Object = object - self.link: Link = object.get_link(urdf_link_name) + self.link: Link = object.get_link(link_name) self.resolution: float = resolution - self.origin: Pose = object.get_link_pose(urdf_link_name) + self.origin: Pose = object.get_link_pose(link_name) self.height: int = 0 self.width: int = 0 self.map: np.ndarray = [] @@ -769,6 +769,16 @@ def __init__(self, object, urdf_link_name, size=100, resolution=0.02, world=None Costmap.__init__(self, resolution, self.height, self.width, self.origin, self.map) + def get_edges_map(self, margin_in_meters: float, horizontal_only: bool = False) -> Costmap: + mask = np.zeros(self.map.shape) + edge_tolerance = int(margin_in_meters / self.resolution) + mask[:edge_tolerance] = 1 + mask[-edge_tolerance:] = 1 + if not horizontal_only: + mask[:, :edge_tolerance] = 1 + mask[:, -edge_tolerance:] = 1 + return Costmap(self.resolution, self.height, self.width, self.origin, mask) + def generate_map(self) -> None: """ Generates the semantic costmap according to the provided parameters. To do this the axis aligned bounding box (AABB) @@ -786,13 +796,7 @@ def get_aabb_for_link(self) -> AxisAlignedBoundingBox: the AABB as close to the actual object as possible, the Object will be rotated such that the link will be in the identity orientation. """ - prospection_object = World.current_world.get_prospection_object_for_object(self.object) - with UseProspectionWorld(): - prospection_object.set_orientation(Pose(orientation=[0, 0, 0, 1])) - link_pose_trans = self.link.transform - inverse_trans = link_pose_trans.invert() - prospection_object.set_orientation(inverse_trans.to_pose()) - return self.link.get_axis_aligned_bounding_box() + return self.link.get_axis_aligned_bounding_box(False) class AlgebraicSemanticCostmap(SemanticCostmap): diff --git a/src/pycram/designators/location_designator.py b/src/pycram/designators/location_designator.py index 02ce91037..3eb499a1a 100644 --- a/src/pycram/designators/location_designator.py +++ b/src/pycram/designators/location_designator.py @@ -341,20 +341,28 @@ class SemanticCostmapLocation(LocationDesignatorDescription): class Location(LocationDesignatorDescription.Location): pass - def __init__(self, urdf_link_name, part_of, for_object=None): + def __init__(self, link_name, part_of, for_object=None, edges_only: bool = False, + horizontal_edges_only: bool = False, edge_size_in_meters: float = 0.06): """ - Creates a distribution over a urdf link to sample poses which are on this link. Can be used, for example, to find + Creates a distribution over a link to sample poses which are on this link. Can be used, for example, to find poses that are on a table. Optionally an object can be given for which poses should be calculated, in that case the poses are calculated such that the bottom of the object is on the link. - :param urdf_link_name: Name of the urdf link for which a distribution should be calculated + :param link_name: Name of the link for which a distribution should be calculated :param part_of: Object of which the urdf link is a part :param for_object: Optional object that should be placed at the found location + :param edges_only: If True, only the edges of the link are considered + :param horizontal_edges_only: If True, only the horizontal edges of the link are considered + :param edge_size_in_meters: Size of the edges in meters. """ super().__init__() - self.urdf_link_name: str = urdf_link_name + self.link_name: str = link_name self.part_of: ObjectDesignatorDescription.Object = part_of self.for_object: Optional[ObjectDesignatorDescription.Object] = for_object + self.edges_only: bool = edges_only + self.horizontal_edges_only: bool = horizontal_edges_only + self.edge_size_in_meters: float = edge_size_in_meters + self.sem_costmap: Optional[SemanticCostmap] = None def ground(self) -> Location: """ @@ -372,11 +380,14 @@ def __iter__(self): :yield: An instance of SemanticCostmapLocation.Location with the found valid position of the Costmap. """ - sem_costmap = SemanticCostmap(self.part_of.world_object, self.urdf_link_name) + self.sem_costmap = SemanticCostmap(self.part_of.world_object, self.link_name) + if self.edges_only or self.horizontal_edges_only: + self.sem_costmap = self.sem_costmap.get_edges_map(self.edge_size_in_meters, + horizontal_only=self.horizontal_edges_only) height_offset = 0 if self.for_object: min_p, max_p = self.for_object.world_object.get_axis_aligned_bounding_box().get_min_max_points() height_offset = (max_p.z - min_p.z) / 2 - for maybe_pose in PoseGenerator(sem_costmap): + for maybe_pose in PoseGenerator(self.sem_costmap): maybe_pose.position.z += height_offset yield self.Location(maybe_pose) diff --git a/src/pycram/world_concepts/world_object.py b/src/pycram/world_concepts/world_object.py index 37c64d12b..7148c743e 100644 --- a/src/pycram/world_concepts/world_object.py +++ b/src/pycram/world_concepts/world_object.py @@ -511,14 +511,15 @@ def get_link_tf_frame(self, link_name: str) -> str: """ return self.links[link_name].tf_frame - def get_link_axis_aligned_bounding_box(self, link_name: str) -> AxisAlignedBoundingBox: + def get_link_axis_aligned_bounding_box(self, link_name: str, transform_to_link_pose: bool = True) -> AxisAlignedBoundingBox: """ Return the axis aligned bounding box of the link with the given name. :param link_name: The name of the link. + :param transform_to_link_pose: If True, the bounding box will be transformed to fit link pose. :return: The axis aligned bounding box of the link. """ - return self.links[link_name].get_axis_aligned_bounding_box() + return self.links[link_name].get_axis_aligned_bounding_box(transform_to_link_pose) def get_transform_between_links(self, from_link: str, to_link: str) -> Transform: """ @@ -1395,14 +1396,15 @@ def links_colors(self) -> Dict[str, Color]: """ return self.world.get_colors_of_object_links(self) - def get_axis_aligned_bounding_box(self) -> AxisAlignedBoundingBox: + def get_axis_aligned_bounding_box(self, transform_to_object_pose: bool = True) -> AxisAlignedBoundingBox: """ Return the axis aligned bounding box of this object. + :param transform_to_object_pose: If True, the bounding box will be transformed to fit object pose. :return: The axis aligned bounding box of this object. """ if self.has_one_link: - return self.root_link.get_axis_aligned_bounding_box() + return self.root_link.get_axis_aligned_bounding_box(transform_to_object_pose) else: return self.world.get_object_axis_aligned_bounding_box(self) From 1e4f319f3570dc2b9851272d7d82984133a1bd96 Mon Sep 17 00:00:00 2001 From: A_Bassiouny Date: Thu, 28 Nov 2024 10:57:07 +0100 Subject: [PATCH 72/91] [MultiverseBugFix] empty send data. --- src/pycram/worlds/multiverse.py | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/pycram/worlds/multiverse.py b/src/pycram/worlds/multiverse.py index 6e5230ca4..82658543e 100644 --- a/src/pycram/worlds/multiverse.py +++ b/src/pycram/worlds/multiverse.py @@ -431,7 +431,8 @@ def reset_multiple_objects_base_poses(self, objects: Dict[Object, Pose]) -> None obj.set_mobile_robot_pose(objects[obj]) objects = {obj: pose for obj, pose in objects.items() if obj.obj_type not in [ObjectType.ENVIRONMENT, ObjectType.ROBOT]} - self._set_multiple_body_poses({obj.name: pose for obj, pose in objects.items()}) + if len(objects) > 0: + self._set_multiple_body_poses({obj.name: pose for obj, pose in objects.items()}) def _set_body_pose(self, body_name: str, pose: Pose) -> None: """ From 6b3d824725d9c0552497ab7bf76b01fb84b8e8b7 Mon Sep 17 00:00:00 2001 From: A_Bassiouny Date: Thu, 28 Nov 2024 23:01:07 +0100 Subject: [PATCH 73/91] [MultiverseGetImages] Made use of target pose to adjust camera pose. --- src/pycram/datastructures/world.py | 5 +- src/pycram/utils.py | 112 +++++++++++++++++++++++++++-- src/pycram/worlds/multiverse.py | 11 +-- test/test_multiverse.py | 2 +- 4 files changed, 117 insertions(+), 13 deletions(-) diff --git a/src/pycram/datastructures/world.py b/src/pycram/datastructures/world.py index cd6a5b5a2..a2a35377b 100644 --- a/src/pycram/datastructures/world.py +++ b/src/pycram/datastructures/world.py @@ -814,9 +814,8 @@ def set_multiple_joint_positions(self, joint_positions: Dict[Joint, float]) -> b """ filtered_joint_positions = copy(joint_positions) for joint, position in joint_positions.items(): - if joint.object.obj_type == ObjectType.ROBOT and self.robot_description.ignore_joints: - if joint.name in self.robot_description.ignore_joints: - filtered_joint_positions.pop(joint) + if joint.name in self.robot_description.ignore_joints: + filtered_joint_positions.pop(joint) return self._set_multiple_joint_positions(filtered_joint_positions) @validate_multiple_joint_positions diff --git a/src/pycram/utils.py b/src/pycram/utils.py index 08e776a4f..bb8b8040c 100644 --- a/src/pycram/utils.py +++ b/src/pycram/utils.py @@ -6,6 +6,7 @@ Classes: GeneratorList -- implementation of generator list wrappers. """ +from __future__ import annotations from inspect import isgeneratorfunction import os import math @@ -13,7 +14,8 @@ import numpy as np from matplotlib import pyplot as plt import matplotlib.colors as mcolors -from typing_extensions import Tuple, Callable, List, Dict, TYPE_CHECKING +from tf.transformations import quaternion_about_axis, quaternion_multiply +from typing_extensions import Tuple, Callable, List, Dict, TYPE_CHECKING, Sequence from .datastructures.dataclasses import Color from .datastructures.pose import Pose @@ -157,6 +159,105 @@ def __exit__(self, *_): os.close(fd) +def adjust_camera_pose_based_on_target(cam_pose: Pose, target_pose: Pose, + camera_description: CameraDescription) -> Pose: + """ + Adjust the camera pose based on the target pose. + + :param cam_pose: The camera pose. + :param target_pose: The target pose. + :param camera_description: The camera description. + :return: The adjusted camera pose. + """ + quaternion = get_quaternion_between_camera_and_target(cam_pose, target_pose, camera_description) + # apply the rotation to the camera pose using quaternion multiplication + return apply_quaternion_to_pose(cam_pose, quaternion) + + +def get_quaternion_between_camera_and_target(cam_pose: Pose, target_pose: Pose, + camera_description: 'CameraDescription') -> np.ndarray: + """ + Get the quaternion between the camera and the target. + + :param cam_pose: The camera pose. + :param target_pose: The target pose. + :param camera_description: The camera description. + :return: The quaternion between the camera and the target. + """ + # Get the front facing axis of the camera in the world frame + front_facing_axis = transform_vector_using_pose(camera_description.front_facing_axis, cam_pose) + front_facing_axis = front_facing_axis - np.array(cam_pose.position_as_list()) + + # Get the vector from the camera to the target + camera_to_target = get_vector_between_poses(cam_pose, target_pose) + + # Get the quaternion between the camera and target + return get_quaternion_between_two_vectors(front_facing_axis, camera_to_target) + + +def get_vector_between_poses(pose1: Pose, pose2: Pose) -> np.ndarray: + """ + Get the vector between two poses. + + :param pose1: The first pose. + :param pose2: The second pose. + :return: The vector between the two poses. + """ + return np.array(pose2.position_as_list()) - np.array(pose1.position_as_list()) + + +def transform_vector_using_pose(vector: Sequence, pose: Pose) -> np.ndarray: + """ + Transform a vector using a pose. + + :param vector: The vector. + :param pose: The pose. + :return: The transformed vector. + """ + vector = np.array(vector).reshape(1, 3) + return pose.to_transform("pose").apply_transform_to_array_of_points(vector).flatten() + + +def apply_quaternion_to_pose(pose: Pose, quaternion: np.ndarray) -> Pose: + """ + Apply a quaternion to a pose. + + :param pose: The pose. + :param quaternion: The quaternion. + :return: The new pose. + """ + pose_quaternion = np.array(pose.orientation_as_list()) + new_quaternion = quaternion_multiply(quaternion, pose_quaternion) + return Pose(pose.position_as_list(), new_quaternion.tolist()) + + +def get_quaternion_between_two_vectors(v1: np.ndarray, v2: np.ndarray) -> np.ndarray: + """ + Get the quaternion between two vectors. + + :param v1: The first vector. + :param v2: The second vector. + :return: The quaternion between the two vectors. + """ + axis, angle = get_axis_angle_between_two_vectors(v1, v2) + return quaternion_about_axis(angle, axis) + + +def get_axis_angle_between_two_vectors(v1: np.ndarray, v2: np.ndarray) -> Tuple[np.ndarray, float]: + """ + Get the axis and angle between two vectors. + + :param v1: The first vector. + :param v2: The second vector. + :return: The axis and angle between the two vectors. + """ + v1 = v1 / np.linalg.norm(v1) + v2 = v2 / np.linalg.norm(v2) + axis = np.cross(v1, v2) + angle = np.arccos(np.dot(v1, v2) - 1e-9) # to avoid numerical errors + return axis, angle + + class RayTestUtils: def __init__(self, ray_test_batch: Callable, object_id_to_name: Dict = None): @@ -256,10 +357,13 @@ def get_camera_rays_start_pose(self, camera_description: 'CameraDescription', ca :param camera_pose: The camera pose. :param camera_min_distance: The minimum distance from which the camera can see. """ - camera_pose_in_camera_frame = self.local_transformer.transform_pose(camera_pose, camera_frame) + camera_transform = camera_pose.to_transform("camera_pose") + self.local_transformer.update_transforms([camera_transform]) + camera_pose_in_camera_frame = Pose(frame="camera_pose") + # camera_pose_in_camera_frame = self.local_transformer.transform_pose(camera_pose, camera_frame) start_position = (np.array(camera_description.front_facing_axis) * camera_min_distance + np.array(camera_pose_in_camera_frame.position_as_list())) - start_pose = Pose(start_position.tolist(), camera_pose_in_camera_frame.orientation_as_list(), camera_frame) + start_pose = Pose(start_position.tolist(), camera_pose_in_camera_frame.orientation_as_list(), "camera_pose") return self.local_transformer.transform_pose(start_pose, "map") def get_camera_rays_end_positions(self, camera_description: 'CameraDescription', camera_frame: str, @@ -292,7 +396,7 @@ def transform_points_from_camera_frame_to_world_frame(camera_pose: Pose, camera_ :param points: The points to transform. :return: The transformed points. """ - cam_to_world_transform = camera_pose.to_transform(camera_frame) + cam_to_world_transform = camera_pose.to_transform("camera_pose") return cam_to_world_transform.apply_transform_to_array_of_points(points) @staticmethod diff --git a/src/pycram/worlds/multiverse.py b/src/pycram/worlds/multiverse.py index 82658543e..e4ac914ba 100644 --- a/src/pycram/worlds/multiverse.py +++ b/src/pycram/worlds/multiverse.py @@ -5,7 +5,7 @@ from time import sleep import numpy as np -from tf.transformations import quaternion_matrix +from tf.transformations import quaternion_matrix, quaternion_about_axis, quaternion_multiply from typing_extensions import List, Dict, Optional, Union, Tuple, Callable from .multiverse_communication.client_manager import MultiverseClientManager @@ -19,9 +19,9 @@ from ..description import Link, Joint from ..object_descriptors.generic import ObjectDescription as GenericObjectDescription from ..object_descriptors.mjcf import ObjectDescription as MJCF, PrimitiveObjectFactory -from ..robot_description import RobotDescription +from ..robot_description import RobotDescription, CameraDescription from ..ros.logging import logwarn -from ..utils import RayTestUtils, wxyz_to_xyzw, xyzw_to_wxyz +from ..utils import RayTestUtils, wxyz_to_xyzw, xyzw_to_wxyz, adjust_camera_pose_based_on_target from ..validation.goal_validator import validate_object_pose, validate_multiple_joint_positions, \ validate_joint_position, validate_multiple_object_poses from ..world_concepts.constraints import Constraint @@ -171,13 +171,14 @@ def get_images_for_target(self, target_pose: Pose, size: int = 256, camera_min_distance: float = 0.1, camera_max_distance: int = 3, - plot: bool = False) -> List[np.ndarray]: + plot: bool = True) -> List[np.ndarray]: """ Uses ray test to get the images for the target object. (target_pose is currently not used) """ camera_description = RobotDescription.current_robot_description.get_default_camera() camera_frame = RobotDescription.current_robot_description.get_camera_frame() - return self.ray_test_utils.get_images_for_target(cam_pose, camera_description, camera_frame, + adjusted_cam_pose = adjust_camera_pose_based_on_target(cam_pose, target_pose, camera_description) + return self.ray_test_utils.get_images_for_target(adjusted_cam_pose, camera_description, camera_frame, size, camera_min_distance, camera_max_distance, plot) @staticmethod diff --git a/test/test_multiverse.py b/test/test_multiverse.py index 399de5906..bc20da075 100644 --- a/test/test_multiverse.py +++ b/test/test_multiverse.py @@ -155,7 +155,7 @@ def test_get_images_for_target(self): milk_spawn_position = np.array(camera_front_facing_axis) * 0.5 orientation = camera_pose.to_transform(camera_frame).invert().rotation_as_list() milk = self.spawn_milk(milk_spawn_position.tolist(), orientation, frame=camera_frame) - _, depth, segmentation_mask = self.multiverse.get_images_for_target(milk.pose, camera_pose, plot=False) + _, depth, segmentation_mask = self.multiverse.get_images_for_target(milk.pose, camera_pose, plot=True) self.assertIsInstance(depth, np.ndarray) self.assertIsInstance(segmentation_mask, np.ndarray) self.assertTrue(depth.shape == (256, 256)) From 9e8a516c2af4863bd3bb69ed4156b726879978ec Mon Sep 17 00:00:00 2001 From: A_Bassiouny Date: Thu, 28 Nov 2024 23:07:18 +0100 Subject: [PATCH 74/91] [MultiverseGetImages] plot is false by default --- src/pycram/worlds/multiverse.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/pycram/worlds/multiverse.py b/src/pycram/worlds/multiverse.py index e4ac914ba..79f8198f3 100644 --- a/src/pycram/worlds/multiverse.py +++ b/src/pycram/worlds/multiverse.py @@ -171,7 +171,7 @@ def get_images_for_target(self, target_pose: Pose, size: int = 256, camera_min_distance: float = 0.1, camera_max_distance: int = 3, - plot: bool = True) -> List[np.ndarray]: + plot: bool = False) -> List[np.ndarray]: """ Uses ray test to get the images for the target object. (target_pose is currently not used) """ From 6a02a2fa44b9ffd4d54d9e1c2b3dd8c933900337 Mon Sep 17 00:00:00 2001 From: A_Bassiouny Date: Thu, 28 Nov 2024 23:07:42 +0100 Subject: [PATCH 75/91] [MultiverseDemo] formatting --- demos/pycram_multiverse_demo/demo.py | 1 + 1 file changed, 1 insertion(+) diff --git a/demos/pycram_multiverse_demo/demo.py b/demos/pycram_multiverse_demo/demo.py index ecf68165b..c12a1da7c 100644 --- a/demos/pycram_multiverse_demo/demo.py +++ b/demos/pycram_multiverse_demo/demo.py @@ -21,6 +21,7 @@ def move_and_detect(obj_type: ObjectType, pick_pose: Pose): return object_desig + world = Multiverse() extension = ObjectDescription.get_file_extension() robot = Object('pr2', ObjectType.ROBOT, f'pr2{extension}', pose=Pose([1.3, 2, 0.01])) From 6beac862dcef61cf3cf5a64f348b2158a3fb0344 Mon Sep 17 00:00:00 2001 From: A_Bassiouny Date: Thu, 28 Nov 2024 23:08:11 +0100 Subject: [PATCH 76/91] [JupyterExamples] correcting link name in multiverse case of the example. --- examples/cram_plan_tutorial.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/examples/cram_plan_tutorial.md b/examples/cram_plan_tutorial.md index 542552075..2ce9819ed 100644 --- a/examples/cram_plan_tutorial.md +++ b/examples/cram_plan_tutorial.md @@ -94,7 +94,7 @@ import pycrap from pycram.costmaps import SemanticCostmap from pycram.pose_generator_and_validator import PoseGenerator -counter_name = "counter_stove_sink" if use_multiverse else "island_countertop" +counter_name = "counter_sink_stove" if use_multiverse else "island_countertop" counter_link = apartment.get_link(counter_name) counter_bounds = counter_link.get_axis_aligned_bounding_box() scm = SemanticCostmap(apartment, counter_name) From 216528c395786ad43b19db4cc71ad9428b2926e9 Mon Sep 17 00:00:00 2001 From: A_Bassiouny Date: Thu, 28 Nov 2024 23:32:37 +0100 Subject: [PATCH 77/91] [AccessingLocation] check for collision at the start to make search faster. --- src/pycram/designators/location_designator.py | 34 ++++--------------- src/pycram/helper.py | 2 +- src/pycram/world_reasoning.py | 28 +++++++++++++++ 3 files changed, 35 insertions(+), 29 deletions(-) diff --git a/src/pycram/designators/location_designator.py b/src/pycram/designators/location_designator.py index 3eb499a1a..98e84020b 100644 --- a/src/pycram/designators/location_designator.py +++ b/src/pycram/designators/location_designator.py @@ -13,7 +13,7 @@ from ..robot_description import RobotDescription from ..ros.logging import logdebug from ..world_concepts.world_object import Object, Link -from ..world_reasoning import link_pose_for_joint_config, contact, is_a_picked_object +from ..world_reasoning import link_pose_for_joint_config, contact, is_a_picked_object, check_for_collision class Location(LocationDesignatorDescription): @@ -146,29 +146,6 @@ def ground(self) -> Location: """ return next(iter(self)) - def check_for_collision(self, robot: Object, pose: Pose) -> bool: - """ - Check if the robot collides with any object in the world at the given pose. - - :param robot: The robot object - :param pose: The pose to check for collision - :return: True if the robot collides with any object, False otherwise - """ - robot.set_pose(pose) - floor = robot.world.get_object_by_name("floor") - ignore = [o.name for o in self.ignore_collision_with] - for obj in robot.world.objects: - if obj.name in ([robot.name, floor.name] + ignore): - continue - in_contact, contact_links = contact(robot, obj, return_links=True) - if in_contact and not is_a_picked_object(robot, obj, [links[0] for links in contact_links]): - logdebug(f"Robot is in contact with {obj.name} in prospection: {obj.world.is_prospection_world}" - f"at position {pose.position_as_list()} and z_angle {pose.z_angle}") - return True - logdebug(f"Robot is not in contact with {obj.name} in prospection: {obj.world.is_prospection_world}" - f"at position {pose.position_as_list()} and z_angle {pose.z_angle}") - return False - def __iter__(self): """ Generates positions for a given set of constrains from a costmap and returns @@ -208,13 +185,12 @@ def __iter__(self): if self.visible_for or self.reachable_for: robot_object = self.visible_for.world_object if self.visible_for else self.reachable_for.world_object test_robot = World.current_world.get_prospection_object_for_object(robot_object) - # test_robot = robot_object self.ignore_collision_with = [World.current_world.get_prospection_object_for_object(o) for o in self.ignore_collision_with] with UseProspectionWorld(): for maybe_pose in PoseGenerator(final_map, number_of_samples=600): if self.check_collision_at_start and (test_robot is not None): - if self.check_for_collision(test_robot, maybe_pose): + if check_for_collision(test_robot, maybe_pose, self.ignore_collision_with): continue res = True arms = None @@ -314,8 +290,10 @@ def __iter__(self) -> Location: with UseProspectionWorld(): for maybe_pose in PoseGenerator(final_map, number_of_samples=600, - orientation_generator=lambda p, o: PoseGenerator.generate_orientation(p, half_pose)): - + orientation_generator=lambda p, o: PoseGenerator.generate_orientation(p, + half_pose)): + if check_for_collision(test_robot, maybe_pose): + continue hand_links = [] for description in RobotDescription.current_robot_description.get_manipulator_chains(): hand_links += description.end_effector.links diff --git a/src/pycram/helper.py b/src/pycram/helper.py index a396f5b5b..3b14578d3 100644 --- a/src/pycram/helper.py +++ b/src/pycram/helper.py @@ -4,7 +4,7 @@ Singleton -- implementation of singleton metaclass """ import os -from typing_extensions import Dict, Optional +from typing_extensions import Dict, Optional, List import xml.etree.ElementTree as ET from .ros.logging import logwarn diff --git a/src/pycram/world_reasoning.py b/src/pycram/world_reasoning.py index 670333d9c..94c8ca6dd 100644 --- a/src/pycram/world_reasoning.py +++ b/src/pycram/world_reasoning.py @@ -6,6 +6,7 @@ from .datastructures.world import World, UseProspectionWorld from .external_interfaces.ik import try_to_reach, try_to_reach_with_grasp from .robot_description import RobotDescription +from .ros.logging import logdebug from .utils import RayTestUtils from .world_concepts.world_object import Object, Link from .config import world_conf as conf @@ -62,6 +63,33 @@ def contact( return objects_are_in_contact +def check_for_collision(robot: Object, pose: Pose, + ignore_collision_with: Optional[List[Object]] = None) -> bool: + """ + Check if the robot collides with any object in the world at the given pose. + + :param robot: The robot object + :param pose: The pose to check for collision + :param ignore_collision_with: A list of objects to ignore collision with + :return: True if the robot collides with any object, False otherwise + """ + robot.set_pose(pose) + floor = robot.world.get_object_by_name("floor") + ignore_collision_with = [] if ignore_collision_with is None else ignore_collision_with + ignore = [o.name for o in ignore_collision_with] + for obj in robot.world.objects: + if obj.name in ([robot.name, floor.name] + ignore): + continue + in_contact, contact_links = contact(robot, obj, return_links=True) + if in_contact and not is_a_picked_object(robot, obj, [links[0] for links in contact_links]): + logdebug(f"Robot is in contact with {obj.name} in prospection: {obj.world.is_prospection_world}" + f"at position {pose.position_as_list()} and z_angle {pose.z_angle}") + return True + logdebug(f"Robot is not in contact with {obj.name} in prospection: {obj.world.is_prospection_world}" + f"at position {pose.position_as_list()} and z_angle {pose.z_angle}") + return False + + def is_a_picked_object(robot: Object, obj: Object, robot_contact_links: List[Link]) -> bool: """ Check if the object is picked by the robot. From f087926c3efa335ecdbf4868d68dd83e85f95ce5 Mon Sep 17 00:00:00 2001 From: Abdelrhman Bassiouny Date: Fri, 29 Nov 2024 16:29:18 +0100 Subject: [PATCH 78/91] [JupyterTests] (WIP) location designator passes. --- config/multiverse_conf.py | 2 + config/world_conf.py | 5 ++ examples/cram_plan_tutorial.md | 5 +- examples/location_designator.md | 26 ++++++-- src/pycram/costmaps.py | 59 ++++++++++++++++++- src/pycram/datastructures/world.py | 2 +- src/pycram/designators/location_designator.py | 42 ++++++++----- src/pycram/pose_generator_and_validator.py | 2 +- src/pycram/world_reasoning.py | 32 +++++----- src/pycram/worlds/multiverse.py | 4 +- 10 files changed, 134 insertions(+), 45 deletions(-) diff --git a/config/multiverse_conf.py b/config/multiverse_conf.py index 5688fbb55..bbba928fa 100644 --- a/config/multiverse_conf.py +++ b/config/multiverse_conf.py @@ -76,3 +76,5 @@ class MultiverseConfig(WorldConfig): allow_gripper_collision = True use_multiverse_process_modules = True + + depth_images_are_in_meter = True diff --git a/config/world_conf.py b/config/world_conf.py index c890d06da..2c0eacf21 100644 --- a/config/world_conf.py +++ b/config/world_conf.py @@ -104,6 +104,11 @@ class WorldConfig: Whether to allow the gripper to collide with the objects when planning for the goals. """ + depth_images_are_in_meter: bool = False + """ + Whether the depth images produced by :meth:`datastructures.world.World.get_images_for_target` are in meters. + """ + @classmethod def get_pose_tolerance(cls) -> Tuple[float, float]: return cls.position_tolerance, cls.orientation_tolerance diff --git a/examples/cram_plan_tutorial.md b/examples/cram_plan_tutorial.md index 2ce9819ed..33458861c 100644 --- a/examples/cram_plan_tutorial.md +++ b/examples/cram_plan_tutorial.md @@ -43,11 +43,10 @@ if use_multiverse: from pycram.worlds.multiverse import Multiverse world = Multiverse() except ImportError: - Multiverse = None - raise ImportError("Multiverse not available") + raise ImportError("Multiverse is not installed, please install it to use it.") else: - world = BulletWorld() from pycram.ros_utils.viz_marker_publisher import VizMarkerPublisher + world = BulletWorld() viz_marker_publisher = VizMarkerPublisher() robot = Object("pr2", pycrap.Robot, "pr2.urdf") diff --git a/examples/location_designator.md b/examples/location_designator.md index 02a2a608a..d213b7332 100644 --- a/examples/location_designator.md +++ b/examples/location_designator.md @@ -44,7 +44,19 @@ from pycram.datastructures.enums import ObjectType, WorldMode from pycram.datastructures.pose import Pose import pycrap -world = BulletWorld(WorldMode.DIRECT) +use_multiverse = False +viz_marker_publisher = None +if use_multiverse: + try: + from pycram.worlds.multiverse import Multiverse + world = Multiverse() + except ImportError: + raise ImportError("Multiverse is not installed, please install it to use it.") +else: + from pycram.ros_utils.viz_marker_publisher import VizMarkerPublisher + world = BulletWorld() + viz_marker_publisher = VizMarkerPublisher() + apartment = Object("apartment", pycrap.Apartment, "apartment.urdf") pr2 = Object("pr2", pycrap.Robot, "pr2.urdf") ``` @@ -139,7 +151,8 @@ from pycram.designators.object_designator import BelieveObject kitchen_desig = BelieveObject(names=["apartment"]).resolve() milk_desig = BelieveObject(names=["milk"]).resolve() -location_description = SemanticCostmapLocation(link_name="island_countertop", +counter_name = "counter_sink_stove" if use_multiverse else "island_countertop" +location_description = SemanticCostmapLocation(link_name=counter_name, part_of=kitchen_desig, for_object=milk_desig) @@ -178,13 +191,14 @@ spawned it in a previous example. Furthermore, we need a robot, so we also spawn ```python from pycram.designators.object_designator import * from pycram.designators.location_designator import * -from pycram.datastructures.enums import ObjectType apartment_desig = BelieveObject(names=["apartment"]) -handle_desig = ObjectPart(names=["handle_cab10_t"], part_of=apartment_desig.resolve()) +handle_name = "cabinet10_drawer1_handle" if use_multiverse else "handle_cab10_t" +handle_desig = ObjectPart(names=[handle_name], part_of=apartment_desig.resolve()) robot_desig = BelieveObject(types=[pycrap.Robot]) -access_location = AccessingLocation(handle_desig.resolve(), robot_desig.resolve()).resolve() +access_location = AccessingLocation(handle_desig.resolve(), robot_desig.resolve(), + prepose_distance=0.03).resolve() print(access_location.pose) ``` @@ -212,5 +226,7 @@ if "/giskard" in rosnode.get_node_names(): If you are finished with this example you can close the world with the following cell: ```python +if viz_marker_publisher is not None: + viz_marker_publisher._stop_publishing() world.exit() ``` diff --git a/src/pycram/costmaps.py b/src/pycram/costmaps.py index 7ab4c48c3..5acdf7058 100644 --- a/src/pycram/costmaps.py +++ b/src/pycram/costmaps.py @@ -514,7 +514,9 @@ def __init__(self, min_height: float, size: Optional[int] = 100, resolution: Optional[float] = 0.02, origin: Optional[Pose] = None, - world: Optional[World] = None): + world: Optional[World] = None, + target_object: Optional[Object] = None, + robot: Optional[Object] = None): """ Visibility Costmaps show for every position around the origin pose if the origin can be seen from this pose. The costmap is able to deal with height differences of the camera while in a single position, for example, if @@ -531,6 +533,8 @@ def __init__(self, min_height: float, :param origin: The pose in world coordinate frame around which the costmap should be created. :param world: The World for which the costmap should be created. + :param target_object: The object that should be visible. + :param robot: The robot for which the visibility costmap should be created. """ if (11 * size ** 2 + size ** 3) * 2 > psutil.virtual_memory().available: raise OSError("Not enough free RAM to calculate a costmap of this size") @@ -544,9 +548,53 @@ def __init__(self, min_height: float, # for pr2 = 1.6 self.min_height: float = min_height self.origin: Pose = Pose() if not origin else origin + self.target_object: Optional[Object] = target_object + self.robot: Optional[Object] = robot self._generate_map() Costmap.__init__(self, resolution, size, size, self.origin, self.map) + @property + def robot(self) -> Optional[Object]: + return self._robot + + @robot.setter + def robot(self, robot: Optional[Object]) -> None: + if robot is not None: + self._robot = World.current_world.get_prospection_object_for_object(robot) + self.robot_original_pose = self._robot.pose + else: + self._robot = None + self.robot_original_pose = None + + @property + def target_object(self) -> Optional[Object]: + return self._target_object + + @target_object.setter + def target_object(self, target_object: Optional[Object]) -> None: + if target_object is not None: + self._target_object = World.current_world.get_prospection_object_for_object(target_object) + self.target_original_pose = self._target_object.pose + else: + self._target_object = None + self.target_original_pose = None + + def move_target_and_robot_far_away(self): + if self.target_object is not None: + self.target_object.set_pose(Pose([self.origin.position.x + self.size * self.resolution * 2, + self.origin.position.y + self.size * self.resolution * 2, + self.target_original_pose.position.z])) + if self.robot is not None: + self.robot.set_pose(Pose([self.origin.position.x + self.size * self.resolution * 3, + self.origin.position.y + self.size * self.resolution * 3, + self.robot_original_pose.position.z])) + + def return_target_and_robot_to_their_original_position(self): + if self.target_original_pose is not None: + self.target_object.set_pose(self.target_original_pose) + if self.robot_original_pose is not None: + self.robot.set_pose(self.robot_original_pose) + def _create_images(self) -> List[np.ndarray]: """ Creates four depth images in every direction around the point @@ -559,6 +607,8 @@ def _create_images(self) -> List[np.ndarray]: images = [] camera_pose = self.origin + self.move_target_and_robot_far_away() + with UseProspectionWorld(): origin_copy = self.origin.copy() origin_copy.position.y += 1 @@ -577,8 +627,11 @@ def _create_images(self) -> List[np.ndarray]: origin_copy.position.x += 1 images.append(self.world.get_images_for_target(origin_copy, camera_pose, size=self.size)[1]) - for i in range(0, 4): - images[i] = self._depth_buffer_to_meter(images[i]) + self.return_target_and_robot_to_their_original_position() + + if not World.current_world.conf.depth_images_are_in_meter: + for i in range(0, 4): + images[i] = self._depth_buffer_to_meter(images[i]) return images def _depth_buffer_to_meter(self, buffer: np.ndarray) -> np.ndarray: diff --git a/src/pycram/datastructures/world.py b/src/pycram/datastructures/world.py index a2a35377b..cfcec807d 100644 --- a/src/pycram/datastructures/world.py +++ b/src/pycram/datastructures/world.py @@ -458,7 +458,7 @@ def remove_object(self, obj: Object) -> None: self.objects.remove(obj) self.remove_object_from_original_state(obj) - if World.robot == obj: + if World.robot == obj and not self.is_prospection_world: World.robot = None self.object_lock.release() diff --git a/src/pycram/designators/location_designator.py b/src/pycram/designators/location_designator.py index 98e84020b..d9f0152f6 100644 --- a/src/pycram/designators/location_designator.py +++ b/src/pycram/designators/location_designator.py @@ -13,7 +13,7 @@ from ..robot_description import RobotDescription from ..ros.logging import logdebug from ..world_concepts.world_object import Object, Link -from ..world_reasoning import link_pose_for_joint_config, contact, is_a_picked_object, check_for_collision +from ..world_reasoning import link_pose_for_joint_config, contact, is_a_picked_object, robot_will_be_in_collision_at_pose class Location(LocationDesignatorDescription): @@ -164,8 +164,10 @@ def __iter__(self): # This ensures that the costmaps always get a position as their origin. if isinstance(self.target, ObjectDesignatorDescription.Object): target_pose = self.target.world_object.get_pose() + target_object = self.target.world_object else: target_pose = self.target.copy() + target_object = None # ground_pose = [[target_pose[0][0], target_pose[0][1], 0], target_pose[1]] ground_pose = Pose(target_pose.position_as_list()) @@ -174,23 +176,26 @@ def __iter__(self): occupancy = OccupancyCostmap(0.32, False, 200, 0.02, ground_pose) final_map = occupancy + test_robot = None + if self.visible_for or self.reachable_for: + robot_object = self.visible_for.world_object if self.visible_for else self.reachable_for.world_object + test_robot = World.current_world.get_prospection_object_for_object(robot_object) + if self.reachable_for: gaussian = GaussianCostmap(200, 15, 0.02, ground_pose) final_map += gaussian if self.visible_for: - visible = VisibilityCostmap(min_height, max_height, 200, 0.02, Pose(target_pose.position_as_list())) + visible = VisibilityCostmap(min_height, max_height, 200, 0.02, + Pose(target_pose.position_as_list()), target_object=target_object, + robot=test_robot) final_map += visible - test_robot = None - if self.visible_for or self.reachable_for: - robot_object = self.visible_for.world_object if self.visible_for else self.reachable_for.world_object - test_robot = World.current_world.get_prospection_object_for_object(robot_object) self.ignore_collision_with = [World.current_world.get_prospection_object_for_object(o) for o in self.ignore_collision_with] with UseProspectionWorld(): for maybe_pose in PoseGenerator(final_map, number_of_samples=600): if self.check_collision_at_start and (test_robot is not None): - if check_for_collision(test_robot, maybe_pose, self.ignore_collision_with): + if robot_will_be_in_collision_at_pose(test_robot, maybe_pose, self.ignore_collision_with): continue res = True arms = None @@ -232,17 +237,21 @@ class Location(LocationDesignatorDescription.Location): List of arms that can be used to for accessing from this pose """ - def __init__(self, handle_desig: ObjectPart.Object, robot_desig: ObjectDesignatorDescription.Object): + def __init__(self, handle_desig: ObjectPart.Object, + robot_desig: ObjectDesignatorDescription.Object, + prepose_distance: float = 0.03): """ Describes a position from where a drawer can be opened. For now this position should be calculated before the drawer will be opened. Calculating the pose while the drawer is open could lead to problems. :param handle_desig: ObjectPart designator for handle of the drawer :param robot_desig: Object designator for the robot which should open the drawer + :param prepose_distance: Distance to the target pose where the robot should be checked for reachability. """ super().__init__() self.handle: ObjectPart.Object = handle_desig self.robot: ObjectDesignatorDescription.Object = robot_desig.world_object + self.prepose_distance = prepose_distance def ground(self) -> Location: """ @@ -292,22 +301,25 @@ def __iter__(self) -> Location: for maybe_pose in PoseGenerator(final_map, number_of_samples=600, orientation_generator=lambda p, o: PoseGenerator.generate_orientation(p, half_pose)): - if check_for_collision(test_robot, maybe_pose): + if robot_will_be_in_collision_at_pose(test_robot, maybe_pose): continue hand_links = [] for description in RobotDescription.current_robot_description.get_manipulator_chains(): hand_links += description.end_effector.links valid_init, arms_init = reachability_validator(maybe_pose, test_robot, init_pose, - allowed_collision={test_robot: hand_links}) + allowed_collision={test_robot: hand_links}, + prepose_distance=self.prepose_distance) - valid_goal, arms_goal = reachability_validator(maybe_pose, test_robot, goal_pose, - allowed_collision={test_robot: hand_links}) + if valid_init: + valid_goal, arms_goal = reachability_validator(maybe_pose, test_robot, goal_pose, + allowed_collision={test_robot: hand_links}, + prepose_distance=self.prepose_distance) - arms_list = list(set(arms_init).intersection(set(arms_goal))) + arms_list = list(set(arms_init).intersection(set(arms_goal))) - if valid_init and valid_goal and len(arms_list) > 0: - yield self.Location(maybe_pose, arms_list) + if valid_goal and len(arms_list) > 0: + yield self.Location(maybe_pose, arms_list) class SemanticCostmapLocation(LocationDesignatorDescription): diff --git a/src/pycram/pose_generator_and_validator.py b/src/pycram/pose_generator_and_validator.py index 6c7e31340..080395fba 100644 --- a/src/pycram/pose_generator_and_validator.py +++ b/src/pycram/pose_generator_and_validator.py @@ -159,7 +159,7 @@ def _in_contact(robot: Object, obj: Object, allowed_collision: Dict[Object, List def reachability_validator(pose: Pose, robot: Object, target: Union[Object, Pose], - prepose_distance: float = 0.07, + prepose_distance: float = 0.03, allowed_collision: Dict[Object, List] = None, arm: Optional[Arms] = None) -> Tuple[bool, List]: """ diff --git a/src/pycram/world_reasoning.py b/src/pycram/world_reasoning.py index 94c8ca6dd..c09dbe589 100644 --- a/src/pycram/world_reasoning.py +++ b/src/pycram/world_reasoning.py @@ -63,8 +63,8 @@ def contact( return objects_are_in_contact -def check_for_collision(robot: Object, pose: Pose, - ignore_collision_with: Optional[List[Object]] = None) -> bool: +def robot_will_be_in_collision_at_pose(robot: Object, pose: Pose, + ignore_collision_with: Optional[List[Object]] = None) -> bool: """ Check if the robot collides with any object in the world at the given pose. @@ -73,20 +73,22 @@ def check_for_collision(robot: Object, pose: Pose, :param ignore_collision_with: A list of objects to ignore collision with :return: True if the robot collides with any object, False otherwise """ - robot.set_pose(pose) - floor = robot.world.get_object_by_name("floor") - ignore_collision_with = [] if ignore_collision_with is None else ignore_collision_with - ignore = [o.name for o in ignore_collision_with] - for obj in robot.world.objects: - if obj.name in ([robot.name, floor.name] + ignore): - continue - in_contact, contact_links = contact(robot, obj, return_links=True) - if in_contact and not is_a_picked_object(robot, obj, [links[0] for links in contact_links]): - logdebug(f"Robot is in contact with {obj.name} in prospection: {obj.world.is_prospection_world}" + with UseProspectionWorld(): + prospection_robot = World.current_world.get_prospection_object_for_object(robot) + prospection_robot.set_pose(pose) + floor = prospection_robot.world.get_object_by_name("floor") + ignore_collision_with = [] if ignore_collision_with is None else ignore_collision_with + ignore = [o.name for o in ignore_collision_with] + for obj in prospection_robot.world.objects: + if obj.name in ([prospection_robot.name, floor.name] + ignore): + continue + in_contact, contact_links = contact(prospection_robot, obj, return_links=True) + if in_contact and not is_a_picked_object(prospection_robot, obj, [links[0] for links in contact_links]): + logdebug(f"Robot is in contact with {obj.name} in prospection: {obj.world.is_prospection_world}" + f"at position {pose.position_as_list()} and z_angle {pose.z_angle}") + return True + logdebug(f"Robot is not in contact with {obj.name} in prospection: {obj.world.is_prospection_world}" f"at position {pose.position_as_list()} and z_angle {pose.z_angle}") - return True - logdebug(f"Robot is not in contact with {obj.name} in prospection: {obj.world.is_prospection_world}" - f"at position {pose.position_as_list()} and z_angle {pose.z_angle}") return False diff --git a/src/pycram/worlds/multiverse.py b/src/pycram/worlds/multiverse.py index 79f8198f3..d32f964e0 100644 --- a/src/pycram/worlds/multiverse.py +++ b/src/pycram/worlds/multiverse.py @@ -5,7 +5,7 @@ from time import sleep import numpy as np -from tf.transformations import quaternion_matrix, quaternion_about_axis, quaternion_multiply +from tf.transformations import quaternion_matrix from typing_extensions import List, Dict, Optional, Union, Tuple, Callable from .multiverse_communication.client_manager import MultiverseClientManager @@ -19,7 +19,7 @@ from ..description import Link, Joint from ..object_descriptors.generic import ObjectDescription as GenericObjectDescription from ..object_descriptors.mjcf import ObjectDescription as MJCF, PrimitiveObjectFactory -from ..robot_description import RobotDescription, CameraDescription +from ..robot_description import RobotDescription from ..ros.logging import logwarn from ..utils import RayTestUtils, wxyz_to_xyzw, xyzw_to_wxyz, adjust_camera_pose_based_on_target from ..validation.goal_validator import validate_object_pose, validate_multiple_joint_positions, \ From 8e15d46bd756cc09cd4d6737bc25695fc4746043 Mon Sep 17 00:00:00 2001 From: Abdelrhman Bassiouny Date: Fri, 29 Nov 2024 16:54:04 +0100 Subject: [PATCH 79/91] [JupyterTests] All tests passed. --- examples/intro.md | 2 +- src/pycram/costmaps.py | 14 +++++++------- 2 files changed, 8 insertions(+), 8 deletions(-) diff --git a/examples/intro.md b/examples/intro.md index c2251194a..48fad1898 100644 --- a/examples/intro.md +++ b/examples/intro.md @@ -134,7 +134,7 @@ o = cm.OccupancyCostmap(0.2, from_ros=False, size=300, resolution=0.02, origin=P ``` ```python -s = cm.SemanticCostmap(kitchen, "kitchen_island_surface", size=100, resolution=0.02) +s = cm.SemanticCostmap(kitchen, "kitchen_island_surface", resolution=0.02) g = cm.GaussianCostmap(200, 15, resolution=0.02) ``` diff --git a/src/pycram/costmaps.py b/src/pycram/costmaps.py index 5acdf7058..a7e049028 100644 --- a/src/pycram/costmaps.py +++ b/src/pycram/costmaps.py @@ -800,23 +800,23 @@ class SemanticCostmap(Costmap): table surface. """ - def __init__(self, object, link_name, resolution=0.02, world=None): + def __init__(self, obj: Object, link_name: str, resolution: float = 0.02, world: Optional[World] = None): """ Creates a semantic costmap for the given parameter. The semantic costmap will be on top of the link of the given Object. - :param object: The object of which the link is a part + :param obj: The object of which the link is a part :param link_name: The link name, as stated in the description of the object :param resolution: Resolution of the final costmap (how much meters one pixel represents) :param world: The World from which the costmap should be created """ self.world: World = world if world else World.current_world - self.object: Object = object - self.link: Link = object.get_link(link_name) + self.object: Object = obj + self.link: Link = obj.get_link(link_name) self.resolution: float = resolution - self.origin: Pose = object.get_link_pose(link_name) - self.height: int = 0 - self.width: int = 0 + self.origin: Pose = obj.get_link_pose(link_name) + self.height: Optional[int] = None + self.width: Optional[int] = None self.map: np.ndarray = [] self.generate_map() From a8dfa0aa5cc1b150bacee1877398dccfedfa91db Mon Sep 17 00:00:00 2001 From: A_Bassiouny Date: Sat, 30 Nov 2024 22:39:12 +0100 Subject: [PATCH 80/91] [MultiversePycrap] used pycrap in multiverse. --- src/pycram/ros_utils/robot_state_updater.py | 2 +- src/pycram/world_concepts/world_object.py | 15 +++++++-- src/pycram/worlds/multiverse.py | 37 +++++++++++---------- src/pycrap/objects.py | 6 ++++ test/test_multiverse.py | 28 ++++++++-------- 5 files changed, 53 insertions(+), 35 deletions(-) diff --git a/src/pycram/ros_utils/robot_state_updater.py b/src/pycram/ros_utils/robot_state_updater.py index c409d09ae..abff550c9 100644 --- a/src/pycram/ros_utils/robot_state_updater.py +++ b/src/pycram/ros_utils/robot_state_updater.py @@ -57,7 +57,7 @@ def _subscribe_tf(self, msg: TransformStamped) -> None: for obj in self.world.objects: if obj.name == self.world.robot.name: tf_frame = RobotDescription.current_robot_description.base_link - elif obj.has_type_environment(): + elif obj.is_an_environment: continue else: tf_frame = obj.tf_frame diff --git a/src/pycram/world_concepts/world_object.py b/src/pycram/world_concepts/world_object.py index 7148c743e..c490122ce 100644 --- a/src/pycram/world_concepts/world_object.py +++ b/src/pycram/world_concepts/world_object.py @@ -11,6 +11,7 @@ from trimesh.parent import Geometry3D from typing_extensions import Type, Optional, Dict, Tuple, List, Union +import pycrap from ..datastructures.dataclasses import (Color, ObjectState, LinkState, JointState, AxisAlignedBoundingBox, VisualShape, ClosestPointsList, ContactPointsList, RotatedBoundingBox, VirtualJoint) @@ -624,13 +625,23 @@ def reset(self, remove_saved_states=False) -> None: if remove_saved_states: self.remove_saved_states() - def has_type_environment(self) -> bool: + @property + def is_an_environment(self) -> bool: """ Check if the object is of type environment. :return: True if the object is of type environment, False otherwise. """ - return self.obj_type == ObjectType.ENVIRONMENT + return issubclass(self.obj_type, pycrap.Location) or issubclass(self.obj_type, pycrap.Floor) + + @property + def is_a_robot(self) -> bool: + """ + Check if the object is a robot. + + :return: True if the object is a robot, False otherwise. + """ + return issubclass(self.obj_type, pycrap.Robot) def attach(self, child_object: Object, diff --git a/src/pycram/worlds/multiverse.py b/src/pycram/worlds/multiverse.py index d32f964e0..794ddfc97 100644 --- a/src/pycram/worlds/multiverse.py +++ b/src/pycram/worlds/multiverse.py @@ -6,13 +6,15 @@ import numpy as np from tf.transformations import quaternion_matrix -from typing_extensions import List, Dict, Optional, Union, Tuple, Callable +from typing_extensions import List, Dict, Optional, Union, Tuple, Callable, Type +import pycrap +from pycrap import PhysicalObject from .multiverse_communication.client_manager import MultiverseClientManager from .multiverse_communication.clients import MultiverseController, MultiverseReader, MultiverseWriter, MultiverseAPI from ..config.multiverse_conf import MultiverseConfig from ..datastructures.dataclasses import Color, ContactPointsList, ContactPoint -from ..datastructures.enums import WorldMode, JointType, ObjectType, MultiverseBodyProperty, MultiverseJointPosition, \ +from ..datastructures.enums import WorldMode, JointType, MultiverseBodyProperty, MultiverseJointPosition, \ MultiverseJointCMD from ..datastructures.pose import Pose from ..datastructures.world import World @@ -141,7 +143,7 @@ def _spawn_floor(self): """ Spawn the plane in the simulator. """ - self.floor = Object("floor", ObjectType.ENVIRONMENT, "plane.urdf", + self.floor = Object("floor", pycrap.Floor, "plane.urdf", world=self) def pause_simulation(self) -> None: @@ -164,7 +166,7 @@ def load_generic_object_and_get_id(self, description: GenericObjectDescription, object_factory = PrimitiveObjectFactory(description.name, description.links[0].geometry, save_path) object_factory.build_shape() object_factory.export_to_mjcf(save_path) - return self.load_object_and_get_id(description.name, pose, ObjectType.GENERIC_OBJECT) + return self.load_object_and_get_id(description.name, pose, pycrap.PhysicalObject) def get_images_for_target(self, target_pose: Pose, cam_pose: Pose, @@ -209,7 +211,7 @@ def spawn_robot_with_controller(self, name: str, pose: Pose) -> None: def load_object_and_get_id(self, name: Optional[str] = None, pose: Optional[Pose] = None, - obj_type: Optional[ObjectType] = None) -> int: + obj_type: Optional[Type[PhysicalObject]] = None) -> int: """ Spawn the object in the simulator and return the object id. Object name has to be unique and has to be same as the name of the object in the description file. @@ -223,12 +225,13 @@ def load_object_and_get_id(self, name: Optional[str] = None, # Do not spawn objects with type environment as they should be already present in the simulator through the # multiverse description file (.muv file). - if not obj_type == ObjectType.ENVIRONMENT: + obj_type = pycrap.PhysicalObject if obj_type is None else obj_type + if not (issubclass(obj_type, pycrap.Location) or issubclass(obj_type, pycrap.Floor)): self.spawn_object(name, obj_type, pose) return self._update_object_id_name_maps_and_get_latest_id(name) - def spawn_object(self, name: str, object_type: ObjectType, pose: Pose) -> None: + def spawn_object(self, name: str, object_type: Type[PhysicalObject], pose: Pose) -> None: """ Spawn the object in the simulator. @@ -236,7 +239,7 @@ def spawn_object(self, name: str, object_type: ObjectType, pose: Pose) -> None: :param object_type: The type of the object. :param pose: The pose of the object. """ - if object_type == ObjectType.ROBOT: + if issubclass(object_type, pycrap.Robot): if self.conf.use_controller: self.spawn_robot_with_controller(name, pose) else: @@ -389,7 +392,7 @@ def get_multiple_link_poses(self, links: List[Link]) -> Dict[str, Pose]: return self._get_multiple_body_poses([link.name for link in links]) def get_object_pose(self, obj: Object) -> Pose: - if obj.has_type_environment(): + if obj.is_an_environment: return Pose() return self._get_body_pose(obj.name) @@ -401,17 +404,17 @@ def get_multiple_object_poses(self, objects: List[Object]) -> Dict[str, Pose]: :param objects: The list of objects. :return: The dictionary of object names and poses. """ - non_env_objects = [obj for obj in objects if not obj.has_type_environment()] + non_env_objects = [obj for obj in objects if not obj.is_an_environment] all_poses = self._get_multiple_body_poses([obj.name for obj in non_env_objects]) - all_poses.update({obj.name: Pose() for obj in objects if obj.has_type_environment()}) + all_poses.update({obj.name: Pose() for obj in objects if obj.is_an_environment}) return all_poses @validate_object_pose def reset_object_base_pose(self, obj: Object, pose: Pose) -> bool: - if obj.has_type_environment(): + if obj.is_an_environment: return False - if (obj.obj_type == ObjectType.ROBOT and + if (obj.is_a_robot and RobotDescription.current_robot_description.virtual_mobile_base_joints is not None): obj.set_mobile_robot_pose(pose) else: @@ -427,11 +430,11 @@ def reset_multiple_objects_base_poses(self, objects: Dict[Object, Pose]) -> None :param objects: The dictionary of objects and poses. """ for obj in objects.keys(): - if (obj.obj_type == ObjectType.ROBOT and + if (obj.is_a_robot and RobotDescription.current_robot_description.virtual_mobile_base_joints is not None): obj.set_mobile_robot_pose(objects[obj]) - objects = {obj: pose for obj, pose in objects.items() if obj.obj_type not in [ObjectType.ENVIRONMENT, - ObjectType.ROBOT]} + objects = {obj: pose for obj, pose in objects.items() + if not obj.is_a_robot and not obj.is_an_environment} if len(objects) > 0: self._set_multiple_body_poses({obj.name: pose for obj, pose in objects.items()}) @@ -506,7 +509,7 @@ def _remove_visual_object(self, obj_id: int) -> bool: return False def remove_object_from_simulator(self, obj: Object) -> bool: - if obj.obj_type != ObjectType.ENVIRONMENT: + if not obj.is_an_environment: self.writer.remove_body(obj.name) return True logwarn("Cannot remove environment objects") diff --git a/src/pycrap/objects.py b/src/pycrap/objects.py index db3c997d5..0eafb0496 100644 --- a/src/pycrap/objects.py +++ b/src/pycrap/objects.py @@ -41,6 +41,12 @@ class Milk(Food): """ +class Bread(Food): + """ + A type of food prepared from a dough of flour and water. + """ + + class Cutlery(PhysicalObject): """ Any implement, tool, or container used for serving or eating food. diff --git a/test/test_multiverse.py b/test/test_multiverse.py index bc20da075..b377f7bd6 100644 --- a/test/test_multiverse.py +++ b/test/test_multiverse.py @@ -7,8 +7,9 @@ from tf.transformations import quaternion_from_euler, quaternion_multiply from typing_extensions import Optional, List +import pycrap from pycram.datastructures.dataclasses import ContactPointsList, ContactPoint, AxisAlignedBoundingBox, Color -from pycram.datastructures.enums import ObjectType, Arms, JointType +from pycram.datastructures.enums import Arms, JointType from pycram.datastructures.pose import Pose from pycram.robot_description import RobotDescriptionManager from pycram.world_concepts.world_object import Object @@ -57,7 +58,7 @@ def tearDown(self): def test_load_generic_object(self): obj_desc = GenericObjectDescription('test_cube', [0, 0, 0], [0.1, 0.1, 0.1], color=Color(1, 0, 0, 1)) - obj = Object(obj_desc.name, ObjectType.GENERIC_OBJECT, description=obj_desc) + obj = Object(obj_desc.name, pycrap.PhysicalObject, description=obj_desc) self.assertIsInstance(obj, Object) self.assertTrue(obj in self.multiverse.objects) obj.set_position([1, 1, 0.1]) @@ -67,10 +68,7 @@ def test_load_generic_object(self): def test_save_and_restore_state(self): milk = self.spawn_milk([1, 1, 0.1]) robot = self.spawn_robot() - if "apartment" not in self.multiverse.get_object_names(): - apartment = Object("apartment", ObjectType.ENVIRONMENT, f"apartment.urdf") - else: - apartment = self.multiverse.get_object_by_name("apartment") + apartment = self.spawn_apartment() apartment.set_joint_position("cabinet10_drawer1_joint", 0.1) robot.attach(milk) all_object_attachments = {obj: obj.attachments.copy() for obj in self.multiverse.objects} @@ -92,7 +90,7 @@ def test_save_and_restore_state(self): self.assertTrue(apartment.get_joint_position("cabinet10_drawer1_joint") == 0.1) def test_spawn_xml_object(self): - bread = Object("bread_1", ObjectType.GENERIC_OBJECT, "bread_1.xml", pose=Pose([1, 1, 0.1])) + bread = Object("bread_1", pycrap.Bread, "bread_1.xml", pose=Pose([1, 1, 0.1])) self.assert_poses_are_equal(bread.get_pose(), Pose([1, 1, 0.1])) def test_get_axis_aligned_bounding_box_for_one_link_object(self): @@ -123,7 +121,7 @@ def test_get_axis_aligned_bounding_box_for_one_link_object(self): self.assertAlmostEqual(max_p_1[0] + position_shift, max_p_2[0], delta=0.001) def test_spawn_mesh_object(self): - milk = Object("milk", ObjectType.MILK, "milk.stl", pose=Pose([1, 1, 0.1])) + milk = Object("milk", pycrap.Milk, "milk.stl", pose=Pose([1, 1, 0.1])) self.assert_poses_are_equal(milk.get_pose(), Pose([1, 1, 0.1])) self.multiverse.simulate(0.2) contact_points = milk.contact_points() @@ -155,7 +153,7 @@ def test_get_images_for_target(self): milk_spawn_position = np.array(camera_front_facing_axis) * 0.5 orientation = camera_pose.to_transform(camera_frame).invert().rotation_as_list() milk = self.spawn_milk(milk_spawn_position.tolist(), orientation, frame=camera_frame) - _, depth, segmentation_mask = self.multiverse.get_images_for_target(milk.pose, camera_pose, plot=True) + _, depth, segmentation_mask = self.multiverse.get_images_for_target(milk.pose, camera_pose, plot=False) self.assertIsInstance(depth, np.ndarray) self.assertIsInstance(segmentation_mask, np.ndarray) self.assertTrue(depth.shape == (256, 256)) @@ -309,7 +307,7 @@ def step_robot_pose(self, robot, position_step, angle_step, num_steps): def test_get_environment_pose(self): if "apartment" not in self.multiverse.get_object_names(): - apartment = Object("apartment", ObjectType.ENVIRONMENT, f"apartment.urdf") + apartment = Object("apartment", pycrap.Apartment, f"apartment.urdf") else: apartment = self.multiverse.get_object_by_name("apartment") pose = apartment.get_pose() @@ -424,7 +422,7 @@ def test_get_rays(self): @staticmethod def spawn_big_bowl() -> Object: - big_bowl = Object("big_bowl", ObjectType.GENERIC_OBJECT, "BigBowl.obj", + big_bowl = Object("big_bowl", pycrap.Bowl, "BigBowl.obj", pose=Pose([2, 2, 0.1], [0, 0, 0, 1])) return big_bowl @@ -432,13 +430,13 @@ def spawn_big_bowl() -> Object: def spawn_milk(position: List, orientation: Optional[List] = None, frame="map") -> Object: if orientation is None: orientation = [0, 0, 0, 1] - milk = Object("milk_box", ObjectType.MILK, "milk_box.urdf", + milk = Object("milk_box", pycrap.Milk, "milk_box.xml", pose=Pose(position, orientation, frame=frame)) return milk def spawn_apartment(self) -> Object: if "apartment" not in self.multiverse.get_object_names(): - apartment = Object("apartment", ObjectType.ENVIRONMENT, f"apartment.urdf") + apartment = Object("apartment", pycrap.Apartment, f"apartment.urdf") else: apartment = self.multiverse.get_object_by_name("apartment") return apartment @@ -454,7 +452,7 @@ def spawn_robot(self, position: Optional[List[float]] = None, if self.multiverse.robot is None or replace: if self.multiverse.robot is not None: self.multiverse.robot.remove() - robot = Object(robot_name, ObjectType.ROBOT, f"{robot_name}.urdf", + robot = Object(robot_name, pycrap.Robot, f"{robot_name}.urdf", pose=Pose(position, orientation)) else: robot = self.multiverse.robot @@ -463,7 +461,7 @@ def spawn_robot(self, position: Optional[List[float]] = None, @staticmethod def spawn_cup(position: List) -> Object: - cup = Object("cup", ObjectType.GENERIC_OBJECT, "cup.xml", + cup = Object("cup", pycrap.Cup, "cup.xml", pose=Pose(position, [0, 0, 0, 1])) return cup From b4b400d55e0a47a8a4adbb7c195376e2d26c26bf Mon Sep 17 00:00:00 2001 From: A_Bassiouny Date: Mon, 11 Nov 2024 13:03:26 +0100 Subject: [PATCH 81/91] [NavigateAction] Add an option that asks if the joint states should be kept while navigating. --- src/pycram/external_interfaces/giskard.py | 11 +++++++++++ 1 file changed, 11 insertions(+) diff --git a/src/pycram/external_interfaces/giskard.py b/src/pycram/external_interfaces/giskard.py index c594660d6..3e0d11941 100644 --- a/src/pycram/external_interfaces/giskard.py +++ b/src/pycram/external_interfaces/giskard.py @@ -328,6 +328,17 @@ def set_joint_goal(goal_poses: Dict[str, float]) -> None: giskard_wrapper.set_joint_goal(goal_poses) +def set_joint_goal(goal_poses: Dict[str, float]) -> None: + """ + Takes a dictionary of joint position that should be achieved, the keys in the dictionary are the joint names and + values are the goal joint positions. + + :param goal_poses: Dictionary with joint names and position goals + """ + sync_worlds() + giskard_wrapper.set_joint_goal(goal_poses) + + @init_giskard_interface @thread_safe def achieve_cartesian_goal(goal_pose: Pose, tip_link: str, root_link: str, From ed85920002ce46e89dca96a10de2c84ea785ab46 Mon Sep 17 00:00:00 2001 From: A_Bassiouny Date: Sat, 30 Nov 2024 23:12:41 +0100 Subject: [PATCH 82/91] [PR2ProcessModulesPyCRAP] used pycrap in pr2 process modules. --- src/pycram/process_modules/pr2_process_modules.py | 9 ++++++--- 1 file changed, 6 insertions(+), 3 deletions(-) diff --git a/src/pycram/process_modules/pr2_process_modules.py b/src/pycram/process_modules/pr2_process_modules.py index 1e04d2924..a3c91f82c 100644 --- a/src/pycram/process_modules/pr2_process_modules.py +++ b/src/pycram/process_modules/pr2_process_modules.py @@ -6,9 +6,10 @@ import numpy as np import rospy +import pycrap from ..external_interfaces.move_base import query_pose_nav 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, \ @@ -203,6 +204,7 @@ def _execute(self, designator: MoveMotion) -> Any: if not World.current_world.robot.pose.almost_equal(designator.target, 0.05, 3): raise NavigationGoalNotReachedError(World.current_world.robot.pose, designator.target) + class Pr2MoveHeadReal(ProcessModule): """ Process module for the real robot to move that such that it looks at the given position. Uses the same calculation @@ -239,6 +241,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.name.lower()) @@ -250,8 +253,8 @@ 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, - use_monitor=World.current_world.conf.use_giskard_monitor) + 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) From aef6b45b43b3bed377964cf28cbcd9b28ddb0d97 Mon Sep 17 00:00:00 2001 From: Abdelrhman Bassiouny Date: Tue, 3 Dec 2024 18:41:34 +0100 Subject: [PATCH 83/91] [MultiverseFallschoolChanges] rebased on giskard_fallschool_changes to have a linear history. --- src/pycram/datastructures/dataclasses.py | 2 +- test/test_task_tree.py | 8 ++++---- 2 files changed, 5 insertions(+), 5 deletions(-) diff --git a/src/pycram/datastructures/dataclasses.py b/src/pycram/datastructures/dataclasses.py index 87288c6b3..128682342 100644 --- a/src/pycram/datastructures/dataclasses.py +++ b/src/pycram/datastructures/dataclasses.py @@ -3,7 +3,7 @@ import os from abc import ABC, abstractmethod from copy import deepcopy, copy -from dataclasses import dataclass, fields +from dataclasses import dataclass, fields, field import numpy as np import trimesh diff --git a/test/test_task_tree.py b/test/test_task_tree.py index a3a1a1cd6..d59b4dbf2 100644 --- a/test/test_task_tree.py +++ b/test/test_task_tree.py @@ -35,8 +35,8 @@ def test_tree_creation(self): # self.tearDownBulletWorld() tt = pycram.tasktree.task_tree - self.assertEqual(16, len(tt.root)) - self.assertEqual(11, len(tt.root.leaves)) + self.assertEqual(15, len(tt.root)) + self.assertEqual(10, len(tt.root.leaves)) # check that all nodes succeeded for node in anytree.PreOrderIter(tt.root): @@ -74,8 +74,8 @@ def test_simulated_tree(self): self.plan() tt = pycram.tasktree.task_tree - self.assertEqual(16, len(tt.root)) - self.assertEqual(11, len(tt.root.leaves)) + self.assertEqual(15, len(tt.root)) + self.assertEqual(10, len(tt.root.leaves)) self.assertEqual(len(pycram.tasktree.task_tree), 1) From 00979003eb1a9a1a992130d58d33f1041cfa6ec7 Mon Sep 17 00:00:00 2001 From: Abdelrhman Bassiouny Date: Thu, 5 Dec 2024 13:15:29 +0100 Subject: [PATCH 84/91] [MultiverseFallSchoolDemo] fixed some multiverse bugs. --- .../worlds/multiverse_communication/clients.py | 12 +++++++----- 1 file changed, 7 insertions(+), 5 deletions(-) diff --git a/src/pycram/worlds/multiverse_communication/clients.py b/src/pycram/worlds/multiverse_communication/clients.py index 5d1aa0742..d73686e9b 100644 --- a/src/pycram/worlds/multiverse_communication/clients.py +++ b/src/pycram/worlds/multiverse_communication/clients.py @@ -34,7 +34,8 @@ def __init__(self, name: str, port: int, is_prospection_world: bool = False, increase or decrease the wait time for the simulation. """ meta_data = MultiverseMetaData() - meta_data.simulation_name = (Conf.prospection_world_prefix if is_prospection_world else "belief_state") + name + meta_data.simulation_name = ((Conf.prospection_world_prefix if is_prospection_world else "belief_state") + + "_" + name) meta_data.world_name = Conf.prospection_world_prefix if is_prospection_world else "belief_state" self.is_prospection_world = is_prospection_world super().__init__(port=str(port), meta_data=meta_data) @@ -811,10 +812,11 @@ def get_contact_points(self, body_name: str) -> List[MultiverseContactPoint]: for contact_point in contact_data: contact_point_data = list(contact_point.split()) position_and_normal = list(map(float, contact_point_data[2:])) - contact_points.append(MultiverseContactPoint(contact_point_data[0], - contact_point_data[1], - position_and_normal[:3], - position_and_normal[3:])) + for i in range(0, len(position_and_normal), 6): + contact_points.append(MultiverseContactPoint(contact_point_data[0], + contact_point_data[1], + position_and_normal[i:i + 3], + position_and_normal[i + 3:i + 6])) return contact_points def pause_simulation(self) -> None: From 0f03bcd79351d80aa87ec0102c8533f3ac5a360b Mon Sep 17 00:00:00 2001 From: Abdelrhman Bassiouny Date: Thu, 5 Dec 2024 15:52:08 +0100 Subject: [PATCH 85/91] [MultiverseFallSchoolDemo] added optional argument grasps to CostmapLocation. Check for continuous joint when clipping. Fixed multiverse demo. Don't set link_state as it is not settable/needed. --- demos/pycram_multiverse_demo/demo.py | 25 ++++++++---- src/pycram/datastructures/pose.py | 2 +- src/pycram/datastructures/world.py | 11 +++-- src/pycram/description.py | 13 ++++++ src/pycram/designators/action_designator.py | 2 +- src/pycram/designators/location_designator.py | 40 +++++++++++++------ src/pycram/object_descriptors/urdf.py | 2 +- src/pycram/pose_generator_and_validator.py | 6 ++- src/pycram/world_concepts/constraints.py | 2 +- src/pycram/world_concepts/world_object.py | 6 +-- src/pycram/worlds/multiverse.py | 10 ++--- 11 files changed, 78 insertions(+), 41 deletions(-) diff --git a/demos/pycram_multiverse_demo/demo.py b/demos/pycram_multiverse_demo/demo.py index c12a1da7c..be70c752e 100644 --- a/demos/pycram_multiverse_demo/demo.py +++ b/demos/pycram_multiverse_demo/demo.py @@ -1,3 +1,6 @@ +from typing_extensions import Type + +import pycrap from pycram.datastructures.dataclasses import Color from pycram.datastructures.enums import ObjectType, Arms, Grasp from pycram.datastructures.pose import Pose @@ -9,10 +12,11 @@ from pycram.process_module import simulated_robot, with_simulated_robot from pycram.world_concepts.world_object import Object from pycram.worlds.multiverse import Multiverse +from pycrap import PhysicalObject @with_simulated_robot -def move_and_detect(obj_type: ObjectType, pick_pose: Pose): +def move_and_detect(obj_type: Type[PhysicalObject], pick_pose: Pose): NavigateAction(target_locations=[Pose([1.7, 2, 0])]).resolve().perform() LookAtAction(targets=[pick_pose]).resolve().perform() @@ -24,13 +28,13 @@ def move_and_detect(obj_type: ObjectType, pick_pose: Pose): world = Multiverse() extension = ObjectDescription.get_file_extension() -robot = Object('pr2', ObjectType.ROBOT, f'pr2{extension}', pose=Pose([1.3, 2, 0.01])) -apartment = Object("apartment", ObjectType.ENVIRONMENT, f"apartment{extension}") +robot = Object('pr2', pycrap.Robot, f'pr2{extension}', pose=Pose([1.3, 2, 0.01])) +apartment = Object("apartment", pycrap.Apartment, f"apartment{extension}") -milk = Object("milk", ObjectType.MILK, f"milk.stl", pose=Pose([2.4, 2, 1.02]), +milk = Object("milk", pycrap.Milk, f"milk.stl", pose=Pose([2.4, 2, 1.02]), color=Color(1, 0, 0, 1)) -spoon = Object("spoon", ObjectType.SPOON, "spoon.stl", pose=Pose([2.5, 2.2, 0.85]), +spoon = Object("spoon", pycrap.Spoon, "spoon.stl", pose=Pose([2.5, 2.2, 0.85]), color=Color(0, 0, 1, 1)) apartment.attach(spoon, 'cabinet10_drawer1') @@ -50,7 +54,7 @@ def move_and_detect(obj_type: ObjectType, pick_pose: Pose): milk_desig = DetectAction(BelieveObject(types=[milk.obj_type])).resolve().perform() - TransportAction(milk_desig, [Arms.LEFT], [Pose([2.4, 3, 1.02])]).resolve().perform() + TransportAction(milk_desig, [Pose([2.4, 3, 1.02])], [Arms.LEFT]).resolve().perform() # Find and navigate to the drawer containing the spoon handle_desig = ObjectPart(names=["cabinet10_drawer1_handle"], part_of=apartment_desig.resolve()) @@ -66,13 +70,20 @@ def move_and_detect(obj_type: ObjectType, pick_pose: Pose): # Detect and pickup the spoon LookAtAction([apartment.get_link_pose("cabinet10_drawer1_handle")]).resolve().perform() - spoon_desig = DetectAction(BelieveObject(types=[ObjectType.SPOON])).resolve().perform() + spoon_desig = DetectAction(BelieveObject(types=[pycrap.Spoon])).resolve().perform() pickup_arm = Arms.LEFT if drawer_open_loc.arms[0] == Arms.RIGHT else Arms.RIGHT + pick_up_loc = CostmapLocation(target=spoon_desig.pose, reachable_for=robot_desig.resolve(), + reachable_arm=pickup_arm, grasps=[Grasp.TOP]).resolve() + + NavigateAction([pick_up_loc.pose]).resolve().perform() + PickUpAction(spoon_desig, [pickup_arm], [Grasp.TOP]).resolve().perform() ParkArmsAction([Arms.LEFT if pickup_arm == Arms.LEFT else Arms.RIGHT]).resolve().perform() + NavigateAction([drawer_open_loc.pose]).resolve().perform() + CloseAction(object_designator_description=handle_desig, arms=[drawer_open_loc.arms[0]]).resolve().perform() ParkArmsAction([Arms.BOTH]).resolve().perform() diff --git a/src/pycram/datastructures/pose.py b/src/pycram/datastructures/pose.py index b5c634fc7..692eeae3a 100644 --- a/src/pycram/datastructures/pose.py +++ b/src/pycram/datastructures/pose.py @@ -311,7 +311,7 @@ def insert(self, session: sqlalchemy.orm.Session) -> ORMPose: return pose - def multiply_quaternions(self, quaternion: List) -> None: + def multiply_quaternion(self, quaternion: List) -> None: """ Multiply the quaternion of this Pose with the given quaternion, the result will be the new orientation of this Pose. diff --git a/src/pycram/datastructures/world.py b/src/pycram/datastructures/world.py index cfcec807d..ed5978cd0 100644 --- a/src/pycram/datastructures/world.py +++ b/src/pycram/datastructures/world.py @@ -22,7 +22,7 @@ CapsuleVisualShape, PlaneVisualShape, MeshVisualShape, ObjectState, WorldState, ClosestPointsList, ContactPointsList, VirtualMobileBaseJoints, RotatedBoundingBox) -from ..datastructures.enums import JointType, ObjectType, WorldMode, Arms +from ..datastructures.enums import JointType, WorldMode, Arms from ..datastructures.pose import Pose, Transform from ..datastructures.world_entity import StateEntity from ..failures import ProspectionObjectNotFound, WorldObjectNotFound @@ -382,7 +382,7 @@ def get_object_by_name(self, name: str) -> Optional[Object]: matching_objects = list(filter(lambda obj: obj.name == name, self.objects)) return matching_objects[0] if len(matching_objects) > 0 else None - def get_object_by_type(self, obj_type: ObjectType) -> List[Object]: + def get_object_by_type(self, obj_type: Type[PhysicalObject]) -> List[Object]: """ Return a list of all Objects which have the type 'obj_type'. @@ -526,7 +526,7 @@ def get_joint_position(self, joint: Joint) -> float: """ Wrapper for :meth:`_get_joint_position` that return 0.0 for a joint if it is in the ignore joints list. """ - if joint.object.obj_type == ObjectType.ROBOT and joint.name in self.robot_description.ignore_joints: + if joint.object.is_a_robot and joint.name in self.robot_description.ignore_joints: return 0.0 return self._get_joint_position(joint) @@ -787,7 +787,7 @@ def reset_joint_position(self, joint: Joint, joint_position: float) -> bool: """ Wrapper around :meth:`_reset_joint_position` that checks if the joint should be ignored. """ - if joint.object.obj_type == ObjectType.ROBOT and self.robot_description.ignore_joints: + if joint.object.is_a_robot and self.robot_description.ignore_joints: if joint.name in self.robot_description.ignore_joints: return True return self._reset_joint_position(joint, joint_position) @@ -837,7 +837,7 @@ def get_multiple_joint_positions(self, joints: List[Joint]) -> Dict[str, float]: """ Wrapper around :meth:`_get_multiple_joint_positions` that checks if any of the joints should be ignored. """ - filtered_joints = [joint for joint in joints if joint.object.obj_type != ObjectType.ROBOT or + filtered_joints = [joint for joint in joints if not joint.object.is_a_robot or joint.name not in self.robot_description.ignore_joints] joint_positions = self._get_multiple_joint_positions(filtered_joints) joint_positions.update({joint.name: 0.0 for joint in joints if joint not in filtered_joints}) @@ -1839,7 +1839,6 @@ def sync_objects_states(self) -> None: self.world.prospection_world.reset_multiple_objects_base_poses(obj_pose_dict) for obj, prospection_obj in self.object_to_prospection_object_map.items(): prospection_obj.set_attachments(obj.attachments) - prospection_obj.link_states = obj.link_states prospection_obj.joint_states = obj.joint_states def check_for_equal(self) -> bool: diff --git a/src/pycram/description.py b/src/pycram/description.py index 0a41fe434..328e4c0e1 100644 --- a/src/pycram/description.py +++ b/src/pycram/description.py @@ -350,8 +350,21 @@ def current_state(self) -> LinkState: @current_state.setter def current_state(self, link_state: LinkState) -> None: if self.current_state != link_state: + if not self.all_constraint_links_belong_to_same_world(link_state): + raise ValueError("All constraint links must belong to the same world, since the constraint ids" + "are unique to the world and cannot be transferred between worlds.") self.constraint_ids = link_state.constraint_ids + def all_constraint_links_belong_to_same_world(self, other: LinkState) -> bool: + """ + Check if all links belong to the same world as the links in the other link state. + + :param other: The state of the other link. + :return: True if all links belong to the same world, False otherwise. + """ + return all([link.world == other_link.world for link, other_link in zip(self.constraint_ids.keys(), + other.constraint_ids.keys())]) + def add_fixed_constraint_with_link(self, child_link: Self, child_to_parent_transform: Optional[Transform] = None) -> int: """ diff --git a/src/pycram/designators/action_designator.py b/src/pycram/designators/action_designator.py index 8c071ce11..5e7c3bfb1 100644 --- a/src/pycram/designators/action_designator.py +++ b/src/pycram/designators/action_designator.py @@ -298,7 +298,7 @@ def plan(self) -> None: adjusted_oTm = object.local_transformer.transform_pose(adjusted_pose, "map") # multiplying the orientation therefore "rotating" it, to get the correct orientation of the gripper - adjusted_oTm.multiply_quaternions(grasp) + adjusted_oTm.multiply_quaternion(grasp) # prepose depending on the gripper (its annoying we have to put pr2_1 here tbh arm_chain = RobotDescription.current_robot_description.get_arm_chain(self.arm) diff --git a/src/pycram/designators/location_designator.py b/src/pycram/designators/location_designator.py index d9f0152f6..3d069a75f 100644 --- a/src/pycram/designators/location_designator.py +++ b/src/pycram/designators/location_designator.py @@ -4,7 +4,7 @@ from .object_designator import ObjectDesignatorDescription, ObjectPart from ..costmaps import OccupancyCostmap, VisibilityCostmap, SemanticCostmap, GaussianCostmap -from ..datastructures.enums import JointType, Arms +from ..datastructures.enums import JointType, Arms, Grasp from ..datastructures.pose import Pose from ..datastructures.world import World, UseProspectionWorld from ..designator import DesignatorError, LocationDesignatorDescription @@ -107,7 +107,11 @@ class CostmapLocation(LocationDesignatorDescription): class Location(LocationDesignatorDescription.Location): reachable_arms: List[Arms] """ - List of arms with which the pose can be reached, is only used when the 'rechable_for' parameter is used + List of arms with which the pose can be reached, is only used when the 'reachable_for' parameter is used + """ + tried_grasps: List[Grasp] + """ + List of grasps that were tried to reach the pose """ def __init__(self, target: Union[Pose, ObjectDesignatorDescription.Object], @@ -116,7 +120,8 @@ def __init__(self, target: Union[Pose, ObjectDesignatorDescription.Object], reachable_arm: Optional[Arms] = None, prepose_distance: float = 0.03, check_collision_at_start: bool = True, - ignore_collision_with: Optional[List[Object]] = None): + ignore_collision_with: Optional[List[Object]] = None, + grasps: Optional[List[Grasp]] = None): """ Location designator that uses costmaps as base to calculate locations for complex constrains like reachable or visible. In case of reachable the resolved location contains a list of arms with which the location is reachable. @@ -128,6 +133,7 @@ def __init__(self, target: Union[Pose, ObjectDesignatorDescription.Object], :param prepose_distance: Distance to the target pose where the robot should be checked for reachability. :param check_collision_at_start: If True, the designator will check for collisions at the start pose. :param ignore_collision_with: List of objects that should be ignored for collision checking. + :param grasps: List of grasps that should be tried to reach the target pose """ super().__init__() self.target: Union[Pose, ObjectDesignatorDescription.Object] = target @@ -137,6 +143,7 @@ def __init__(self, target: Union[Pose, ObjectDesignatorDescription.Object], self.prepose_distance = prepose_distance self.check_collision_at_start = check_collision_at_start self.ignore_collision_with = ignore_collision_with if ignore_collision_with is not None else [] + self.grasps: List[Optional[Grasp]] = grasps if grasps is not None else [None] def ground(self) -> Location: """ @@ -199,6 +206,7 @@ def __iter__(self): continue res = True arms = None + found_grasps = [] if self.visible_for: visible_prospection_object = World.current_world.get_prospection_object_for_object(self.target.world_object) res = res and visibility_validator(maybe_pose, test_robot, visible_prospection_object, @@ -213,16 +221,24 @@ def __iter__(self): hand_links += description.end_effector.links allowed_collision = {test_robot: hand_links} allowed_collision.update({o: o.link_names for o in self.ignore_collision_with}) - valid, arms = reachability_validator(maybe_pose, test_robot, target_pose, - allowed_collision=allowed_collision, - arm=self.reachable_arm, - prepose_distance=self.prepose_distance) - if self.reachable_arm: - res = res and valid and self.reachable_arm in arms - else: - res = res and valid + for grasp in self.grasps: + target_pose_oriented = target_pose.copy() + if grasp is not None: + grasp_quaternion = RobotDescription.current_robot_description.grasps[grasp] + target_pose_oriented.multiply_quaternion(grasp_quaternion) + valid, arms = reachability_validator(maybe_pose, test_robot, target_pose_oriented, + allowed_collision=allowed_collision, + arm=self.reachable_arm, + prepose_distance=self.prepose_distance) + if self.reachable_arm: + res = res and valid and self.reachable_arm in arms + else: + res = res and valid + if res: + found_grasps.append(grasp) + break if res: - yield self.Location(maybe_pose, arms) + yield self.Location(maybe_pose, arms, found_grasps) class AccessingLocation(LocationDesignatorDescription): diff --git a/src/pycram/object_descriptors/urdf.py b/src/pycram/object_descriptors/urdf.py index dc0d7c728..bdb48b8f7 100644 --- a/src/pycram/object_descriptors/urdf.py +++ b/src/pycram/object_descriptors/urdf.py @@ -108,7 +108,7 @@ def name(self) -> str: @property def has_limits(self) -> bool: - return bool(self.parsed_description.limit) + return bool(self.parsed_description.limit) and not self.type == JointType.CONTINUOUS @property def type(self) -> JointType: diff --git a/src/pycram/pose_generator_and_validator.py b/src/pycram/pose_generator_and_validator.py index 080395fba..31c731c78 100644 --- a/src/pycram/pose_generator_and_validator.py +++ b/src/pycram/pose_generator_and_validator.py @@ -2,7 +2,7 @@ import tf from typing_extensions import Tuple, List, Union, Dict, Iterable, Optional -from .datastructures.enums import Arms +from .datastructures.enums import Arms, Grasp from .costmaps import Costmap from .datastructures.pose import Pose, Transform from .datastructures.world import World @@ -161,7 +161,7 @@ def reachability_validator(pose: Pose, target: Union[Object, Pose], prepose_distance: float = 0.03, allowed_collision: Dict[Object, List] = None, - arm: Optional[Arms] = None) -> Tuple[bool, List]: + arm: Optional[Arms] = None) -> Tuple[bool, List[Arms]]: """ This method validates if a target position is reachable for a pose candidate. This is done by asking the ik solver if there is a valid solution if the @@ -191,6 +191,8 @@ def reachability_validator(pose: Pose, # TODO Make orientation adhere to grasping orientation res = False arms = [] + found_grasps = [] + original_target = target for description in manipulator_descs: retract_target_pose = LocalTransformer().transform_pose(target, robot.get_link_tf_frame( description.end_effector.tool_frame)) diff --git a/src/pycram/world_concepts/constraints.py b/src/pycram/world_concepts/constraints.py index 8de36bda1..14449d302 100644 --- a/src/pycram/world_concepts/constraints.py +++ b/src/pycram/world_concepts/constraints.py @@ -253,7 +253,7 @@ def remove_constraint_if_exists(self) -> None: """ Remove the constraint between the parent and the child links if one exists. """ - if self.child_link in self.parent_link.constraint_ids: + if self.child_link in self.parent_link.constraint_ids.keys(): self.parent_link.remove_constraint_with_link(self.child_link) def get_inverse(self) -> 'Attachment': diff --git a/src/pycram/world_concepts/world_object.py b/src/pycram/world_concepts/world_object.py index c490122ce..034385126 100644 --- a/src/pycram/world_concepts/world_object.py +++ b/src/pycram/world_concepts/world_object.py @@ -1149,11 +1149,7 @@ def set_joint_position(self, joint_name: str, joint_position: float) -> None: :param joint_name: The name of the joint :param joint_position: The target pose for this joint """ - if (self.joints[joint_name].has_limits and - (not self.joints[joint_name].lower_limit <= joint_position <= self.joints[joint_name].upper_limit)): - joint_position = np.clip(joint_position, self.joints[joint_name].lower_limit, - self.joints[joint_name].upper_limit) - logwarn(f"Joint position for joint {joint_name} was clipped to the joint limits.") + self.clip_joint_positions_to_limits({joint_name: joint_position}) if self.world.reset_joint_position(self.joints[joint_name], joint_position): self._update_on_joint_position_change() diff --git a/src/pycram/worlds/multiverse.py b/src/pycram/worlds/multiverse.py index 794ddfc97..7ecbfb82e 100644 --- a/src/pycram/worlds/multiverse.py +++ b/src/pycram/worlds/multiverse.py @@ -55,12 +55,12 @@ class Multiverse(World): Add the MJCF description extension to the extension to description type mapping for the objects. """ - def __init__(self, is_prospection: Optional[bool] = False, + def __init__(self, is_prospection_world: Optional[bool] = False, clear_cache: bool = False): """ Initialize the Multiverse Socket and the PyCram World. - :param is_prospection: Whether the world is prospection or not. + :param is_prospection_world: Whether the world is prospection or not. :param clear_cache: Whether to clear the cache or not. """ @@ -68,11 +68,11 @@ def __init__(self, is_prospection: Optional[bool] = False, self.saved_simulator_states: Dict = {} self.make_sure_multiverse_resources_are_added(clear_cache=clear_cache) - self.simulation = self.conf.prospection_world_prefix if is_prospection else "belief_state" + self.simulation = self.conf.prospection_world_prefix if is_prospection_world else "belief_state" self.client_manager = MultiverseClientManager(self.conf.simulation_wait_time_factor) - self._init_clients(is_prospection=is_prospection) + self._init_clients(is_prospection=is_prospection_world) - World.__init__(self, WorldMode.DIRECT, is_prospection) + World.__init__(self, WorldMode.DIRECT, is_prospection_world) self._init_constraint_and_object_id_name_map_collections() From 7460f595fae02e7b7d7f3810365f789cf3754c41 Mon Sep 17 00:00:00 2001 From: Abdelrhman Bassiouny Date: Thu, 5 Dec 2024 16:07:35 +0100 Subject: [PATCH 86/91] [VizMarkerPublisher] use scale values of geometry if exists. --- src/pycram/ros_utils/viz_marker_publisher.py | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/src/pycram/ros_utils/viz_marker_publisher.py b/src/pycram/ros_utils/viz_marker_publisher.py index 280605257..613bfbebb 100644 --- a/src/pycram/ros_utils/viz_marker_publisher.py +++ b/src/pycram/ros_utils/viz_marker_publisher.py @@ -92,7 +92,10 @@ def _make_marker_array(self) -> MarkerArray: if isinstance(geom, MeshVisualShape): msg.type = Marker.MESH_RESOURCE msg.mesh_resource = "file://" + geom.file_name - msg.scale = Vector3(1, 1, 1) + if hasattr(geom, "scale") and geom.scale is not None: + msg.scale = Vector3(*geom.scale) + else: + msg.scale = Vector3(1, 1, 1) msg.mesh_use_embedded_materials = True elif isinstance(geom, CylinderVisualShape): msg.type = Marker.CYLINDER From 74a19e08c660f75c1a84a03ba40fcea3618b5ffc Mon Sep 17 00:00:00 2001 From: Abdelrhman Bassiouny Date: Thu, 5 Dec 2024 15:52:08 +0100 Subject: [PATCH 87/91] [MultiverseFallschoolDemo] making review changes. --- demos/pycram_multiverse_demo/demo.py | 2 +- .../demo_euROBIN_industrial_robotics.py | 7 ++- .../pycram_multiverse_demo/fallschool_demo.py | 19 +++--- .../pycram_multiverse_demo/testing_giskard.py | 11 ++-- scripts/test_notebook_examples.sh | 2 +- src/pycram/costmaps.py | 22 ++++--- src/pycram/datastructures/dataclasses.py | 21 ++++--- src/pycram/designators/action_designator.py | 15 +---- src/pycram/designators/location_designator.py | 6 +- src/pycram/failure_handling.py | 30 +++++++++- src/pycram/utils.py | 3 +- src/pycram/world_reasoning.py | 8 +-- .../multiverse_communication/clients.py | 17 +++--- src/pycrap/agent.py | 60 ++++++++++++++++++- src/pycrap/objects.py | 7 +++ test/test_costmaps.py | 16 +++++ 16 files changed, 178 insertions(+), 68 deletions(-) diff --git a/demos/pycram_multiverse_demo/demo.py b/demos/pycram_multiverse_demo/demo.py index be70c752e..3b57c69d2 100644 --- a/demos/pycram_multiverse_demo/demo.py +++ b/demos/pycram_multiverse_demo/demo.py @@ -52,7 +52,7 @@ def move_and_detect(obj_type: Type[PhysicalObject], pick_pose: Pose): LookAtAction(targets=[Pose([2.6, 2.15, 1])]).resolve().perform() - milk_desig = DetectAction(BelieveObject(types=[milk.obj_type])).resolve().perform() + milk_desig = DetectAction(BelieveObject(types=[pycrap.Milk])).resolve().perform() TransportAction(milk_desig, [Pose([2.4, 3, 1.02])], [Arms.LEFT]).resolve().perform() diff --git a/demos/pycram_multiverse_demo/demo_euROBIN_industrial_robotics.py b/demos/pycram_multiverse_demo/demo_euROBIN_industrial_robotics.py index 0fd3b6b78..67dbf38da 100644 --- a/demos/pycram_multiverse_demo/demo_euROBIN_industrial_robotics.py +++ b/demos/pycram_multiverse_demo/demo_euROBIN_industrial_robotics.py @@ -1,4 +1,5 @@ -from pycram.datastructures.enums import ObjectType, GripperState, Arms +import pycrap +from pycram.datastructures.enums import GripperState, Arms from pycram.datastructures.world import UseProspectionWorld from pycram.process_module import simulated_robot, real_robot from pycram.world_concepts.world_object import Object @@ -14,8 +15,8 @@ WorldStateUpdater(tf_topic="/tf", joint_state_topic="/real/ur5e/joint_states") # Load the robot and the gripper - robot = Object("ur5e", ObjectType.ROBOT, "universal_robot/ur5e/urdf/ur5e.urdf") - gripper = Object("gripper-2F-85", ObjectType.GRIPPER, "robotiq/gripper-2F-85/gripper-2F-85.urdf") + robot = Object("ur5e", pycrap.Robot, "universal_robot/ur5e/urdf/ur5e.urdf") + gripper = Object("gripper-2F-85", pycrap.Gripper, "robotiq/gripper-2F-85/gripper-2F-85.urdf") # Attach the gripper to the robot at the wrist_3_link with the correct pose wrist_3_tf_frame = robot.get_link_tf_frame("wrist_3_link") diff --git a/demos/pycram_multiverse_demo/fallschool_demo.py b/demos/pycram_multiverse_demo/fallschool_demo.py index 7828b0a2f..d60276976 100644 --- a/demos/pycram_multiverse_demo/fallschool_demo.py +++ b/demos/pycram_multiverse_demo/fallschool_demo.py @@ -3,9 +3,11 @@ import rospy from tf.transformations import quaternion_from_euler +from typing_extensions import Type +import pycrap from pycram.datastructures.dataclasses import Color -from pycram.datastructures.enums import ObjectType, Arms +from pycram.datastructures.enums import Arms from pycram.datastructures.pose import Pose from pycram.datastructures.world import UseProspectionWorld, World from pycram.designators.action_designator import ParkArmsAction, MoveTorsoAction, TransportAction, NavigateAction, \ @@ -17,14 +19,11 @@ from pycram.worlds.bullet_world import BulletWorld from pycram.worlds.multiverse import Multiverse from pycram.ros_utils.viz_marker_publisher import VizMarkerPublisher - - -rospy_logger = logging.getLogger('rosout') -rospy_logger.setLevel(logging.DEBUG) +from pycrap import PhysicalObject @with_simulated_robot -def move_and_detect(obj_type: ObjectType, pick_pose: Pose): +def move_and_detect(obj_type: Type[PhysicalObject], pick_pose: Pose): NavigateAction(target_locations=[Pose([1.7, 2, 0])]).resolve().perform() LookAtAction(targets=[pick_pose]).resolve().perform() @@ -45,11 +44,11 @@ def move_and_detect(obj_type: ObjectType, pick_pose: Pose): vis_publisher = None milk_path = "milk.xml" -robot = Object('pr2', ObjectType.ROBOT, f'pr2.urdf', pose=Pose([1.3, 2.6, 0.01])) +robot = Object('pr2', pycrap.Robot, f'pr2.urdf', pose=Pose([1.3, 2.6, 0.01])) WorldStateUpdater(tf_topic="/tf", joint_state_topic="/real/pr2/joint_states", update_rate=timedelta(seconds=2), world=world) -apartment = Object("apartment", ObjectType.ENVIRONMENT, f"apartment.urdf") -milk = Object("milk", ObjectType.MILK, milk_path, pose=Pose([0.4, 2.6, 1.34], +apartment = Object("apartment", pycrap.Apartment, f"apartment.urdf") +milk = Object("milk", pycrap.Milk, milk_path, pose=Pose([0.4, 2.6, 1.34], [1, 0, 0, 0]), color=Color(1, 0, 0, 1)) @@ -79,7 +78,7 @@ def move_and_detect(obj_type: ObjectType, pick_pose: Pose): milk_desig = DetectAction(BelieveObject(types=[milk.obj_type])).resolve().perform() - TransportAction(milk_desig, [Arms.LEFT], [Pose([2.4, 3, 1.02])]).resolve().perform() + TransportAction(milk_desig, [Pose([2.4, 3, 1.02])], [Arms.LEFT]).resolve().perform() ParkArmsAction([Arms.BOTH]).resolve().perform() diff --git a/demos/pycram_multiverse_demo/testing_giskard.py b/demos/pycram_multiverse_demo/testing_giskard.py index f5181efe4..78d80ae72 100644 --- a/demos/pycram_multiverse_demo/testing_giskard.py +++ b/demos/pycram_multiverse_demo/testing_giskard.py @@ -1,13 +1,17 @@ +import os + import actionlib from control_msgs.msg import GripperCommandGoal, GripperCommandAction from giskard_msgs.msg import WorldResult +from typing_extensions import Optional from giskardpy.python_interface.old_python_interface import OldGiskardWrapper as GiskardWrapper -import rospy from geometry_msgs.msg import PoseStamped, Point, Quaternion -from giskardpy.utils.logging import loginfo +from pycram.helper import find_multiverse_resources_path +import pycram.ros # this to start the ros node +from pycram.ros.logging import loginfo def spawn_urdf(name: str, urdf_path: str, pose: PoseStamped) -> WorldResult: @@ -116,9 +120,8 @@ def move_arm_tool(pose_stamped: PoseStamped, arm: str): if __name__ == '__main__': - rospy.init_node('test_giskard') - multiverse_resources = '/home/bassioun/Documents/repos/Multiverse/multiverse/resources/' + multiverse_resources = find_multiverse_resources_path() cached_dir = multiverse_resources + 'cached/' giskard = GiskardWrapper() diff --git a/scripts/test_notebook_examples.sh b/scripts/test_notebook_examples.sh index a83921afb..f381fa86a 100755 --- a/scripts/test_notebook_examples.sh +++ b/scripts/test_notebook_examples.sh @@ -1,6 +1,6 @@ #!/bin/bash source /opt/ros/noetic/setup.bash -source ~/cram_ws/devel/setup.bash +source ~/catkin_ws/devel/setup.bash roscd pycram/examples rm -rf tmp mkdir tmp diff --git a/src/pycram/costmaps.py b/src/pycram/costmaps.py index a7e049028..98dc6e85d 100644 --- a/src/pycram/costmaps.py +++ b/src/pycram/costmaps.py @@ -221,7 +221,7 @@ def merge(self, other_cm: Costmap) -> Costmap: merge = np.logical_and(self.map > 0, other_cm.map > 0) new_map[merge] = self.map[merge] * other_cm.map[merge] max_val = np.max(new_map) - if max_val > 0: + if max_val != 0: new_map = (new_map / np.max(new_map)).reshape((self.height, self.width)) else: new_map = new_map.reshape((self.height, self.width)) @@ -815,14 +815,21 @@ def __init__(self, obj: Object, link_name: str, resolution: float = 0.02, world: self.link: Link = obj.get_link(link_name) self.resolution: float = resolution self.origin: Pose = obj.get_link_pose(link_name) - self.height: Optional[int] = None - self.width: Optional[int] = None + self.height: int = 0 + self.width: int = 0 self.map: np.ndarray = [] self.generate_map() Costmap.__init__(self, resolution, self.height, self.width, self.origin, self.map) def get_edges_map(self, margin_in_meters: float, horizontal_only: bool = False) -> Costmap: + """ + Return a Costmap with only the edges of the original Costmap marked as possible positions. + + :param margin_in_meters: The edge thickness in meters that should be marked as possible positions. + :param horizontal_only: If True only the horizontal edges will be marked as possible positions. + :return: The modified Costmap. + """ mask = np.zeros(self.map.shape) edge_tolerance = int(margin_in_meters / self.resolution) mask[:edge_tolerance] = 1 @@ -844,12 +851,11 @@ def generate_map(self) -> None: def get_aabb_for_link(self) -> AxisAlignedBoundingBox: """ - - :return: The axis aligned bounding box (AABB) of the link provided when creating this costmap. To try and let - the AABB as close to the actual object as possible, the Object will be rotated such that the link will be in the - identity orientation. + :return: The original untransformed (doesn't take the current pose of the link into consideration, since only + the size is important here not the pose) axis aligned bounding box (AABB) of the link provided when creating + this costmap. """ - return self.link.get_axis_aligned_bounding_box(False) + return self.link.get_axis_aligned_bounding_box_from_geometry() class AlgebraicSemanticCostmap(SemanticCostmap): diff --git a/src/pycram/datastructures/dataclasses.py b/src/pycram/datastructures/dataclasses.py index 128682342..e083f42bf 100644 --- a/src/pycram/datastructures/dataclasses.py +++ b/src/pycram/datastructures/dataclasses.py @@ -173,6 +173,18 @@ def depth(self) -> float: @dataclass class AxisAlignedBoundingBox(BoundingBox): + @classmethod + def from_origin_and_half_extents(cls, origin: Point, half_extents: Point): + """ + Set the axis-aligned bounding box from the origin of the body and half the size. + + :param origin: The origin point + :param half_extents: The half size of the bounding box. + """ + min_point = [origin.x - half_extents.x, origin.y - half_extents.y, origin.z - half_extents.z] + max_point = [origin.x + half_extents.x, origin.y + half_extents.y, origin.z + half_extents.z] + return cls.from_min_max(min_point, max_point) + @classmethod def from_multiple_bounding_boxes(cls, bounding_boxes: List[AxisAlignedBoundingBox]) -> 'AxisAlignedBoundingBox': """ @@ -851,15 +863,6 @@ def get_axes(self) -> Dict[str, Point]: return {getattr(self, field.name).name: getattr(self, field.name).axes for field in fields(self)} -@dataclass -class MultiverseBoundingBox: - """ - Dataclass for storing the bounding box of a body in the Multiverse simulation. - """ - min_point: List[float] - max_point: List[float] - - @dataclass class MultiverseMetaData: """Meta data for the Multiverse Client, the simulation_name should be non-empty and unique for each simulation""" diff --git a/src/pycram/designators/action_designator.py b/src/pycram/designators/action_designator.py index 5e7c3bfb1..d3754c4e7 100644 --- a/src/pycram/designators/action_designator.py +++ b/src/pycram/designators/action_designator.py @@ -18,6 +18,7 @@ from ..datastructures.partial_designator import PartialDesignator from ..datastructures.property import GraspableProperty, ReachableProperty, GripperIsFreeProperty, SpaceIsFreeProperty, \ VisibleProperty +from ..failure_handling import try_action from ..knowledge.knowledge_engine import ReasoningInstance from ..local_transformer import LocalTransformer from ..failures import ObjectUnfetchable, ReachabilityFailure, NavigationGoalNotReachedError, PerceptionObjectNotFound, \ @@ -46,18 +47,6 @@ from ..orm.action_designator import Action as ORMAction from dataclasses import dataclass, field - -def try_action(action, exception, num_retries=3): - current_retry = 0 - result = None - while current_retry < num_retries: - try: - result = action.perform() - break - except exception: - current_retry += 1 - return result - # ---------------------------------------------------------------------------- # ---------------- Performables ---------------------------------------------- # ---------------------------------------------------------------------------- @@ -416,7 +405,7 @@ class NavigateActionPerformable(ActionAbstract): @with_tree def plan(self) -> None: motion_action = MoveMotion(self.target_location, self.keep_joint_states) - return try_action(motion_action, NavigationGoalNotReachedError) + return try_action(motion_action, failure_type=NavigationGoalNotReachedError).perform() diff --git a/src/pycram/designators/location_designator.py b/src/pycram/designators/location_designator.py index 3d069a75f..4ef7e5c76 100644 --- a/src/pycram/designators/location_designator.py +++ b/src/pycram/designators/location_designator.py @@ -13,7 +13,7 @@ from ..robot_description import RobotDescription from ..ros.logging import logdebug from ..world_concepts.world_object import Object, Link -from ..world_reasoning import link_pose_for_joint_config, contact, is_a_picked_object, robot_will_be_in_collision_at_pose +from ..world_reasoning import link_pose_for_joint_config, contact, is_held_object, prospect_robot_contact class Location(LocationDesignatorDescription): @@ -202,7 +202,7 @@ def __iter__(self): with UseProspectionWorld(): for maybe_pose in PoseGenerator(final_map, number_of_samples=600): if self.check_collision_at_start and (test_robot is not None): - if robot_will_be_in_collision_at_pose(test_robot, maybe_pose, self.ignore_collision_with): + if prospect_robot_contact(test_robot, maybe_pose, self.ignore_collision_with): continue res = True arms = None @@ -317,7 +317,7 @@ def __iter__(self) -> Location: for maybe_pose in PoseGenerator(final_map, number_of_samples=600, orientation_generator=lambda p, o: PoseGenerator.generate_orientation(p, half_pose)): - if robot_will_be_in_collision_at_pose(test_robot, maybe_pose): + if prospect_robot_contact(test_robot, maybe_pose): continue hand_links = [] for description in RobotDescription.current_robot_description.get_manipulator_chains(): diff --git a/src/pycram/failure_handling.py b/src/pycram/failure_handling.py index 1c53061a4..46376e684 100644 --- a/src/pycram/failure_handling.py +++ b/src/pycram/failure_handling.py @@ -1,8 +1,10 @@ +import logging + from .datastructures.enums import State -from .designator import DesignatorDescription +from .designator import DesignatorDescription, ActionDesignatorDescription from .failures import PlanFailure from threading import Lock -from typing_extensions import Union, Tuple, Any, List +from typing_extensions import Union, Tuple, Any, List, Optional, Type from .language import Language, Monitor @@ -15,7 +17,7 @@ class FailureHandling(Language): to be extended by subclasses that implement specific failure handling behaviors. """ - def __init__(self, designator_description: Union[DesignatorDescription, Monitor]): + def __init__(self, designator_description: Optional[Union[DesignatorDescription, Monitor]] = None): """ Initializes a new instance of the FailureHandling class. @@ -169,3 +171,25 @@ def flatten(result): if exception_type in self.recovery: self.recovery[exception_type].perform() return status, flatten(res) + + +def try_action(action: Any, failure_type: Type[Exception], max_tries: int = 3): + """ + A generic function to retry an action a certain number of times before giving up, with a specific failure type. + + :param action: The action to be performed, it must have a perform() method. + :param failure_type: The type of exception to catch. + :param max_tries: The maximum number of attempts to retry the action. Defaults to 3. + """ + current_retry = 0 + result = None + while current_retry < max_tries: + try: + result = action.perform() + break + except failure_type as e: + logging.debug(f"Caught exception {e} during action execution. Retrying...") + current_retry += 1 + if current_retry == max_tries: + logging.error(f"Failed to execute action {action} after {max_tries} retries.") + return result diff --git a/src/pycram/utils.py b/src/pycram/utils.py index bb8b8040c..d762f5d3f 100644 --- a/src/pycram/utils.py +++ b/src/pycram/utils.py @@ -162,7 +162,8 @@ def __exit__(self, *_): def adjust_camera_pose_based_on_target(cam_pose: Pose, target_pose: Pose, camera_description: CameraDescription) -> Pose: """ - Adjust the camera pose based on the target pose. + Adjust the given cam_pose orientation such that it is facing the target_pose, which partly depends on the + front_facing_axis of the that is defined in the camera_description. :param cam_pose: The camera pose. :param target_pose: The target pose. diff --git a/src/pycram/world_reasoning.py b/src/pycram/world_reasoning.py index c09dbe589..743c6f4de 100644 --- a/src/pycram/world_reasoning.py +++ b/src/pycram/world_reasoning.py @@ -63,8 +63,8 @@ def contact( return objects_are_in_contact -def robot_will_be_in_collision_at_pose(robot: Object, pose: Pose, - ignore_collision_with: Optional[List[Object]] = None) -> bool: +def prospect_robot_contact(robot: Object, pose: Pose, + ignore_collision_with: Optional[List[Object]] = None) -> bool: """ Check if the robot collides with any object in the world at the given pose. @@ -83,7 +83,7 @@ def robot_will_be_in_collision_at_pose(robot: Object, pose: Pose, if obj.name in ([prospection_robot.name, floor.name] + ignore): continue in_contact, contact_links = contact(prospection_robot, obj, return_links=True) - if in_contact and not is_a_picked_object(prospection_robot, obj, [links[0] for links in contact_links]): + if in_contact and not is_held_object(prospection_robot, obj, [links[0] for links in contact_links]): logdebug(f"Robot is in contact with {obj.name} in prospection: {obj.world.is_prospection_world}" f"at position {pose.position_as_list()} and z_angle {pose.z_angle}") return True @@ -92,7 +92,7 @@ def robot_will_be_in_collision_at_pose(robot: Object, pose: Pose, return False -def is_a_picked_object(robot: Object, obj: Object, robot_contact_links: List[Link]) -> bool: +def is_held_object(robot: Object, obj: Object, robot_contact_links: List[Link]) -> bool: """ Check if the object is picked by the robot. diff --git a/src/pycram/worlds/multiverse_communication/clients.py b/src/pycram/worlds/multiverse_communication/clients.py index d73686e9b..e02ee92aa 100644 --- a/src/pycram/worlds/multiverse_communication/clients.py +++ b/src/pycram/worlds/multiverse_communication/clients.py @@ -6,11 +6,13 @@ from time import time, sleep +from geometry_msgs.msg import Point from typing_extensions import List, Dict, Tuple, Optional, Callable, Union from .socket import MultiverseSocket, MultiverseMetaData from ...config.multiverse_conf import MultiverseConfig as Conf -from ...datastructures.dataclasses import RayResult, MultiverseContactPoint, MultiverseBoundingBox +from ...datastructures.dataclasses import RayResult, MultiverseContactPoint, \ + AxisAlignedBoundingBox from ...datastructures.enums import (MultiverseAPIName as API, MultiverseBodyProperty as BodyProperty, MultiverseProperty as Property) from ...datastructures.pose import Pose @@ -537,18 +539,19 @@ def __init__(self, name: str, port: int, simulation: str, is_prospection_world: self.wait: bool = False # Whether to wait after sending the API request. def get_body_bounding_box(self, body_name: str, - with_children: bool = False) -> Union[MultiverseBoundingBox, List[MultiverseBoundingBox]]: + with_children: bool = False) -> Union[AxisAlignedBoundingBox, List[AxisAlignedBoundingBox]]: """ Get the body bounding box from the multiverse server, they are with respect to the body's frame. """ bounding_boxes_data = self._get_bounding_box(body_name, with_children) - multiverse_bounding_boxes = [] + bounding_boxes = [] for bounding_box in bounding_boxes_data: - min_point, max_point = bounding_box[:3], bounding_box[3:] - multiverse_bounding_boxes.append(MultiverseBoundingBox(min_point, max_point)) + origin = Point(bounding_box[0], bounding_box[1], bounding_box[2]) + size = Point(bounding_box[3], bounding_box[4], bounding_box[5]) + bounding_boxes.append(AxisAlignedBoundingBox.from_origin_and_half_extents(origin, size)) if with_children: - return multiverse_bounding_boxes - return multiverse_bounding_boxes[0] + return bounding_boxes + return bounding_boxes[0] def _get_bounding_box(self, body_name: str, with_children: bool = False) -> List[List[float]]: """ diff --git a/src/pycrap/agent.py b/src/pycrap/agent.py index 26b666689..803f709c1 100644 --- a/src/pycrap/agent.py +++ b/src/pycrap/agent.py @@ -1,14 +1,72 @@ +from . import BodyPart from .base import PhysicalObject + +class AgentPart(BodyPart): + """ + An agent part is a part of an agent's body. + """ + + +class HumanPart(BodyPart): + """ + A human part is a part of a human's body. + """ + + +class RobotPart(AgentPart): + """ + A robot part is a part of a robot's body, could be a link or a set of links or a joint, etc. + """ + + class Agent(PhysicalObject): """ An agent is an entity that acts. """ + has_part = [AgentPart] + class Robot(Agent): """ A robot is a machine that can carry out a complex series of actions automatically. """ + has_part = [RobotPart] + class Human(Agent): - ... \ No newline at end of file + """ + A human is a physical and biological agent + """ + has_part = [HumanPart] + + +class PrehensileEffector(AgentPart): + """ + A prehensile effector is a part of an agent's body that is used for grasping. + """ + + +class Hand(PrehensileEffector): + """ + A hand is a prehensile effector that is used for grasping, it has a palm, fingers, and a thumb. + """ + + +class HumanHand(HumanPart, Hand): + """ + A human hand is a prehensile that is used for grasping. + """ + + +class EndEffector(RobotPart, PrehensileEffector): + """ + An end effector is a device or tool that is connected to the end of a robot arm, + that is designed to interact with and manipulate the environment. + """ + + +class Gripper(EndEffector): + """ + A gripper is a device that can grasp and hold objects. + """ diff --git a/src/pycrap/objects.py b/src/pycrap/objects.py index 0eafb0496..ab49ed20c 100644 --- a/src/pycrap/objects.py +++ b/src/pycrap/objects.py @@ -1,6 +1,13 @@ from .base import PhysicalObject +class BodyPart(PhysicalObject): + """ + A body part is a part of an object's body. + """ + ... + + class Container(PhysicalObject): """ Any object that can contain other objects. diff --git a/test/test_costmaps.py b/test/test_costmaps.py index 32452844a..0c0eef03c 100644 --- a/test/test_costmaps.py +++ b/test/test_costmaps.py @@ -61,6 +61,21 @@ def test_visualize(self): origin=Pose([0, 0, 0], [0, 0, 0, 1])) o.visualize() + def test_merge_costmap(self): + o = OccupancyCostmap(0.2, from_ros=False, size=200, resolution=0.02, + origin=Pose([0, 0, 0], [0, 0, 0, 1])) + o2 = OccupancyCostmap(0.2, from_ros=False, size=200, resolution=0.02, + origin=Pose([0, 0, 0], [0, 0, 0, 1])) + o3 = o + o2 + self.assertTrue(np.all(o.map == o3.map)) + o2.map[100:120, 100:120] = 0 + o3 = o + o2 + self.assertTrue(np.all(o3.map[100:120, 100:120] == 0)) + self.assertTrue(np.all(o3.map[0:100, 0:100] == o.map[0:100, 0:100])) + o2.map = np.zeros_like(o2.map) + o3 = o + o2 + self.assertTrue(np.all(o3.map == o2.map)) + class SemanticCostmapTestCase(BulletWorldTestCase): @@ -90,6 +105,7 @@ def test_iterate(self): self.assertTrue(costmap.valid_area.contains([sample.position.x, sample.position.y])) + class ProbabilisticCostmapTestCase(BulletWorldTestCase): origin = Pose([1.5, 1, 0], [0, 0, 0, 1]) From 368e90e4747951f169812d990c1e39cc0d42d443 Mon Sep 17 00:00:00 2001 From: Abdelrhman Bassiouny Date: Fri, 6 Dec 2024 19:08:46 +0100 Subject: [PATCH 88/91] [MultiverseFallschoolDemo] making review changes. --- src/pycram/description.py | 8 ++-- src/pycram/designators/action_designator.py | 2 +- src/pycram/designators/motion_designator.py | 13 +----- src/pycram/external_interfaces/giskard.py | 11 ----- src/pycram/failure_handling.py | 42 ++++++++++++++++--- .../knowledge_sources/facts_knowledge.py | 2 +- src/pycram/object_descriptors/mjcf.py | 37 ++++++++++++++-- src/pycram/process_module.py | 24 +++++------ .../process_modules/boxy_process_modules.py | 4 +- .../process_modules/pr2_process_modules.py | 7 ++-- .../robotiq_gripper_process_module.py | 11 ++--- src/pycram/robot_description.py | 4 +- .../ur5e_controlled_description.py | 19 +++++---- src/pycram/ros/data_types.py | 2 +- src/pycram/ros/publisher.py | 5 ++- src/pycram/world_reasoning.py | 2 +- src/pycram/worlds/multiverse.py | 2 +- test/test_bullet_world.py | 2 +- test/test_multiverse.py | 2 +- 19 files changed, 119 insertions(+), 80 deletions(-) diff --git a/src/pycram/description.py b/src/pycram/description.py index 328e4c0e1..10257a9ab 100644 --- a/src/pycram/description.py +++ b/src/pycram/description.py @@ -51,8 +51,9 @@ class LinkDescription(EntityDescription): A link description of an object. """ - def __init__(self, parsed_link_description: Any): + def __init__(self, parsed_link_description: Any, mesh_dir: Optional[str] = None): self.parsed_description = parsed_link_description + self.mesh_dir = mesh_dir @property @abstractmethod @@ -216,7 +217,7 @@ class Link(ObjectEntity, LinkDescription, ABC): def __init__(self, _id: int, link_description: LinkDescription, obj: Object): ObjectEntity.__init__(self, _id, obj) - LinkDescription.__init__(self, link_description.parsed_description) + LinkDescription.__init__(self, link_description.parsed_description, link_description.mesh_dir) self.local_transformer: LocalTransformer = LocalTransformer() self.constraint_ids: Dict[Link, int] = {} self._current_pose: Optional[Pose] = None @@ -287,8 +288,7 @@ def get_mesh_path(self, geometry: MeshVisualShape) -> str: :param geometry: The geometry for which the mesh path should be returned. :return: The path of the mesh file of this link if the geometry is a mesh. """ - mesh_filename = self.get_mesh_filename(geometry) - return self.world.cache_manager.look_for_file_in_data_dir(pathlib.Path(mesh_filename)) + return self.get_mesh_filename(geometry) def get_mesh_filename(self, geometry: MeshVisualShape) -> str: """ diff --git a/src/pycram/designators/action_designator.py b/src/pycram/designators/action_designator.py index d3754c4e7..3303ea7a6 100644 --- a/src/pycram/designators/action_designator.py +++ b/src/pycram/designators/action_designator.py @@ -405,7 +405,7 @@ class NavigateActionPerformable(ActionAbstract): @with_tree def plan(self) -> None: motion_action = MoveMotion(self.target_location, self.keep_joint_states) - return try_action(motion_action, failure_type=NavigationGoalNotReachedError).perform() + return try_action(motion_action, failure_type=NavigationGoalNotReachedError) diff --git a/src/pycram/designators/motion_designator.py b/src/pycram/designators/motion_designator.py index 33b97d52a..20071cddd 100644 --- a/src/pycram/designators/motion_designator.py +++ b/src/pycram/designators/motion_designator.py @@ -5,6 +5,7 @@ from pycrap import PhysicalObject, Location from .object_designator import ObjectDesignatorDescription, ObjectPart, RealObject from ..datastructures.enums import MovementType +from ..failure_handling import try_motion from ..failures import PerceptionObjectNotFound, ToolPoseNotReachedError from ..process_module import ProcessModuleManager from ..orm.motion_designator import (MoveMotion as ORMMoveMotion, @@ -21,18 +22,6 @@ from ..external_interfaces.robokudo import robokudo_found -def try_motion(motion, motion_designator_instance, exception, num_retries=3): - current_retry = 0 - result = None - while current_retry < num_retries: - try: - result = motion.execute(motion_designator_instance) - break - except exception: - current_retry += 1 - return result - - @dataclass class MoveMotion(BaseMotion): """ diff --git a/src/pycram/external_interfaces/giskard.py b/src/pycram/external_interfaces/giskard.py index 3e0d11941..c594660d6 100644 --- a/src/pycram/external_interfaces/giskard.py +++ b/src/pycram/external_interfaces/giskard.py @@ -328,17 +328,6 @@ def set_joint_goal(goal_poses: Dict[str, float]) -> None: giskard_wrapper.set_joint_goal(goal_poses) -def set_joint_goal(goal_poses: Dict[str, float]) -> None: - """ - Takes a dictionary of joint position that should be achieved, the keys in the dictionary are the joint names and - values are the goal joint positions. - - :param goal_poses: Dictionary with joint names and position goals - """ - sync_worlds() - giskard_wrapper.set_joint_goal(goal_poses) - - @init_giskard_interface @thread_safe def achieve_cartesian_goal(goal_pose: Pose, tip_link: str, root_link: str, diff --git a/src/pycram/failure_handling.py b/src/pycram/failure_handling.py index 46376e684..3e3f0a965 100644 --- a/src/pycram/failure_handling.py +++ b/src/pycram/failure_handling.py @@ -1,11 +1,12 @@ import logging from .datastructures.enums import State -from .designator import DesignatorDescription, ActionDesignatorDescription +from .designator import DesignatorDescription, ActionDesignatorDescription, BaseMotion from .failures import PlanFailure from threading import Lock -from typing_extensions import Union, Tuple, Any, List, Optional, Type +from typing_extensions import Union, Tuple, Any, List, Optional, Type, Callable from .language import Language, Monitor +from .process_module import ProcessModule class FailureHandling(Language): @@ -181,15 +182,46 @@ def try_action(action: Any, failure_type: Type[Exception], max_tries: int = 3): :param failure_type: The type of exception to catch. :param max_tries: The maximum number of attempts to retry the action. Defaults to 3. """ + return try_method(method=lambda: action.perform(), + failure_type=failure_type, + max_tries=max_tries, + name="action") + + +def try_motion(motion: ProcessModule, motion_designator_instance: BaseMotion, + failure_type: Type[Exception], max_tries: int = 3): + """ + A generic function to retry a motion a certain number of times before giving up, with a specific exception. + + :param motion: The motion to be executed. + :param motion_designator_instance: The instance of the motion designator that has the description of the motion. + :param failure_type: The type of exception to catch. + :param max_tries: The maximum number of attempts to retry the motion. + """ + return try_method(method=lambda: motion.execute(motion_designator_instance), + failure_type=failure_type, + max_tries=max_tries, + name="motion") + + +def try_method(method: Callable, failure_type: Type[Exception], max_tries: int = 3, name: str = "method"): + """ + A generic function to retry a method a certain number of times before giving up, with a specific exception. + + :param method: The method to be called. + :param failure_type: The type of exception to catch. + :param max_tries: The maximum number + :param name: The name of the method to be called. + """ current_retry = 0 result = None while current_retry < max_tries: try: - result = action.perform() + result = method() break except failure_type as e: - logging.debug(f"Caught exception {e} during action execution. Retrying...") + logging.debug(f"Caught exception {e} during {name} execution {method}. Retrying...") current_retry += 1 if current_retry == max_tries: - logging.error(f"Failed to execute action {action} after {max_tries} retries.") + logging.error(f"Failed to execute {name} {method} after {max_tries} retries.") return result diff --git a/src/pycram/knowledge/knowledge_sources/facts_knowledge.py b/src/pycram/knowledge/knowledge_sources/facts_knowledge.py index 367acaf9a..90fdd1a3d 100644 --- a/src/pycram/knowledge/knowledge_sources/facts_knowledge.py +++ b/src/pycram/knowledge/knowledge_sources/facts_knowledge.py @@ -124,7 +124,7 @@ def is_visible(self, object_designator: ObjectDesignatorDescription) -> Reasonin :param object_designator: The object in question :return: Reasoning result with the visibility of the object """ - cam_pose = World.robot.get_link_pose(RobotDescription.current_robot_description.get_camera_frame()) + cam_pose = World.robot.get_link_pose(RobotDescription.current_robot_description.get_camera_link()) return ReasoningResult(visible(object_designator.resolve().world_object, cam_pose)) def empty(self) -> ReasoningResult: diff --git a/src/pycram/object_descriptors/mjcf.py b/src/pycram/object_descriptors/mjcf.py index cc9b8e914..d9475d0b2 100644 --- a/src/pycram/object_descriptors/mjcf.py +++ b/src/pycram/object_descriptors/mjcf.py @@ -1,3 +1,4 @@ +import glob import os import pathlib from xml.etree import ElementTree as ET @@ -34,8 +35,9 @@ class LinkDescription(AbstractLinkDescription): A class that represents a link description of an object. """ - def __init__(self, mjcf_description: mjcf.Element): + def __init__(self, mjcf_description: mjcf.Element, mesh_dir: Optional[str] = None): super().__init__(mjcf_description) + self.mesh_dir = mesh_dir @property def geometry(self) -> Union[List[VisualShape], VisualShape, None]: @@ -50,8 +52,7 @@ def geometry(self) -> Union[List[VisualShape], VisualShape, None]: else: return [self._get_visual_shape(geom) for geom in all_geoms] - @staticmethod - def _get_visual_shape(mjcf_geometry) -> Union[VisualShape, None]: + def _get_visual_shape(self, mjcf_geometry) -> Union[VisualShape, None]: """ :param mjcf_geometry: The MJCFGeometry to get the visual shape for. :return: The VisualShape of the given MJCFGeometry object. @@ -64,9 +65,27 @@ def _get_visual_shape(mjcf_geometry) -> Union[VisualShape, None]: return SphereVisualShape(Color(), [0, 0, 0], mjcf_geometry.size[0]) if mjcf_geometry.type == MJCFGeomType.MESH.value: mesh_filename = mjcf_geometry.mesh.file.prefix + mjcf_geometry.mesh.file.extension + mesh_filename = self.look_for_file_in_mesh_dir(mesh_filename) return MeshVisualShape(Color(), [0, 0, 0], mjcf_geometry.mesh.scale, mesh_filename) return None + def look_for_file_in_mesh_dir(self, file_name: str) -> str: + """ + Look for a file in the mesh directory of the object. + + :param file_name: The name of the file. + :return: The path to the file. + """ + # search for the file in the mesh directory + if self.mesh_dir is not None: + if os.path.exists(os.path.join(self.mesh_dir, file_name)): + return os.path.join(self.mesh_dir, file_name) + else: + # use glob to search for the file in the mesh directory + files = glob.glob(os.path.join(self.mesh_dir, '**', file_name), recursive=True) + if len(files) > 0: + return files[0] + @property def origin(self) -> Union[Pose, None]: """ @@ -336,6 +355,14 @@ def __init__(self): self._links = None self._joints = None self.virtual_joint_names = [] + self._meshes_dir: Optional[str] = None + + @property + def mesh_dir(self) -> Optional[str]: + try: + return getattr(self.parsed_description.compiler, self.MESH_DIR_ATTR) + except AttributeError: + return None @property def child_map(self) -> Dict[str, List[Tuple[str, str]]]: @@ -464,6 +491,8 @@ def replace_relative_paths_with_absolute_paths(self, model_path: str) -> str: for rel_dir_attrib in [self.MESH_DIR_ATTR, self.TEXTURE_DIR_ATTR]: rel_dir = compiler.get(rel_dir_attrib) abs_dir = str(pathlib.Path(os.path.join(model_dir, rel_dir)).resolve()) + if rel_dir_attrib == self.MESH_DIR_ATTR: + self._meshes_dir = abs_dir compiler.set(rel_dir_attrib, abs_dir) return ET.tostring(root, encoding='unicode', method='xml') @@ -486,7 +515,7 @@ def links(self) -> List[LinkDescription]: :return: A list of link descriptions of this object. """ if self._links is None: - self._links = [LinkDescription(link) for link in self.parsed_description.find_all('body')] + self._links = [LinkDescription(link, self.mesh_dir) for link in self.parsed_description.find_all('body')] return self._links def get_root(self) -> str: diff --git a/src/pycram/process_module.py b/src/pycram/process_module.py index c696a68fc..0fe6d51f5 100644 --- a/src/pycram/process_module.py +++ b/src/pycram/process_module.py @@ -308,7 +308,7 @@ def get_manager() -> Union[ProcessModuleManager, None]: f", and no default process modules available") return None - def navigate(self) -> Type[ProcessModule]: + def navigate(self) -> ProcessModule: """ Get the Process Module for navigating the robot with respect to the :py:attr:`~ProcessModuleManager.execution_type` @@ -318,7 +318,7 @@ def navigate(self) -> Type[ProcessModule]: raise NotImplementedError( f"There are no Process Modules for '{inspect.currentframe().f_code.co_name}' for robot '{self.robot_name}'") - def pick_up(self) -> Type[ProcessModule]: + def pick_up(self) -> ProcessModule: """ Get the Process Module for picking up with respect to the :py:attr:`~ProcessModuleManager.execution_type` @@ -327,7 +327,7 @@ def pick_up(self) -> Type[ProcessModule]: raise NotImplementedError( f"There are no Process Modules for '{inspect.currentframe().f_code.co_name}' for robot '{self.robot_name}'") - def place(self) -> Type[ProcessModule]: + def place(self) -> ProcessModule: """ Get the Process Module for placing with respect to the :py:attr:`~ProcessModuleManager.execution_type` @@ -336,7 +336,7 @@ def place(self) -> Type[ProcessModule]: raise NotImplementedError( f"There are no Process Modules for '{inspect.currentframe().f_code.co_name}' for robot '{self.robot_name}'") - def looking(self) -> Type[ProcessModule]: + def looking(self) -> ProcessModule: """ Get the Process Module for looking at a point with respect to the :py:attr:`~ProcessModuleManager.execution_type` @@ -346,7 +346,7 @@ def looking(self) -> Type[ProcessModule]: raise NotImplementedError( f"There are no Process Modules for '{inspect.currentframe().f_code.co_name}' for robot '{self.robot_name}'") - def detecting(self) -> Type[ProcessModule]: + def detecting(self) -> ProcessModule: """ Get the Process Module for detecting an object with respect to the :py:attr:`~ProcessModuleManager.execution_type` @@ -356,7 +356,7 @@ def detecting(self) -> Type[ProcessModule]: raise NotImplementedError( f"There are no Process Modules for '{inspect.currentframe().f_code.co_name}' for robot '{self.robot_name}'") - def move_tcp(self) -> Type[ProcessModule]: + def move_tcp(self) -> ProcessModule: """ Get the Process Module for moving the Tool Center Point with respect to the :py:attr:`~ProcessModuleManager.execution_type` @@ -366,7 +366,7 @@ def move_tcp(self) -> Type[ProcessModule]: raise NotImplementedError( f"There are no Process Modules for '{inspect.currentframe().f_code.co_name}' for robot '{self.robot_name}'") - def move_arm_joints(self) -> Type[ProcessModule]: + def move_arm_joints(self) -> ProcessModule: """ Get the Process Module for moving the joints of the robot arm with respect to the :py:attr:`~ProcessModuleManager.execution_type` @@ -376,7 +376,7 @@ def move_arm_joints(self) -> Type[ProcessModule]: raise NotImplementedError( f"There are no Process Modules for '{inspect.currentframe().f_code.co_name}' for robot '{self.robot_name}'") - def world_state_detecting(self) -> Type[ProcessModule]: + def world_state_detecting(self) -> ProcessModule: """ Get the Process Module for detecting an object using the world state with respect to the :py:attr:`~ProcessModuleManager.execution_type` @@ -386,7 +386,7 @@ def world_state_detecting(self) -> Type[ProcessModule]: raise NotImplementedError( f"There are no Process Modules for '{inspect.currentframe().f_code.co_name}' for robot '{self.robot_name}'") - def move_joints(self) -> Type[ProcessModule]: + def move_joints(self) -> ProcessModule: """ Get the Process Module for moving any joint of the robot with respect to the :py:attr:`~ProcessModuleManager.execution_type` @@ -396,7 +396,7 @@ def move_joints(self) -> Type[ProcessModule]: raise NotImplementedError( f"There are no Process Modules for '{inspect.currentframe().f_code.co_name}' for robot '{self.robot_name}'") - def move_gripper(self) -> Type[ProcessModule]: + def move_gripper(self) -> ProcessModule: """ Get the Process Module for moving the gripper with respect to the :py:attr:`~ProcessModuleManager.execution_type` @@ -406,7 +406,7 @@ def move_gripper(self) -> Type[ProcessModule]: raise NotImplementedError( f"There are no Process Modules for '{inspect.currentframe().f_code.co_name}' for robot '{self.robot_name}'") - def open(self) -> Type[ProcessModule]: + def open(self) -> ProcessModule: """ Get the Process Module for opening drawers with respect to the :py:attr:`~ProcessModuleManager.execution_type` @@ -416,7 +416,7 @@ def open(self) -> Type[ProcessModule]: raise NotImplementedError( f"There are no Process Modules for '{inspect.currentframe().f_code.co_name}' for robot '{self.robot_name}'") - def close(self) -> Type[ProcessModule]: + def close(self) -> ProcessModule: """ Get the Process Module for closing drawers with respect to the :py:attr:`~ProcessModuleManager.execution_type` diff --git a/src/pycram/process_modules/boxy_process_modules.py b/src/pycram/process_modules/boxy_process_modules.py index 4cb8b6e89..e67bd53f2 100644 --- a/src/pycram/process_modules/boxy_process_modules.py +++ b/src/pycram/process_modules/boxy_process_modules.py @@ -128,13 +128,13 @@ def _execute(self, desig): robot = World.robot object_type = desig.object_type # Should be "wide_stereo_optical_frame" - cam_frame_name = RobotDescription.current_robot_description.get_camera_frame() + cam_link_name = RobotDescription.current_robot_description.get_camera_link() # should be [0, 0, 1] front_facing_axis = RobotDescription.current_robot_description.get_default_camera().front_facing_axis objects = World.current_world.get_object_by_type(object_type) for obj in objects: - if btr.visible(obj, robot.get_link_pose(cam_frame_name), front_facing_axis): + if btr.visible(obj, robot.get_link_pose(cam_link_name), front_facing_axis): return obj diff --git a/src/pycram/process_modules/pr2_process_modules.py b/src/pycram/process_modules/pr2_process_modules.py index a3c91f82c..bd34181ed 100644 --- a/src/pycram/process_modules/pr2_process_modules.py +++ b/src/pycram/process_modules/pr2_process_modules.py @@ -4,7 +4,6 @@ from .default_process_modules import DefaultDetectingReal, DefaultDetecting from .. import world_reasoning as btr import numpy as np -import rospy import pycrap from ..external_interfaces.move_base import query_pose_nav @@ -24,6 +23,7 @@ from ..ros.logging import logdebug from ..utils import _apply_ik from ..world_concepts.world_object import Object +from ..ros.data_types import Duration if TYPE_CHECKING: from ..designators.object_designator import ObjectDesignatorDescription @@ -36,7 +36,8 @@ try: from control_msgs.msg import GripperCommandGoal, GripperCommandAction except ImportError: - logwarn("control_msgs import failed") + if Multiverse is not None: + logwarn("Import for control_msgs for gripper in Multiverse failed") try: from pr2_controllers_msgs.msg import Pr2GripperCommandGoal, Pr2GripperCommandAction, Pr2 @@ -311,7 +312,7 @@ def feedback_callback(msg): loginfo("Waiting for action server") client.wait_for_server() client.send_goal(goal, active_cb=activate_callback, done_cb=done_callback, feedback_cb=feedback_callback) - wait = client.wait_for_result(rospy.Duration.from_sec(5)) + wait = client.wait_for_result(Duration(5)) # client.cancel_all_goals() diff --git a/src/pycram/process_modules/robotiq_gripper_process_module.py b/src/pycram/process_modules/robotiq_gripper_process_module.py index e685a685b..1400ddc8e 100644 --- a/src/pycram/process_modules/robotiq_gripper_process_module.py +++ b/src/pycram/process_modules/robotiq_gripper_process_module.py @@ -1,6 +1,5 @@ from threading import Lock -import rospy from std_msgs.msg import Float64 from typing_extensions import Any @@ -9,12 +8,8 @@ from ..datastructures.enums import GripperState, ExecutionType from ..designators.motion_designator import MoveGripperMotion from ..process_module import ProcessModule, ProcessModuleManager - - -GRIPPER_NAME = "gripper-2F-85" -GRIPPER_CMD_TOPIC = "/gripper_command" -OPEN_VALUE = 0.0 -CLOSE_VALUE = 255.0 +from ..ros.publisher import create_publisher +from ..robot_descriptions.ur5e_controlled_description import GRIPPER_NAME, GRIPPER_CMD_TOPIC, OPEN_VALUE, CLOSE_VALUE class RobotiqMoveGripperReal(ProcessModule): @@ -23,7 +18,7 @@ class RobotiqMoveGripperReal(ProcessModule): """ def _execute(self, designator: MoveGripperMotion) -> Any: value = OPEN_VALUE if designator.motion == GripperState.OPEN else CLOSE_VALUE - publisher = rospy.Publisher(GRIPPER_CMD_TOPIC, Float64, queue_size=10, latch=True) + publisher = create_publisher(GRIPPER_CMD_TOPIC, Float64, queue_size=10, latch=True) World.current_world.step(func=lambda: publisher.publish(Float64(value)), step_seconds=0.1) return True diff --git a/src/pycram/robot_description.py b/src/pycram/robot_description.py index 708bcd690..5b8ba2aab 100644 --- a/src/pycram/robot_description.py +++ b/src/pycram/robot_description.py @@ -250,13 +250,13 @@ def get_manipulator_chains(self) -> List[KinematicChainDescription]: result.append(chain) return result - def get_camera_frame(self) -> str: + def get_camera_frame(self, robot_object_name: str) -> str: """ Quick method to get the name of a link of a camera. Uses the first camera in the list of cameras. :return: A name of the link of a camera """ - return f"{self.name}/{self.get_camera_link()}" + return f"{robot_object_name}/{self.get_camera_link()}" def get_camera_link(self) -> str: """ diff --git a/src/pycram/robot_descriptions/ur5e_controlled_description.py b/src/pycram/robot_descriptions/ur5e_controlled_description.py index 4936f0135..403c5669a 100644 --- a/src/pycram/robot_descriptions/ur5e_controlled_description.py +++ b/src/pycram/robot_descriptions/ur5e_controlled_description.py @@ -8,15 +8,18 @@ from ..ros.logging import logwarn multiverse_resources = find_multiverse_resources_path() +ROBOT_NAME = "ur5e" +GRIPPER_NAME = "gripper-2F-85" +GRIPPER_CMD_TOPIC = "/gripper_command" +OPEN_VALUE = 0.0 +CLOSE_VALUE = 255.0 if multiverse_resources is None: logwarn("Could not initialize ur5e description as Multiverse resources path not found.") else: robot_relative_dir = "universal_robot" - robot_name = "ur5e" - gripper_name = "gripper-2F-85" - filename = get_robot_urdf_path_from_multiverse(robot_relative_dir, robot_name, resources_dir=multiverse_resources) - mjcf_filename = get_robot_mjcf_path(robot_relative_dir, robot_name, multiverse_resources=multiverse_resources) - ur5_description = RobotDescription(robot_name, "base_link", "", "", + filename = get_robot_urdf_path_from_multiverse(robot_relative_dir, ROBOT_NAME, resources_dir=multiverse_resources) + mjcf_filename = get_robot_mjcf_path(robot_relative_dir, ROBOT_NAME, multiverse_resources=multiverse_resources) + ur5_description = RobotDescription(ROBOT_NAME, "base_link", "", "", filename, mjcf_path=mjcf_filename) ################################## Arm ################################## @@ -34,11 +37,11 @@ ################################## Gripper ################################## gripper_relative_dir = "robotiq" - gripper_filename = get_robot_urdf_path_from_multiverse(gripper_relative_dir, gripper_name, + gripper_filename = get_robot_urdf_path_from_multiverse(gripper_relative_dir, GRIPPER_NAME, resources_dir=multiverse_resources) gripper_urdf_obj = URDFObject(gripper_filename) - gripper = EndEffectorDescription("gripper", gripper_name, "right_pad", - gripper_urdf_obj, gripper_object_name=gripper_name) + gripper = EndEffectorDescription("gripper", GRIPPER_NAME, "right_pad", + gripper_urdf_obj, gripper_object_name=GRIPPER_NAME) gripper.add_static_joint_states(GripperState.OPEN, {'right_driver_joint': 0.0, 'right_coupler_joint': 0.0, diff --git a/src/pycram/ros/data_types.py b/src/pycram/ros/data_types.py index 4b6956279..4cd21c027 100644 --- a/src/pycram/ros/data_types.py +++ b/src/pycram/ros/data_types.py @@ -5,7 +5,7 @@ def Time(time=0.0): return rospy.Time(time) def Duration(duration=0.0): - return rospy.Duration(duration) + return rospy.Duration.from_sec(duration) def Rate(rate): return rospy.Rate(rate) \ No newline at end of file diff --git a/src/pycram/ros/publisher.py b/src/pycram/ros/publisher.py index 533a7c89b..ae72dd164 100644 --- a/src/pycram/ros/publisher.py +++ b/src/pycram/ros/publisher.py @@ -1,4 +1,5 @@ import rospy -def create_publisher(topic, msg_type, queue_size=10) -> rospy.Publisher: - return rospy.Publisher(topic, msg_type, queue_size=queue_size) \ No newline at end of file + +def create_publisher(topic, msg_type, queue_size=10, latch: bool = False) -> rospy.Publisher: + return rospy.Publisher(topic, msg_type, queue_size=queue_size, latch=latch) diff --git a/src/pycram/world_reasoning.py b/src/pycram/world_reasoning.py index 743c6f4de..b87e7a073 100644 --- a/src/pycram/world_reasoning.py +++ b/src/pycram/world_reasoning.py @@ -128,7 +128,7 @@ def get_visible_objects( if front_facing_axis is None: front_facing_axis = RobotDescription.current_robot_description.get_default_camera().front_facing_axis - camera_frame = RobotDescription.current_robot_description.get_camera_frame() + camera_frame = RobotDescription.current_robot_description.get_camera_frame(World.robot.name) world_to_cam = camera_pose.to_transform(camera_frame) cam_to_point = Transform(list(np.multiply(front_facing_axis, 2)), [0, 0, 0, 1], camera_frame, diff --git a/src/pycram/worlds/multiverse.py b/src/pycram/worlds/multiverse.py index 7ecbfb82e..5e17c4899 100644 --- a/src/pycram/worlds/multiverse.py +++ b/src/pycram/worlds/multiverse.py @@ -178,7 +178,7 @@ def get_images_for_target(self, target_pose: Pose, Uses ray test to get the images for the target object. (target_pose is currently not used) """ camera_description = RobotDescription.current_robot_description.get_default_camera() - camera_frame = RobotDescription.current_robot_description.get_camera_frame() + camera_frame = RobotDescription.current_robot_description.get_camera_frame(World.robot.name) adjusted_cam_pose = adjust_camera_pose_based_on_target(cam_pose, target_pose, camera_description) return self.ray_test_utils.get_images_for_target(adjusted_cam_pose, camera_description, camera_frame, size, camera_min_distance, camera_max_distance, plot) diff --git a/test/test_bullet_world.py b/test/test_bullet_world.py index e9774138c..f749a7b01 100644 --- a/test/test_bullet_world.py +++ b/test/test_bullet_world.py @@ -228,7 +228,7 @@ def test_remove_all_text(self): class BulletWorldTestGUI(BulletWorldGUITestCase): def test_add_vis_axis(self): time.sleep(10) - self.world.add_vis_axis(self.robot.get_link_pose(RobotDescription.current_robot_description.get_camera_frame())) + self.world.add_vis_axis(self.robot.get_link_pose(RobotDescription.current_robot_description.get_camera_link())) self.assertTrue(len(self.world.vis_axis) == 1) self.world.remove_vis_axis() self.assertTrue(len(self.world.vis_axis) == 0) diff --git a/test/test_multiverse.py b/test/test_multiverse.py index b377f7bd6..8bb3cf711 100644 --- a/test/test_multiverse.py +++ b/test/test_multiverse.py @@ -148,7 +148,7 @@ def test_get_images_for_target(self): camera_description = self.multiverse.robot_description.get_default_camera() camera_link_name = camera_description.link_name camera_pose = robot.get_link_pose(camera_link_name) - camera_frame = self.multiverse.robot_description.get_camera_frame() + camera_frame = self.multiverse.robot_description.get_camera_frame(robot.name) camera_front_facing_axis = camera_description.front_facing_axis milk_spawn_position = np.array(camera_front_facing_axis) * 0.5 orientation = camera_pose.to_transform(camera_frame).invert().rotation_as_list() From c0c5847e264a12a2c6dd5dbcbcf9163a8a0598cb Mon Sep 17 00:00:00 2001 From: A_Bassiouny Date: Mon, 9 Dec 2024 22:17:03 +0100 Subject: [PATCH 89/91] [Accessing] Adjusted costmap of accessing location to remove locations that cannot be accessed when drawer is open. --- demos/pycram_bullet_world_demo/demo.py | 3 +- demos/pycram_multiverse_demo/demo.py | 33 +++++---- src/pycram/designators/action_designator.py | 51 ++++++++++---- src/pycram/designators/location_designator.py | 69 +++++++++++++++---- src/pycram/orm/action_designator.py | 3 + src/pycram/pose_generator_and_validator.py | 14 ++-- test/test_orm/test_orm.py | 4 +- 7 files changed, 123 insertions(+), 54 deletions(-) diff --git a/demos/pycram_bullet_world_demo/demo.py b/demos/pycram_bullet_world_demo/demo.py index 37c832344..96995c155 100644 --- a/demos/pycram_bullet_world_demo/demo.py +++ b/demos/pycram_bullet_world_demo/demo.py @@ -8,7 +8,7 @@ from pycram.object_descriptors.urdf import ObjectDescription from pycram.world_concepts.world_object import Object from pycram.datastructures.dataclasses import Color -from pycram.ros.viz_marker_publisher import VizMarkerPublisher +from pycram.ros_utils.viz_marker_publisher import VizMarkerPublisher from pycrap import Robot, Apartment, Milk, Cereal, Spoon, Bowl import numpy as np @@ -101,4 +101,5 @@ def move_and_detect(obj_type): ParkArmsAction([Arms.BOTH]).resolve().perform() + world.exit() diff --git a/demos/pycram_multiverse_demo/demo.py b/demos/pycram_multiverse_demo/demo.py index 3b57c69d2..65850ceea 100644 --- a/demos/pycram_multiverse_demo/demo.py +++ b/demos/pycram_multiverse_demo/demo.py @@ -2,39 +2,30 @@ import pycrap from pycram.datastructures.dataclasses import Color -from pycram.datastructures.enums import ObjectType, Arms, Grasp +from pycram.datastructures.enums import ObjectType, Arms, Grasp, DetectionTechnique from pycram.datastructures.pose import Pose from pycram.designators.action_designator import ParkArmsAction, MoveTorsoAction, TransportAction, NavigateAction, \ LookAtAction, DetectAction, OpenAction, PickUpAction, CloseAction, PlaceAction from pycram.designators.location_designator import CostmapLocation, AccessingLocation +from pycram.designators.motion_designator import MoveArmJointsMotion, MoveTCPMotion from pycram.designators.object_designator import BelieveObject, ObjectPart from pycram.object_descriptors.urdf import ObjectDescription from pycram.process_module import simulated_robot, with_simulated_robot +from pycram.robot_description import RobotDescription from pycram.world_concepts.world_object import Object from pycram.worlds.multiverse import Multiverse from pycrap import PhysicalObject -@with_simulated_robot -def move_and_detect(obj_type: Type[PhysicalObject], pick_pose: Pose): - NavigateAction(target_locations=[Pose([1.7, 2, 0])]).resolve().perform() - - LookAtAction(targets=[pick_pose]).resolve().perform() - - object_desig = DetectAction(BelieveObject(types=[obj_type])).resolve().perform() - - return object_desig - - world = Multiverse() extension = ObjectDescription.get_file_extension() robot = Object('pr2', pycrap.Robot, f'pr2{extension}', pose=Pose([1.3, 2, 0.01])) apartment = Object("apartment", pycrap.Apartment, f"apartment{extension}") -milk = Object("milk", pycrap.Milk, f"milk.stl", pose=Pose([2.4, 2, 1.02]), +milk = Object("milk", pycrap.Milk, f"milk.xml", pose=Pose([2.4, 2, 1.02]), color=Color(1, 0, 0, 1)) -spoon = Object("spoon", pycrap.Spoon, "spoon.stl", pose=Pose([2.5, 2.2, 0.85]), +spoon = Object("spoon", pycrap.Spoon, "spoon.xml", pose=Pose([2.5, 2.2, 0.85]), color=Color(0, 0, 1, 1)) apartment.attach(spoon, 'cabinet10_drawer1') @@ -52,7 +43,8 @@ def move_and_detect(obj_type: Type[PhysicalObject], pick_pose: Pose): LookAtAction(targets=[Pose([2.6, 2.15, 1])]).resolve().perform() - milk_desig = DetectAction(BelieveObject(types=[pycrap.Milk])).resolve().perform() + milk_desig = DetectAction(DetectionTechnique.TYPES, + object_designator_description=BelieveObject(types=[pycrap.Milk])).resolve().perform()[0] TransportAction(milk_desig, [Pose([2.4, 3, 1.02])], [Arms.LEFT]).resolve().perform() @@ -65,18 +57,25 @@ def move_and_detect(obj_type: Type[PhysicalObject], pick_pose: Pose): OpenAction(object_designator_description=handle_desig, arms=[drawer_open_loc.arms[0]]).resolve().perform() - spoon.detach(apartment) + + arm_ee = RobotDescription.current_robot_description.get_arm_chain(drawer_open_loc.arms[0]).get_tool_frame() + closing_arm_pose = robot.get_link_pose(arm_ee) # Detect and pickup the spoon + spoon.detach(apartment) LookAtAction([apartment.get_link_pose("cabinet10_drawer1_handle")]).resolve().perform() - spoon_desig = DetectAction(BelieveObject(types=[pycrap.Spoon])).resolve().perform() + spoon_desig = DetectAction(DetectionTechnique.TYPES, + object_designator_description=BelieveObject(types=[pycrap.Spoon])).resolve().perform()[0] + + ParkArmsAction([Arms.BOTH]).resolve().perform() pickup_arm = Arms.LEFT if drawer_open_loc.arms[0] == Arms.RIGHT else Arms.RIGHT pick_up_loc = CostmapLocation(target=spoon_desig.pose, reachable_for=robot_desig.resolve(), reachable_arm=pickup_arm, grasps=[Grasp.TOP]).resolve() NavigateAction([pick_up_loc.pose]).resolve().perform() + MoveTCPMotion(closing_arm_pose, drawer_open_loc.arms[0]).perform() PickUpAction(spoon_desig, [pickup_arm], [Grasp.TOP]).resolve().perform() diff --git a/src/pycram/designators/action_designator.py b/src/pycram/designators/action_designator.py index 3303ea7a6..8f868171e 100644 --- a/src/pycram/designators/action_designator.py +++ b/src/pycram/designators/action_designator.py @@ -527,11 +527,15 @@ class OpenActionPerformable(ActionAbstract): """ Arm that should be used for opening the container """ + grasping_prepose_distance: float + """ + The distance in meters the gripper should be at in the x-axis away from the handle. + """ orm_class: Type[ActionAbstract] = field(init=False, default=ORMOpenAction) @with_tree def plan(self) -> None: - GraspingActionPerformable(self.arm, self.object_designator).perform() + GraspingActionPerformable(self.arm, self.object_designator, self.grasping_prepose_distance).perform() OpeningMotion(self.object_designator, self.arm).perform() MoveGripperMotion(GripperState.OPEN, self.arm, allow_gripper_collision=True).perform() @@ -551,11 +555,15 @@ class CloseActionPerformable(ActionAbstract): """ Arm that should be used for closing """ + grasping_prepose_distance: float + """ + The distance in meters between the gripper and the handle before approaching to grasp. + """ orm_class: Type[ActionAbstract] = field(init=False, default=ORMCloseAction) @with_tree def plan(self) -> None: - GraspingActionPerformable(self.arm, self.object_designator).perform() + GraspingActionPerformable(self.arm, self.object_designator, self.grasping_prepose_distance).perform() ClosingMotion(self.object_designator, self.arm).perform() MoveGripperMotion(GripperState.OPEN, self.arm, allow_gripper_collision=True).perform() @@ -574,6 +582,10 @@ class GraspingActionPerformable(ActionAbstract): """ Object Designator for the object that should be grasped """ + prepose_distance: float + """ + The distance in meters the gripper should be at before grasping the object + """ orm_class: Type[ActionAbstract] = field(init=False, default=ORMGraspingAction) @with_tree @@ -589,7 +601,7 @@ def plan(self) -> None: World.robot.get_link_tf_frame(gripper_name)) pre_grasp = object_pose_in_gripper.copy() - pre_grasp.pose.position.x -= 0.1 + pre_grasp.pose.position.x -= self.prepose_distance MoveTCPMotion(pre_grasp, self.arm).perform() MoveGripperMotion(GripperState.OPEN, self.arm).perform() @@ -1146,16 +1158,19 @@ class OpenAction(ActionDesignatorDescription): performable_class = OpenActionPerformable - def __init__(self, object_designator_description: ObjectPart, arms: List[Arms] = None): + def __init__(self, object_designator_description: ObjectPart, arms: List[Arms] = None, + grasping_prepose_distance: float = 0.03): """ Moves the arm of the robot to open a container. :param object_designator_description: Object designator_description describing the handle that should be used to open :param arms: A list of possible arms that should be used + :param grasping_prepose_distance: The distance in meters between gripper and handle before approaching to grasp. """ super().__init__() self.object_designator_description: ObjectPart = object_designator_description self.arms: List[Arms] = arms + self.grasping_prepose_distance: float = grasping_prepose_distance self.knowledge_condition = GripperIsFreeProperty(self.arms) def ground(self) -> OpenActionPerformable: @@ -1165,7 +1180,8 @@ def ground(self) -> OpenActionPerformable: :return: A performable designator_description """ - return OpenActionPerformable(self.object_designator_description.resolve(), self.arms[0]) + return OpenActionPerformable(self.object_designator_description.resolve(), self.arms[0], + grasping_prepose_distance=self.grasping_prepose_distance) def __iter__(self) -> OpenActionPerformable: """ @@ -1174,7 +1190,8 @@ def __iter__(self) -> OpenActionPerformable: :return: A performable action designator_description """ ri = ReasoningInstance(self, - PartialDesignator(OpenActionPerformable, self.object_designator_description, self.arms)) + PartialDesignator(OpenActionPerformable, self.object_designator_description, self.arms, + self.grasping_prepose_distance)) for desig in ri: yield desig @@ -1188,16 +1205,20 @@ class CloseAction(ActionDesignatorDescription): performable_class = CloseActionPerformable - def __init__(self, object_designator_description: ObjectPart, arms: List[Arms] = None): + def __init__(self, object_designator_description: ObjectPart, arms: List[Arms] = None, + grasping_prepose_distance: float = 0.03): """ Attempts to close an open container :param object_designator_description: Object designator_description description of the handle that should be used :param arms: A list of possible arms to use + :param grasping_prepose_distance: The distance in meters between the gripper and the handle before approaching + to grasp. """ super().__init__() self.object_designator_description: ObjectPart = object_designator_description self.arms: List[Arms] = arms + self.grasping_prepose_distance: float = grasping_prepose_distance self.knowledge_condition = GripperIsFreeProperty(self.arms) def ground(self) -> CloseActionPerformable: @@ -1207,7 +1228,8 @@ def ground(self) -> CloseActionPerformable: :return: A performable designator_description """ - return CloseActionPerformable(self.object_designator_description.resolve(), self.arms[0]) + return CloseActionPerformable(self.object_designator_description.resolve(), self.arms[0], + self.grasping_prepose_distance) def __iter__(self) -> CloseActionPerformable: """ @@ -1216,7 +1238,8 @@ def __iter__(self) -> CloseActionPerformable: :yield: A performable fully parametrized Action designator """ ri = ReasoningInstance(self, - PartialDesignator(CloseActionPerformable, self.object_designator_description, self.arms)) + PartialDesignator(CloseActionPerformable, self.object_designator_description, self.arms, + self.grasping_prepose_distance)) for desig in ri: yield desig @@ -1228,17 +1251,20 @@ class GraspingAction(ActionDesignatorDescription): performable_class = GraspingActionPerformable - def __init__(self, object_description: Union[ObjectDesignatorDescription, ObjectPart], arms: List[Arms] = None): + def __init__(self, object_description: Union[ObjectDesignatorDescription, ObjectPart], arms: List[Arms] = None, + prepose_distance: float = 0.03): """ Will try to grasp the object described by the given description. Grasping is done by moving into a pre grasp position 10 cm before the object, opening the gripper, moving to the object and then closing the gripper. :param arms: List of Arms that should be used for grasping :param object_description: Description of the object that should be grasped + :param prepose_distance: The distance in meters between the gripper and the object before approaching to grasp. """ super().__init__() self.arms: List[Arms] = arms self.object_description: ObjectDesignatorDescription = object_description + self.prepose_distance: float = prepose_distance def ground(self) -> GraspingActionPerformable: """ @@ -1247,7 +1273,7 @@ def ground(self) -> GraspingActionPerformable: :return: A performable action designator_description that contains specific arguments """ - return GraspingActionPerformable(self.arms[0], self.object_description.resolve()) + return GraspingActionPerformable(self.arms[0], self.object_description.resolve(), self.prepose_distance) def __iter__(self) -> CloseActionPerformable: """ @@ -1257,6 +1283,7 @@ def __iter__(self) -> CloseActionPerformable: :yield: A fully parametrized Action designator """ ri = ReasoningInstance(self, - PartialDesignator(GraspingActionPerformable, self.object_description, self.arms)) + PartialDesignator(GraspingActionPerformable, self.object_description, self.arms, + self.prepose_distance)) for desig in ri: yield desig diff --git a/src/pycram/designators/location_designator.py b/src/pycram/designators/location_designator.py index 4ef7e5c76..3962bc685 100644 --- a/src/pycram/designators/location_designator.py +++ b/src/pycram/designators/location_designator.py @@ -1,9 +1,10 @@ import dataclasses +import numpy as np from typing_extensions import List, Union, Iterable, Optional, Callable from .object_designator import ObjectDesignatorDescription, ObjectPart -from ..costmaps import OccupancyCostmap, VisibilityCostmap, SemanticCostmap, GaussianCostmap +from ..costmaps import OccupancyCostmap, VisibilityCostmap, SemanticCostmap, GaussianCostmap, Costmap from ..datastructures.enums import JointType, Arms, Grasp from ..datastructures.pose import Pose from ..datastructures.world import World, UseProspectionWorld @@ -208,7 +209,8 @@ def __iter__(self): arms = None found_grasps = [] if self.visible_for: - visible_prospection_object = World.current_world.get_prospection_object_for_object(self.target.world_object) + visible_prospection_object = World.current_world.get_prospection_object_for_object( + self.target.world_object) res = res and visibility_validator(maybe_pose, test_robot, visible_prospection_object, World.current_world) if self.reachable_for: @@ -277,6 +279,36 @@ def ground(self) -> Location: """ return next(iter(self)) + @staticmethod + def adjust_map_for_drawer_opening(cost_map: Costmap, init_pose: Pose, goal_pose: Pose, + width: float = 0.2): + """ + Adjust the cost map for opening a drawer. This is done by removing all locations between the initial and final + pose of the drawer/container. + + :param cost_map: Costmap that should be adjusted. + :param init_pose: Pose of the drawer/container when it is fully closed. + :param goal_pose: Pose of the drawer/container when it is fully opened. + :param width: Width of the drawer/container. + """ + motion_vector = [goal_pose.position.x - init_pose.position.x, goal_pose.position.y - init_pose.position.y, + goal_pose.position.z - init_pose.position.z] + # remove locations between the initial and final pose + motion_vector_length = np.linalg.norm(motion_vector) + unit_motion_vector = np.array(motion_vector) / motion_vector_length + orthogonal_vector = np.array([unit_motion_vector[1], -unit_motion_vector[0], 0]) + orthogonal_vector /= np.linalg.norm(orthogonal_vector) + orthogonal_size = width + map_origin_idx = cost_map.map.shape[0] // 2, cost_map.map.shape[1] // 2 + for i in range(int(motion_vector_length / 0.02)): + for j in range(int(orthogonal_size / 0.02)): + idx = (int(map_origin_idx[0] + i * unit_motion_vector[0] + j * orthogonal_vector[0]), + int(map_origin_idx[1] + i * unit_motion_vector[1] + j * orthogonal_vector[1])) + cost_map.map[idx] = 0 + idx = (int(map_origin_idx[0] + i * unit_motion_vector[0] - j * orthogonal_vector[0]), + int(map_origin_idx[1] + i * unit_motion_vector[1] - j * orthogonal_vector[1])) + cost_map.map[idx] = 0 + def __iter__(self) -> Location: """ Creates poses from which the robot can open the drawer specified by the ObjectPart designator describing the @@ -285,16 +317,6 @@ def __iter__(self) -> Location: :yield: A location designator containing the pose and the arms that can be used. """ - # ground_pose = [[self.handle.part_pose[0][0], self.handle.part_pose[0][1], 0], self.handle.part_pose[1]] - ground_pose = Pose(self.handle.part_pose.position_as_list()) - ground_pose.position.z = 0 - occupancy = OccupancyCostmap(distance_to_obstacle=0.25, from_ros=False, size=200, resolution=0.02, - origin=ground_pose) - gaussian = GaussianCostmap(200, 15, 0.02, ground_pose) - - final_map = occupancy + gaussian - - test_robot = World.current_world.get_prospection_object_for_object(self.robot) # Find a Joint of type prismatic which is above the handle in the URDF tree container_joint = self.handle.world_object.find_joint_above_link(self.handle.name, JointType.PRISMATIC) @@ -313,12 +335,27 @@ def __iter__(self) -> Location: container_joint: self.handle.world_object.get_joint_limits(container_joint)[1] / 1.5}, self.handle.name) + ground_pose = Pose(self.handle.part_pose.position_as_list()) + ground_pose.position.z = 0 + occupancy = OccupancyCostmap(distance_to_obstacle=0.25, from_ros=False, size=200, resolution=0.02, + origin=ground_pose) + gaussian = GaussianCostmap(200, 15, 0.02, ground_pose) + + final_map = occupancy + gaussian + joint_type = self.handle.world_object.joints[container_joint].type + if joint_type == JointType.PRISMATIC: + self.adjust_map_for_drawer_opening(final_map, init_pose, goal_pose) + + test_robot = World.current_world.get_prospection_object_for_object(self.robot) + with UseProspectionWorld(): + orientation_generator = lambda p, o: PoseGenerator.generate_orientation(p, half_pose) for maybe_pose in PoseGenerator(final_map, number_of_samples=600, - orientation_generator=lambda p, o: PoseGenerator.generate_orientation(p, - half_pose)): + orientation_generator=orientation_generator): if prospect_robot_contact(test_robot, maybe_pose): + logdebug("Robot is initially in collision, skipping that pose") continue + logdebug("Robot is initially in a valid pose") hand_links = [] for description in RobotDescription.current_robot_description.get_manipulator_chains(): hand_links += description.end_effector.links @@ -328,6 +365,7 @@ def __iter__(self) -> Location: prepose_distance=self.prepose_distance) if valid_init: + logdebug(f"Found a valid init pose for accessing {self.handle.name} with arms {arms_init}") valid_goal, arms_goal = reachability_validator(maybe_pose, test_robot, goal_pose, allowed_collision={test_robot: hand_links}, prepose_distance=self.prepose_distance) @@ -335,6 +373,7 @@ def __iter__(self) -> Location: arms_list = list(set(arms_init).intersection(set(arms_goal))) if valid_goal and len(arms_list) > 0: + logdebug(f"Found a valid goal pose for accessing {self.handle.name} with arms {arms_list}") yield self.Location(maybe_pose, arms_list) @@ -389,7 +428,7 @@ def __iter__(self): self.sem_costmap = SemanticCostmap(self.part_of.world_object, self.link_name) if self.edges_only or self.horizontal_edges_only: self.sem_costmap = self.sem_costmap.get_edges_map(self.edge_size_in_meters, - horizontal_only=self.horizontal_edges_only) + horizontal_only=self.horizontal_edges_only) height_offset = 0 if self.for_object: min_p, max_p = self.for_object.world_object.get_axis_aligned_bounding_box().get_min_max_points() diff --git a/src/pycram/orm/action_designator.py b/src/pycram/orm/action_designator.py index 4f24c8cb0..0aff86afd 100644 --- a/src/pycram/orm/action_designator.py +++ b/src/pycram/orm/action_designator.py @@ -107,6 +107,7 @@ class OpenAction(ObjectMixin, Action): id: Mapped[int] = mapped_column(ForeignKey(f'{Action.__tablename__}.id'), primary_key=True, init=False) arm: Mapped[Arms] + grasping_prepose_distance: Mapped[float] class CloseAction(ObjectMixin, Action): @@ -114,6 +115,7 @@ class CloseAction(ObjectMixin, Action): id: Mapped[int] = mapped_column(ForeignKey(f'{Action.__tablename__}.id'), primary_key=True, init=False) arm: Mapped[Arms] + grasping_prepose_distance: Mapped[float] class GraspingAction(ObjectMixin, Action): @@ -121,6 +123,7 @@ class GraspingAction(ObjectMixin, Action): id: Mapped[int] = mapped_column(ForeignKey(f'{Action.__tablename__}.id'), primary_key=True, init=False) arm: Mapped[Arms] + prepose_distance: Mapped[float] class FaceAtAction(PoseMixin, Action): diff --git a/src/pycram/pose_generator_and_validator.py b/src/pycram/pose_generator_and_validator.py index 31c731c78..a6d55412c 100644 --- a/src/pycram/pose_generator_and_validator.py +++ b/src/pycram/pose_generator_and_validator.py @@ -10,6 +10,7 @@ from .failures import IKError from .local_transformer import LocalTransformer from .robot_description import RobotDescription +from .ros.logging import logdebug from .world_concepts.world_object import Object from .world_reasoning import contact @@ -181,8 +182,7 @@ def reachability_validator(pose: Pose, target = target.get_pose() robot.set_pose(pose) - # manipulator_descs = list( - # filter(lambda chain: isinstance(chain[1], ManipulatorDescription), robot_description.chains.items())) + if arm is not None: manipulator_descs = [RobotDescription.current_robot_description.get_arm_chain(arm)] else: @@ -191,8 +191,6 @@ def reachability_validator(pose: Pose, # TODO Make orientation adhere to grasping orientation res = False arms = [] - found_grasps = [] - original_target = target for description in manipulator_descs: retract_target_pose = LocalTransformer().transform_pose(target, robot.get_link_tf_frame( description.end_effector.tool_frame)) @@ -205,26 +203,28 @@ def reachability_validator(pose: Pose, tool_frame = description.end_effector.tool_frame # TODO Make orientation adhere to grasping orientation - in_contact = False joint_state_before_ik = robot.get_positions_of_all_joints() try: # test the possible solution and apply it to the robot pose, joint_states = request_ik(target, robot, joints, tool_frame) + logdebug(f"Robot {description.name} can reach the the target pose") robot.set_pose(pose) robot.set_multiple_joint_positions(joint_states) - # _apply_ik(robot, resp, joints) in_contact = collision_check(robot, allowed_collision) if not in_contact: # only check for retract pose if pose worked + logdebug("Robot is not in contact at target pose") pose, joint_states = request_ik(retract_target_pose, robot, joints, tool_frame) + logdebug(f"Robot {description.name} can reach retract pose") robot.set_pose(pose) robot.set_multiple_joint_positions(joint_states) - # _apply_ik(robot, resp, joints) in_contact = collision_check(robot, allowed_collision) if not in_contact: + logdebug("Robot is not in contact at retract pose") arms.append(description.arm_type) except IKError: + logdebug(f"Robot {description.name} cannot reach pose") pass finally: robot.set_multiple_joint_positions(joint_state_before_ik) diff --git a/test/test_orm/test_orm.py b/test/test_orm/test_orm.py index 1ef6815cb..a332e900e 100644 --- a/test/test_orm/test_orm.py +++ b/test/test_orm/test_orm.py @@ -289,8 +289,8 @@ def test_open_and_closeAction(self): ParkArmsActionPerformable(pycram.datastructures.enums.Arms.BOTH).perform() NavigateActionPerformable(Pose([1.81, 1.73, 0.0], [0.0, 0.0, 0.594, 0.804]), True).perform() - OpenActionPerformable(handle_desig, arm=Arms.LEFT).perform() - CloseActionPerformable(handle_desig, arm=Arms.LEFT).perform() + OpenActionPerformable(handle_desig, arm=Arms.LEFT, grasping_prepose_distance=0.03).perform() + CloseActionPerformable(handle_desig, arm=Arms.LEFT, grasping_prepose_distance=0.03).perform() pycram.orm.base.ProcessMetaData().description = "open_and_closeAction_test" pycram.tasktree.task_tree.root.insert(self.session) From 8b471bbc960fc4b9964468eddbe56fdceb1dc274 Mon Sep 17 00:00:00 2001 From: A_Bassiouny Date: Mon, 9 Dec 2024 22:17:24 +0100 Subject: [PATCH 90/91] [Logging] Corrected logger name in logdebug. --- src/pycram/ros/logging.py | 3 ++- test/test_logging.py | 7 +++++-- 2 files changed, 7 insertions(+), 3 deletions(-) diff --git a/src/pycram/ros/logging.py b/src/pycram/ros/logging.py index d7e59516b..213d911eb 100644 --- a/src/pycram/ros/logging.py +++ b/src/pycram/ros/logging.py @@ -75,7 +75,8 @@ def logerr(message: str): def logdebug(message: str): - rospy.logdebug(f"[{_get_caller_file_name()}:{_get_caller_method_line()}:{_get_caller_method_name()}] {message}") + rospy.logdebug(f"[{_get_caller_file_name()}:{_get_caller_method_line()}:{_get_caller_method_name()}] {message}", + logger_name=PYCRAM_LOGGER_NAME) def logwarn_once(message: str): diff --git a/test/test_logging.py b/test/test_logging.py index c03459266..4bf1adec7 100644 --- a/test/test_logging.py +++ b/test/test_logging.py @@ -1,5 +1,5 @@ from pycram.testing import BulletWorldTestCase -from pycram.ros.logging import set_logger_level, logwarn, logerr +from pycram.ros.logging import set_logger_level, logwarn, logerr, logdebug from pycram.datastructures.enums import LoggerLevel @@ -7,8 +7,11 @@ class TestLogging(BulletWorldTestCase): """Testcase for the pycram logging functions.""" def test_set_logger_level(self): - logwarn("This is a warning, it should not be printed") + set_logger_level(LoggerLevel.DEBUG) + logdebug("This is a debug message, it should be printed") + logwarn("This is a warning, it should be printed") logerr("This is an error, it should be printed") set_logger_level(LoggerLevel.ERROR) + logdebug("This is a debug message, it should not be printed") logwarn("This is a warning, it should not be printed") logerr("This is an error, it should be printed") From 6fbff056cba0c56358340d1a025c73cc6272b64e Mon Sep 17 00:00:00 2001 From: Abdelrhman Bassiouny Date: Tue, 10 Dec 2024 10:33:50 +0100 Subject: [PATCH 91/91] [Accessing] use costmap resolution. --- src/pycram/designators/location_designator.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/pycram/designators/location_designator.py b/src/pycram/designators/location_designator.py index 3962bc685..bbfc108c3 100644 --- a/src/pycram/designators/location_designator.py +++ b/src/pycram/designators/location_designator.py @@ -300,8 +300,8 @@ def adjust_map_for_drawer_opening(cost_map: Costmap, init_pose: Pose, goal_pose: orthogonal_vector /= np.linalg.norm(orthogonal_vector) orthogonal_size = width map_origin_idx = cost_map.map.shape[0] // 2, cost_map.map.shape[1] // 2 - for i in range(int(motion_vector_length / 0.02)): - for j in range(int(orthogonal_size / 0.02)): + for i in range(int(motion_vector_length / cost_map.resolution)): + for j in range(int(orthogonal_size / cost_map.resolution)): idx = (int(map_origin_idx[0] + i * unit_motion_vector[0] + j * orthogonal_vector[0]), int(map_origin_idx[1] + i * unit_motion_vector[1] + j * orthogonal_vector[1])) cost_map.map[idx] = 0