First addition of multi-robot plans

src/pycram/costmaps.py

@@ -13,6 +13,7 @@
 from random_events.interval import Interval, reals, closed_open, closed
 from random_events.product_algebra import Event, SimpleEvent
 from random_events.variable import Continuous
+from .multirobot import RobotManager
 from .tf_transformations import quaternion_from_matrix, quaternion_from_euler
 from typing_extensions import Tuple, List, Optional, Iterator
 
@@ -462,10 +463,10 @@ def _create_from_world(self, size: int, resolution: float) -> np.ndarray:
             while r_t is None:
                 r_t = World.current_world.ray_test_batch(n[:, 0], n[:, 1], num_threads=0)
             j += len(n)
-            if World.robot:
-                attached_objs_id = [o.id for o in self.world.robot.attachments.keys()]
+            if RobotManager.get_active_robot():
+                attached_objs_id = [o.id for o in RobotManager.get_active_robot().attachments.keys()]
                 res[i:j] = [
-                    1 if ray.obj_id in [-1, self.world.robot.id, floor_id] + attached_objs_id else 0 for
+                    1 if ray.obj_id in [-1, RobotManager.get_active_robot().id, floor_id] + attached_objs_id else 0 for
                     ray in r_t]
             else:
                 res[i:j] = [1 if ray.obj_id in [-1, floor_id] else 0 for ray in r_t]
