[MultiverseFallSchoolDemo] added optional argument grasps to CostmapL…

…ocation.

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

@@ -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.xml", 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", ObjectType.SPOON, "spoon.xml", 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')
 
@@ -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()

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.

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, PhysicalBody, WorldEntity
 from ..failures import ProspectionObjectNotFound, WorldObjectNotFound
@@ -353,7 +353,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'.
 
@@ -491,7 +491,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)
 
@@ -756,7 +756,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)
@@ -806,7 +806,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})
@@ -1817,7 +1817,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:

src/pycram/description.py

@@ -338,9 +338,22 @@ 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.body_state = link_state.body_state
             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:
         """

src/pycram/designators/action_designator.py

@@ -772,7 +772,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)

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):

src/pycram/object_descriptors/urdf.py

@@ -112,7 +112,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:

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))

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':

src/pycram/world_concepts/world_object.py

@@ -1180,11 +1180,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._set_attached_objects_poses()
 

src/pycram/worlds/multiverse.py

@@ -56,22 +56,22 @@ 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.
         """
 
         self.latest_save_id: Optional[int] = None
         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, mode=WorldMode.DIRECT, is_prospection=is_prospection)