Object dynamic state

config/world_conf.py

@@ -70,7 +70,7 @@ class WorldConfig:
      and the world synchronization.
     """
 
-    position_tolerance: float = 1e-2
+    position_tolerance: float = 1e-3
     orientation_tolerance: float = 10 * math.pi / 180
     prismatic_joint_position_tolerance: float = 1e-2
     revolute_joint_position_tolerance: float = 5 * math.pi / 180

demos/pycram_multiverse_demo/demo_euROBIN_industrial_robotics.py

@@ -32,3 +32,54 @@
 
 
     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)

examples/cram_plan_tutorial.md

@@ -88,6 +88,7 @@ def get_n_random_positions(pose_list, n=4, dist=0.5, random=True):
 ```
 
 ```python
+import pycrap
 from tf.transformations import quaternion_from_euler
 import pycrap
 from pycram.costmaps import SemanticCostmap

src/pycram/datastructures/dataclasses.py

@@ -17,6 +17,7 @@
     from ..description import Link
     from ..world_concepts.world_object import Object
     from ..world_concepts.constraints import Attachment
+    from .world_entity import PhysicalBody
 
 
 def get_point_as_list(point: Point) -> List[float]:
@@ -478,15 +479,78 @@ class State(ABC):
     pass
 
 
+@dataclass
+class PhysicalBodyState(State):
+    """
+    Dataclass for storing the state of a physical body.
+    """
+    pose: Pose
+    is_translating: bool
+    is_rotating: bool
+    velocity: List[float]
+    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'):
+        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:
+        """
+        Check if the pose of the object is almost equal to the pose of another object.
+
+        :param other: The state of the other object.
+        :return: True if the poses are almost equal, False otherwise.
+        """
+        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:
+        """
+        Check if the velocity of the object is almost equal to the velocity of another object.
+
+        :param other: The state of the other object.
+        :return: True if the velocities are almost equal, False otherwise.
+        """
+        if self.velocity is None or other.velocity is None:
+            return self.velocity == other.velocity
+        return (self.vector_is_almost_equal(self.velocity[:3], other.velocity[:3], self.acceptable_velocity_error[0])
+                and self.vector_is_almost_equal(self.velocity[3:], other.velocity[3:], self.acceptable_velocity_error[1]))
+
+    @staticmethod
+    def vector_is_almost_equal(vector1: List[float], vector2: List[float], acceptable_error: float) -> bool:
+        """
+        Check if the vector is almost equal to another vector.
+
+        :param vector1: The first vector.
+        :param vector2: The second vector.
+        :param acceptable_error: The acceptable error.
+        :return: True if the vectors are almost equal, False otherwise.
+        """
+        return np.all(np.array(vector1) - np.array(vector2) <= acceptable_error)
+
+    def __copy__(self):
+        return PhysicalBodyState(pose=self.pose.copy(),
+                                 is_translating=self.is_translating, is_rotating=self.is_rotating,
+                                 velocity=self.velocity.copy(),
+                                 acceptable_pose_error=deepcopy(self.acceptable_pose_error),
+                                 acceptable_velocity_error=deepcopy(self.acceptable_velocity_error))
+
+
 @dataclass
 class LinkState(State):
     """
     Dataclass for storing the state of a link.
     """
+    body_state: PhysicalBodyState
     constraint_ids: Dict[Link, int]
 
     def __eq__(self, other: 'LinkState'):
-        return self.all_constraints_exist(other) and self.all_constraints_are_equal(other)
+        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:
         """
@@ -509,7 +573,7 @@ def all_constraints_are_equal(self, other: 'LinkState') -> bool:
                                                                other.constraint_ids.values())])
 
     def __copy__(self):
-        return LinkState(constraint_ids=copy(self.constraint_ids))
+        return LinkState(copy(self.body_state), constraint_ids=copy(self.constraint_ids))
 
 
 @dataclass
@@ -533,26 +597,20 @@ class ObjectState(State):
     """
     Dataclass for storing the state of an object.
     """
-    pose: Pose
+    body_state: PhysicalBodyState
     attachments: Dict[Object, Attachment]
     link_states: Dict[int, LinkState]
     joint_states: Dict[int, JointState]
-    acceptable_pose_error: Tuple[float, float]
 
     def __eq__(self, other: 'ObjectState'):
-        return (self.pose_is_almost_equal(other)
+        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
                 and self.joint_states == other.joint_states)
 
-    def pose_is_almost_equal(self, other: 'ObjectState') -> bool:
-        """
-        Check if the pose of the object is almost equal to the pose of another object.
-
-        :param other: The state of the other object.
-        :return: True if the poses are almost equal, False otherwise.
-        """
-        return self.pose.almost_equal(other.pose, other.acceptable_pose_error[0], other.acceptable_pose_error[1])
+    @property
+    def pose(self) -> Pose:
+        return self.body_state.pose
 
     def all_attachments_exist(self, other: 'ObjectState') -> bool:
         """
@@ -640,20 +698,24 @@ class LateralFriction:
 @dataclass
 class ContactPoint:
     """
-    Dataclass for storing the information of a contact point between two objects.
+    Dataclass for storing the information of a contact point between two bodies.
     """
-    link_a: Link
-    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  # the contact normal vector on object b pointing towards object a
+    body_a: PhysicalBody
+    body_b: PhysicalBody
+    position_on_body_a: Optional[List[float]] = None
+    position_on_body_b: Optional[List[float]] = None
+    normal_on_body_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
 
+    @property
+    def normal(self) -> List[float]:
+        return self.normal_on_body_b
+
     def __str__(self):
-        return f"ContactPoint: {self.link_a.object.name} - {self.link_b.object.name}"
+        return f"ContactPoint: {self.body_a.name} - {self.body_b.name}"
 
     def __repr__(self):
         return self.__str__()
@@ -670,24 +732,24 @@ class ContactPointsList(list):
     A list of contact points.
     """
 
