[PhysicalBody] minor review changes.

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)

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.
 

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