From 5da450f3576a36493f37768d597228b2babe5d75 Mon Sep 17 00:00:00 2001 From: Abdelrhman Bassiouny Date: Thu, 19 Dec 2024 11:08:59 +0100 Subject: [PATCH] [PhysicalBody] minor review changes. --- .../demo_euROBIN_industrial_robotics.py | 57 +------------------ src/pycram/datastructures/dataclasses.py | 51 ++++++++--------- src/pycram/datastructures/world_entity.py | 4 +- 3 files changed, 29 insertions(+), 83 deletions(-) diff --git a/demos/pycram_multiverse_demo/demo_euROBIN_industrial_robotics.py b/demos/pycram_multiverse_demo/demo_euROBIN_industrial_robotics.py index 44272a030..7269caea3 100644 --- a/demos/pycram_multiverse_demo/demo_euROBIN_industrial_robotics.py +++ b/demos/pycram_multiverse_demo/demo_euROBIN_industrial_robotics.py @@ -1,7 +1,6 @@ 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.datastructures.enums import GripperState +from pycram.process_module import real_robot from pycram.world_concepts.world_object import Object from pycram.datastructures.pose import Pose from pycram.worlds.multiverse import Multiverse @@ -30,56 +29,4 @@ with real_robot: SetGripperAction(robot_arms, [GripperState.CLOSE]).resolve().perform() - world.exit() -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]) -} - - -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) - - 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") - 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/src/pycram/datastructures/dataclasses.py b/src/pycram/datastructures/dataclasses.py index 7a63668f5..9d5578c51 100644 --- a/src/pycram/datastructures/dataclasses.py +++ b/src/pycram/datastructures/dataclasses.py @@ -187,7 +187,7 @@ def from_origin_and_half_extents(cls, origin: Point, half_extents: Point): return cls.from_min_max(min_point, max_point) @classmethod - def from_multiple_bounding_boxes(cls, bounding_boxes: List[AxisAlignedBoundingBox]) -> 'AxisAlignedBoundingBox': + def from_multiple_bounding_boxes(cls, bounding_boxes: List[AxisAlignedBoundingBox]) -> AxisAlignedBoundingBox: """ Set the axis-aligned bounding box from multiple axis-aligned bounding boxes. @@ -201,7 +201,7 @@ def from_multiple_bounding_boxes(cls, bounding_boxes: List[AxisAlignedBoundingBo max_z = max([box.max_z for box in bounding_boxes]) return cls(min_x, min_y, min_z, max_x, max_y, max_z) - def get_transformed_box(self, transform: Transform) -> 'AxisAlignedBoundingBox': + def get_transformed_box(self, transform: Transform) -> AxisAlignedBoundingBox: """ Apply a transformation to the axis-aligned bounding box and return the transformed axis-aligned bounding box. @@ -250,7 +250,7 @@ def from_min_max(cls, min_point: Sequence[float], max_point: Sequence[float], tr @classmethod def from_axis_aligned_bounding_box(cls, axis_aligned_bounding_box: AxisAlignedBoundingBox, - transform: Transform) -> 'RotatedBoundingBox': + transform: Transform) -> RotatedBoundingBox: """ Set the rotated bounding box from an axis-aligned bounding box and a transformation. @@ -491,14 +491,14 @@ class PhysicalBodyState(State): acceptable_pose_error: Tuple[float, float] = (0.001, 0.001) acceptable_velocity_error: Tuple[float, float] = (0.001, 0.001) - def __eq__(self, other: 'PhysicalBodyState'): + def __eq__(self, other: PhysicalBodyState): return (self.pose_is_almost_equal(other) and self.is_translating == other.is_translating and self.is_rotating == other.is_rotating and self.velocity_is_almost_equal(other) ) - def pose_is_almost_equal(self, other: 'PhysicalBodyState') -> bool: + def pose_is_almost_equal(self, other: PhysicalBodyState) -> bool: """ Check if the pose of the object is almost equal to the pose of another object. @@ -507,7 +507,7 @@ def pose_is_almost_equal(self, other: 'PhysicalBodyState') -> bool: """ return self.pose.almost_equal(other.pose, other.acceptable_pose_error[0], other.acceptable_pose_error[1]) - def velocity_is_almost_equal(self, other: 'PhysicalBodyState') -> bool: + def velocity_is_almost_equal(self, other: PhysicalBodyState) -> bool: """ Check if the velocity of the object is almost equal to the velocity of another object. @@ -547,12 +547,12 @@ class LinkState(State): body_state: PhysicalBodyState constraint_ids: Dict[Link, int] - def __eq__(self, other: 'LinkState'): + def __eq__(self, other: LinkState): return (self.body_state == other.body_state and self.all_constraints_exist(other) and self.all_constraints_are_equal(other)) - def all_constraints_exist(self, other: 'LinkState') -> bool: + def all_constraints_exist(self, other: LinkState) -> bool: """ Check if all constraints exist in the other link state. @@ -562,7 +562,7 @@ def all_constraints_exist(self, other: 'LinkState') -> bool: return (all([cid_k in other.constraint_ids.keys() for cid_k in self.constraint_ids.keys()]) and len(self.constraint_ids.keys()) == len(other.constraint_ids.keys())) - def all_constraints_are_equal(self, other: 'LinkState') -> bool: + def all_constraints_are_equal(self, other: LinkState) -> bool: """ Check if all constraints are equal to the ones in the other link state. @@ -584,7 +584,7 @@ class JointState(State): position: float acceptable_error: float - def __eq__(self, other: 'JointState'): + def __eq__(self, other: JointState): error = calculate_joint_position_error(self.position, other.position) return is_error_acceptable(error, other.acceptable_error) @@ -602,7 +602,7 @@ class ObjectState(State): link_states: Dict[int, LinkState] joint_states: Dict[int, JointState] - def __eq__(self, other: 'ObjectState'): + def __eq__(self, other: ObjectState): return (self.body_state == other.body_state and self.all_attachments_exist(other) and self.all_attachments_are_equal(other) and self.link_states == other.link_states @@ -612,7 +612,7 @@ def __eq__(self, other: 'ObjectState'): def pose(self) -> Pose: return self.body_state.pose - def all_attachments_exist(self, other: 'ObjectState') -> bool: + def all_attachments_exist(self, other: ObjectState) -> bool: """ Check if all attachments exist in the other object state. @@ -622,7 +622,7 @@ def all_attachments_exist(self, other: 'ObjectState') -> bool: return (all([att_k in other.attachments.keys() for att_k in self.attachments.keys()]) and len(self.attachments.keys()) == len(other.attachments.keys())) - def all_attachments_are_equal(self, other: 'ObjectState') -> bool: + def all_attachments_are_equal(self, other: ObjectState) -> bool: """ Check if all attachments are equal to the ones in the other object state. @@ -632,10 +632,9 @@ def all_attachments_are_equal(self, other: 'ObjectState') -> bool: return all([att == other_att for att, other_att in zip(self.attachments.values(), other.attachments.values())]) def __copy__(self): - return ObjectState(pose=self.pose.copy(), attachments=copy(self.attachments), + return ObjectState(body_state=self.body_state.copy(), attachments=copy(self.attachments), link_states=copy(self.link_states), - joint_states=copy(self.joint_states), - acceptable_pose_error=deepcopy(self.acceptable_pose_error)) + joint_states=copy(self.joint_states)) @dataclass @@ -646,11 +645,11 @@ class WorldState(State): object_states: Dict[str, ObjectState] simulator_state_id: Optional[int] = None - def __eq__(self, other: 'WorldState'): + def __eq__(self, other: WorldState): return (self.simulator_state_is_equal(other) and self.all_objects_exist(other) and self.all_objects_states_are_equal(other)) - def simulator_state_is_equal(self, other: 'WorldState') -> bool: + def simulator_state_is_equal(self, other: WorldState) -> bool: """ Check if the simulator state is equal to the simulator state of another world state. @@ -659,7 +658,7 @@ def simulator_state_is_equal(self, other: 'WorldState') -> bool: """ return self.simulator_state_id == other.simulator_state_id - def all_objects_exist(self, other: 'WorldState') -> bool: + def all_objects_exist(self, other: WorldState) -> bool: """ Check if all objects exist in the other world state. @@ -669,7 +668,7 @@ def all_objects_exist(self, other: 'WorldState') -> bool: return (all([obj_name in other.object_states.keys() for obj_name in self.object_states.keys()]) and len(self.object_states.keys()) == len(other.object_states.keys())) - def all_objects_states_are_equal(self, other: 'WorldState') -> bool: + def all_objects_states_are_equal(self, other: WorldState) -> bool: """ Check if all object states are equal to the ones in the other world state. @@ -732,7 +731,7 @@ class ContactPointsList(list): A list of contact points. """ - def get_bodies_that_got_removed(self, previous_points: 'ContactPointsList') -> List[Link]: + def get_bodies_that_got_removed(self, previous_points: ContactPointsList) -> List[PhysicalBody]: """ Return the bodies that are not in the current points list but were in the initial points list. @@ -801,7 +800,7 @@ def get_links_in_contact_of_object(self, obj: Object) -> List[PhysicalBody]: """ return [point.body_b for point in self if point.body_b.parent_entity == obj] - def get_points_of_object(self, obj: Object) -> 'ContactPointsList': + def get_points_of_object(self, obj: Object) -> ContactPointsList: """ Get the points of the object. @@ -810,7 +809,7 @@ def get_points_of_object(self, obj: Object) -> 'ContactPointsList': """ return ContactPointsList([point for point in self if self.is_body_in_object(point.body_b, obj)]) - def get_points_of_link(self, link: Link) -> 'ContactPointsList': + def get_points_of_link(self, link: Link) -> ContactPointsList: """ Get the points of the link. @@ -819,7 +818,7 @@ def get_points_of_link(self, link: Link) -> 'ContactPointsList': """ return self.get_points_of_body(link) - def get_points_of_body(self, body: PhysicalBody) -> 'ContactPointsList': + def get_points_of_body(self, body: PhysicalBody) -> ContactPointsList: """ Get the points of the body. @@ -828,7 +827,7 @@ def get_points_of_body(self, body: PhysicalBody) -> 'ContactPointsList': """ return ContactPointsList([point for point in self if body == point.body_b]) - def get_objects_that_got_removed(self, previous_points: 'ContactPointsList') -> List[Object]: + def get_objects_that_got_removed(self, previous_points: ContactPointsList) -> List[Object]: """ Return the object that is not in the current points list but was in the initial points list. @@ -839,7 +838,7 @@ def get_objects_that_got_removed(self, previous_points: 'ContactPointsList') -> current_objects_in_contact = self.get_objects_that_have_points() return [obj for obj in initial_objects_in_contact if obj not in current_objects_in_contact] - def get_new_objects(self, previous_points: 'ContactPointsList') -> List[Object]: + def get_new_objects(self, previous_points: ContactPointsList) -> List[Object]: """ Return the object that is not in the initial points list but is in the current points list. diff --git a/src/pycram/datastructures/world_entity.py b/src/pycram/datastructures/world_entity.py index 1a1bf9296..fe9756bf2 100644 --- a/src/pycram/datastructures/world_entity.py +++ b/src/pycram/datastructures/world_entity.py @@ -325,7 +325,7 @@ def contact_points(self) -> ContactPointsList: """ return self.world.get_body_contact_points(self) - def get_contact_points_with_body(self, body: 'PhysicalBody') -> ContactPointsList: + def get_contact_points_with_body(self, body: PhysicalBody) -> ContactPointsList: """ :param body: The body to get the contact points with. :return: The contact points of this body with the given body. @@ -340,7 +340,7 @@ def closest_points(self, max_distance: float) -> ClosestPointsList: """ return self.world.get_body_closest_points(self, max_distance) - def get_closest_points_with_body(self, body: 'PhysicalBody', max_distance: float) -> ClosestPointsList: + def get_closest_points_with_body(self, body: PhysicalBody, max_distance: float) -> ClosestPointsList: """ :param body: The body to get the points with. :param max_distance: The maximum distance to consider a body as close, only points closer than or equal to this