-    def get_links_that_got_removed(self, previous_points: 'ContactPointsList') -> List[Link]:
+    def get_bodies_that_got_removed(self, previous_points: 'ContactPointsList') -> List[Link]:
         """
-        Return the links that are not in the current points list but were in the initial points list.
+        Return the bodies that are not in the current points list but were in the initial points list.
 
         :param previous_points: The initial points list.
-        :return: A list of Link instances that represent the links that got removed.
+        :return: A list of bodies that got removed.
         """
-        initial_links_in_contact = previous_points.get_links_in_contact()
-        current_links_in_contact = self.get_links_in_contact()
-        return [link for link in initial_links_in_contact if link not in current_links_in_contact]
+        initial_bodies_in_contact = previous_points.get_bodies_in_contact()
+        current_bodies_in_contact = self.get_bodies_in_contact()
+        return [body for body in initial_bodies_in_contact if body not in current_bodies_in_contact]
 
-    def get_links_in_contact(self) -> List[Link]:
+    def get_bodies_in_contact(self) -> List[PhysicalBody]:
         """
-        Get the links in contact.
+        Get the bodies in contact.
 
-        :return: A list of Link instances that represent the links in contact.
+        :return: A list of bodies that are in contact.
         """
-        return [point.link_b for point in self]
+        return [point.body_b for point in self]
 
     def check_if_two_objects_are_in_contact(self, obj_a: Object, obj_b: Object) -> bool:
         """
@@ -697,8 +759,21 @@ def check_if_two_objects_are_in_contact(self, obj_a: Object, obj_b: Object) -> b
         :param obj_b: An instance of the Object class that represents the second object.
         :return: True if the objects are in contact, False otherwise.
         """
-        return (any([point.link_b.object == obj_b and point.link_a.object == obj_a for point in self]) or
-                any([point.link_a.object == obj_b and point.link_b.object == obj_a for point in self]))
+        return (any([self.is_body_in_object(point.body_b, obj_b)
+                     and self.is_body_in_object(point.body_a, obj_a) for point in self]) or
+                any([self.is_body_in_object(point.body_a, obj_b)
+                     and self.is_body_in_object(point.body_b, obj_a) for point in self]))
+
+    @staticmethod
+    def is_body_in_object(body: PhysicalBody, obj: Object) -> bool:
+        """
+        Check if the body belongs to the object.
+
+        :param body: The body.
+        :param obj: The object.
+        :return: True if the body belongs to the object, False otherwise.
+        """
+        return body in list(obj.links.values()) or body == obj
 
     def get_normals_of_object(self, obj: Object) -> List[List[float]]:
         """
@@ -715,16 +790,16 @@ def get_normals(self) -> List[List[float]]:
 
         :return: A list of float vectors that represent the normals of the contact points.
         """
-        return [point.normal_on_b for point in self]
+        return [point.normal_on_body_b for point in self]
 
-    def get_links_in_contact_of_object(self, obj: Object) -> List[Link]:
+    def get_links_in_contact_of_object(self, obj: Object) -> List[PhysicalBody]:
         """
         Get the links in contact of the object.
 
         :param obj: An instance of the Object class that represents the object.
         :return: A list of Link instances that represent the links in contact of the object.
         """
-        return [point.link_b for point in self if point.link_b.object == obj]
+        return [point.body_b for point in self if point.body_b.parent_entity == obj]
 
     def get_points_of_object(self, obj: Object) -> 'ContactPointsList':
         """
@@ -733,7 +808,25 @@ def get_points_of_object(self, obj: Object) -> 'ContactPointsList':
         :param obj: An instance of the Object class that represents the object that the points are related to.
         :return: A ContactPointsList instance that represents the contact points of the object.
         """
-        return ContactPointsList([point for point in self if point.link_b.object == obj])
+        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':
+        """
+        Get the points of the link.
+
+        :param link: An instance of the Link class that represents the link that the points are related to.
+        :return: A ContactPointsList instance that represents the contact points of the link.
+        """
+        return self.get_points_of_body(link)
+
+    def get_points_of_body(self, body: PhysicalBody) -> 'ContactPointsList':
+        """
+        Get the points of the body.
+
+        :param body: An instance of the PhysicalBody class that represents the body that the points are related to.
+        :return: A ContactPointsList instance that represents the contact points of the body.
+        """
+        return ContactPointsList([point for point in self if body == point.body_b])
 
     def get_objects_that_got_removed(self, previous_points: 'ContactPointsList') -> List[Object]:
         """
@@ -780,7 +873,7 @@ def get_objects_that_have_points(self) -> List[Object]:
 
         :return: A list of Object instances that represent the objects that have points in the list.
         """
-        return list({point.link_b.object for point in self})
+        return list({point.body_b.parent_entity for point in self})
 
     def __str__(self):
         return f"ContactPointsList: {', '.join([point.__str__() for point in self])}"
@@ -833,6 +926,7 @@ class VirtualMobileBaseJoints:
     """
     Dataclass for storing the names, types and axes of the virtual mobile base joints of a mobile robot.
     """
+
     translation_x: Optional[VirtualJoint] = VirtualJoint(VirtualMobileBaseJointName.LINEAR_X.value,
                                                          JointType.PRISMATIC,
                                                          Point(1, 0, 0))

src/pycram/datastructures/mixins.py

@@ -19,4 +19,4 @@ class HasConcept:
     """
 
     def __init__(self):
-        self.ontology_individual = self.ontology_concept()
+        self.ontology_individual = self.ontology_concept()