@@ -544,10 +545,10 @@ def __init__(self, min_height: float,
         if robot:
             # this is done because otherwise the robot would interfere with the costmap
             current_pose = robot.get_pose()
-            robot.world.robot.set_pose(PoseStamped.from_list([current_pose.position.x, current_pose.position.y+1000, current_pose.position.z]))
+            robot.set_pose(PoseStamped.from_list([current_pose.position.x, current_pose.position.y+1000, current_pose.position.z]))
         self._generate_map()
         if robot:
-            robot.world.robot.set_pose(current_pose)
+            robot.set_pose(current_pose)
         Costmap.__init__(self, resolution, size, size, self.origin, self.map)
 
     @property

src/pycram/datastructures/world.py

@@ -30,7 +30,7 @@
 from ..datastructures.world_entity import PhysicalBody, WorldEntity
 from ..failures import ProspectionObjectNotFound, ObjectNotFound
 from ..local_transformer import LocalTransformer
-from ..robot_description import RobotDescription
+from ..multirobot import RobotManager
 from ..ros import Time
 from ..ros import logwarn
 from ..validation.goal_validator import (GoalValidator,
@@ -43,6 +43,7 @@
     from ..world_concepts.world_object import Object
     from ..description import Link, Joint, ObjectDescription
     from ..object_descriptors.generic import ObjectDescription as GenericObjectDescription
+    from ..robot_description import RobotDescription
 
 
 class World(WorldEntity, ABC):
@@ -248,7 +249,7 @@ def robot_description(self) -> RobotDescription:
         """
         Return the current robot description.
         """
-        return RobotDescription.current_robot_description
+        return RobotManager.get_robot_description()
 
     @property
     def robot_has_actuators(self) -> bool:

src/pycram/designators/location_designator.py

@@ -32,6 +32,7 @@
 from ..failures import RobotInCollision
 from ..graph_of_convex_sets import GraphOfConvexSets, plot_bounding_boxes_in_rviz
 from ..local_transformer import LocalTransformer
+from ..multirobot import RobotManager
 from ..object_descriptors.urdf import ObjectDescription
 from ..pose_generator_and_validator import PoseGenerator, visibility_validator, pose_sequence_reachability_validator, \
     collision_check, OrientationGenerator
@@ -98,7 +99,7 @@ def _create_target_sequence(grasp_description: GraspDescription, target: Union[P
             target_pose.rotate_by_quaternion(side_grasp.tolist())
 
         target_pose = object_in_hand.attachments[
-            World.robot].get_child_link_target_pose_given_parent(target_pose)
+            RobotManager.get_active_robot()].get_child_link_target_pose_given_parent(target_pose)
         approach_offset_cm = object_in_hand.get_approach_offset()
     else:
 
@@ -239,7 +240,7 @@ def __iter__(self) -> Iterator[PoseStamped]:
                 robot_object = params_box.visible_for if params_box.visible_for else params_box.reachable_for
                 test_robot = World.current_world.get_prospection_object_for_object(robot_object)
             else:
-                test_robot = World.current_world.robot
+                test_robot = RobotManager.get_active_robot()
 
             allowed_collision = self.create_allowed_collisions(params_box.ignore_collision_with,
                                                                params_box.object_in_hand)
@@ -765,9 +766,9 @@ def __iter__(self) -> Iterator[PoseStamped]:
         for params in self.generate_permutations():
 
             params_box = Box(params)
-            test_robot = World.current_world.get_prospection_object_for_object(World.current_world.robot)
+            test_robot = World.current_world.get_prospection_object_for_object(RobotManager.get_active_robot())
             # Using normal robot here because of prospection world bug that will be irrelevant after semantic world integration
-            test_robot = World.current_world.robot
+            test_robot = RobotManager.get_active_robot()
             world = World.current_world
 
             links: List[Link] = [params_box.part_of.links[link_name] for link_name in params_box.link_names]
@@ -958,7 +959,7 @@ def _calculate_room_event(world, free_space_graph: GraphOfConvexSets, target_pos
         rays_end = [[node.origin[0], node.origin[1], target_position.z + 0.2] for node in free_space_graph.nodes]
         rays_start = [[target_position.x, target_position.y, target_position.z + 0.2]] * len(rays_end)
 
-        robot = World.robot
+        robot = RobotManager.get_active_robot()
         robot_pose = robot.get_pose()
         robot.set_pose(
             PoseStamped.from_list([robot_pose.position.x, robot_pose.position.y, robot_pose.position.z + 100]))
@@ -1069,7 +1070,7 @@ def __iter__(self) -> Iterator[PoseStamped]:
                 robot_object = params_box.visible_for if params_box.visible_for else params_box.reachable_for
                 test_robot = world.get_prospection_object_for_object(robot_object)
             else:
-                test_robot = world.robot
+                test_robot = RobotManager.get_active_robot()
 
             allowed_collision = self.create_allowed_collisions(params_box.ignore_collision_with,
                                                                params_box.object_in_hand)

src/pycram/designators/specialized_designators/action/dual_arm_pickup_action.py

@@ -1,4 +1,4 @@
-from typing_extensions import List, Union, Optional
+from typing_extensions import List, Union, Optional, TYPE_CHECKING
 from numpy.linalg import norm
 from numpy import array
 from geometry_msgs.msg import Vector3
@@ -10,7 +10,7 @@
 from ....datastructures.world import World
 from ....datastructures.pose import PoseStamped, TransformStamped
 from ....datastructures.enums import Arms, Grasp
-from ....robot_description import RobotDescription, KinematicChainDescription
+from ....robot_description import KinematicChainDescription
 from ....designator import ObjectDesignatorDescription
 from ....ros import loginfo
 
@@ -40,11 +40,10 @@ def __init__(self,
         self.object_designator_description: Union[
             ObjectDesignatorDescription, ObjectDesignatorDescription.Object] = object_designator_description
 
-        left_gripper = RobotDescription.current_robot_description.get_arm_chain(Arms.LEFT)
-        right_gripper = RobotDescription.current_robot_description.get_arm_chain(Arms.RIGHT)
+        left_gripper = RobotManager.get_robot_description().get_arm_chain(Arms.LEFT)
+        right_gripper = RobotManager.get_robot_description().get_arm_chain(Arms.RIGHT)
         self.gripper_list: List[KinematicChainDescription] = [left_gripper, right_gripper]
 
-
     def ground(self) -> PickUpAction:
         if isinstance(self.object_designator_description, ObjectDesignatorDescription.Object):
             obj_desig = self.object_designator_description

src/pycram/designators/specialized_designators/location/giskard_location.py

@@ -4,7 +4,7 @@
 from ....datastructures.pose import PoseStamped
 from ....designators.location_designator import CostmapLocation
 from ....external_interfaces.giskard import projection_cartesian_goal_with_approach, projection_joint_goal
-from ....robot_description import RobotDescription
+from ....multirobot import RobotManager
 from ....datastructures.world import UseProspectionWorld, World
 from ....local_transformer import LocalTransformer
 from ....costmaps import OccupancyCostmap, GaussianCostmap
@@ -29,7 +29,7 @@ def __iter__(self) -> CostmapLocation.Location:
         local_transformer = LocalTransformer()
         target_map = local_transformer.transform_pose(self.target, "map")
 
-        manipulator_descs = RobotDescription.current_robot_description.get_manipulator_chains()
+        manipulator_descs = RobotManager.get_robot_description().get_manipulator_chains()
 
         near_costmap = (OccupancyCostmap(0.35, False, 200, 0.02, target_map)
                         + GaussianCostmap(200, 15, 0.02, target_map))
@@ -38,7 +38,7 @@ def __iter__(self) -> CostmapLocation.Location:
                 projection_joint_goal(chain.get_static_joint_states(StaticJointState.Park), allow_collisions=True)
 
                 trajectory = projection_cartesian_goal_with_approach(maybe_pose, target_map, chain.tool_frame,
-                                                                     "map", RobotDescription.current_robot_description.base_link)
+                                                                     "map", RobotManager.get_robot_description().base_link)
                 last_point_positions = trajectory.trajectory.points[-1].positions
                 last_point_names = trajectory.trajectory.joint_names
                 last_joint_states = dict(zip(last_point_names, last_point_positions))

src/pycram/external_interfaces/giskard.py

@@ -13,7 +13,7 @@
 from ..datastructures.dataclasses import MeshVisualShape
 from ..ros import get_service_proxy
 from ..world_concepts.world_object import Object
-from ..robot_description import RobotDescription
+from ..multirobot import RobotManager
 
 from typing_extensions import List, Dict, Callable, Optional
 from threading import Lock, RLock
@@ -99,7 +99,7 @@ def initial_adding_objects() -> None:
     """
     groups = giskard_wrapper.get_group_names()
     for obj in World.current_world.objects:
-        if obj is World.robot or obj is World.current_world.get_prospection_object_for_object(World.robot):
+        if obj is RobotManager.get_active_robot() or obj is World.current_world.get_prospection_object_for_object(RobotManager.get_active_robot()):
             continue
         name = obj.name
         if name not in groups:
@@ -131,21 +131,21 @@ def sync_worlds(projection: bool = False) -> None:
     add_gripper_groups()
     world_object_names = set()
     for obj in World.current_world.objects:
-        if obj.name != RobotDescription.current_robot_description.name and obj.obj_type != ObjectType.ROBOT and len(obj.link_name_to_id) != 1:
+        if obj.name != RobotManager.get_robot_description().name and obj.obj_type != ObjectType.ROBOT and len(obj.link_name_to_id) != 1:
             world_object_names.add(obj.name)
-        if obj.name == RobotDescription.current_robot_description.name or obj.obj_type == ObjectType.ROBOT:
+        if obj.name == RobotManager.get_robot_description().name or obj.obj_type == ObjectType.ROBOT:
             joint_config = obj.get_positions_of_all_joints()
             non_fixed_joints = list(filter(lambda joint: joint.type != JointType.FIXED and not joint.is_virtual,
                                            obj.joints.values()))
             joint_config_filtered = {joint.name: joint_config[joint.name] for joint in non_fixed_joints}
 
             if projection:
                 giskard_wrapper.monitors.add_set_seed_configuration(joint_config_filtered,
-                                                                    RobotDescription.current_robot_description.name)
+                                                                    RobotManager.get_robot_description().name)
                 giskard_wrapper.monitors.add_set_seed_odometry(obj.get_pose().ros_message(),
-                                                               RobotDescription.current_robot_description.name)
+                                                               RobotManager.get_robot_description().name)
     giskard_object_names = set(giskard_wrapper.get_group_names())
-    robot_name = {RobotDescription.current_robot_description.name}
+    robot_name = {RobotManager.get_robot_description().name}
     if not world_object_names.union(robot_name).issubset(giskard_object_names):
         giskard_wrapper.clear_world()
     initial_adding_objects()
@@ -256,9 +256,9 @@ def _manage_par_motion_goals(goal_func, *args) -> Optional['MoveResult']:
             for cmd in giskard_wrapper.motion_goals.get_goals():
                 par_value_pair = json.loads(cmd.kwargs)
                 if "tip_link" in par_value_pair.keys() and "root_link" in par_value_pair.keys():
-                    if par_value_pair["tip_link"] == RobotDescription.current_robot_description.base_link:
+                    if par_value_pair["tip_link"] == RobotManager.get_robot_description().base_link:
                         continue
-                    chain = World.robot.description.get_chain(par_value_pair["root_link"],
+                    chain = RobotManager.get_active_robot().description.get_chain(par_value_pair["root_link"],
                                                               par_value_pair["tip_link"])
                     if set(chain).intersection(used_joints) != set():
                         giskard_wrapper.motion_goals._goals = tmp_goals
@@ -705,9 +705,9 @@ def add_gripper_groups() -> None:
         for name in giskard_wrapper.get_group_names():
             if "gripper" in name:
                 return
-        for description in RobotDescription.current_robot_description.get_manipulator_chains():
+        for description in RobotManager.get_robot_description().get_manipulator_chains():
             giskard_wrapper.register_group(description.name + "_gripper", description.end_effector.start_link,
-                                           RobotDescription.current_robot_description.name)
+                                           RobotManager.get_robot_description().name)
 
 
 @init_giskard_interface

src/pycram/external_interfaces/ik.py

@@ -12,7 +12,7 @@
 from ..utils import _apply_ik
 from ..local_transformer import LocalTransformer
 from ..datastructures.pose import PoseStamped
-from ..robot_description import RobotDescription
+from ..multirobot import RobotManager
 from ..failures import IKError
 from ..external_interfaces.giskard import projection_cartesian_goal, allow_gripper_collision
 from .pinocchio_ik import compute_ik
@@ -76,7 +76,7 @@ def try_to_reach(pose_or_object: Union[PoseStamped, Object], prospection_robot:
     """
     input_pose = pose_or_object.get_pose() if isinstance(pose_or_object, Object) else pose_or_object
 
-    arm_chain = list(filter(lambda chain: chain.get_tool_frame() == gripper_name, RobotDescription.current_robot_description.get_manipulator_chains()))[0]
+    arm_chain = list(filter(lambda chain: chain.get_tool_frame() == gripper_name, RobotManager.get_robot_description().get_manipulator_chains()))[0]
 
     joints = arm_chain.joints
 
@@ -145,7 +145,7 @@ def request_giskard_ik(target_pose: PoseStamped, robot: Object, gripper: str) ->
         dist = tip_pose.position.euclidean_distance(target_map.position)
 
         if dist > 0.01:
-            raise IKError(target_map, 'map', gripper, World.robot.get_pose(), robot.get_positions_of_all_joints())
+            raise IKError(target_map, 'map', gripper, RobotManager.get_active_robot().get_pose(), robot.get_positions_of_all_joints())
         return pose, robot_joint_states
 
 def request_pinocchio_ik(target_pose: PoseStamped, robot: Object, target_link: str, joints: List[str]) -> Dict[str, float]:
@@ -163,7 +163,7 @@ def request_pinocchio_ik(target_pose: PoseStamped, robot: Object, target_link: s
     target_pose = lt.transform_pose(target_pose_map, robot.tf_frame)
 
     # Get link after last joint in chain
-    wrist_link = RobotDescription.current_robot_description.get_child(joints[-1])
+    wrist_link = RobotManager.get_robot_description().get_child(joints[-1])
 
     wrist_tool_frame_offset = robot.get_transform_between_links(wrist_link, target_link)
     target_diff = target_pose.to_transform_stamped("target").inverse_times(wrist_tool_frame_offset).to_pose_stamped()

src/pycram/external_interfaces/pycramgym.py

@@ -1,4 +1,5 @@
 from ..language import LanguageMixin
+from ..multirobot import RobotManager
 from ..world_concepts.world_object import Object, World
 from ..datastructures.enums import ObjectType
 from ..datastructures.pose import PoseStamped
@@ -57,4 +58,5 @@ def close(self):
         """
         if World.current_world is not None and self.world is not None:
             World.current_world.remove_object(self.world)
-            World.current_world.remove_object(self.robot)
+            for name, robot in RobotManager.available_robots:
+                World.current_world.remove_object(robot)

src/pycram/multirobot/__init__.py

@@ -0,0 +1,3 @@
+from .robot_manager import RobotManager
+
+RobotManager()