Merge branch 'dev' into enum_pr

README.md

@@ -29,18 +29,19 @@ The plan that both robots execute is a relativly simple pick and place plan:
 
 The code for this plan can be seen below.
 ```
-from pycram.bullet_world import BulletWorld, Object
+from pycram.worlds.bullet_world import BulletWorld
+from pycram.world_concepts.world_object import Object
 from pycram.process_module import simulated_robot
 from pycram.designators.motion_designator import *
 from pycram.designators.location_designator import *
 from pycram.designators.action_designator import *
 from pycram.designators.object_designator import *
-from pycram.enums import ObjectType
+from pycram.datastructures.enums import ObjectType, Arms, Grasp, WorldMode
 
-world = BulletWorld()
+world = BulletWorld(WorldMode.GUI)
 kitchen = Object("kitchen", ObjectType.ENVIRONMENT, "kitchen.urdf")
 robot = Object("pr2", ObjectType.ROBOT, "pr2.urdf")
-cereal = Object("cereal", ObjectType.BREAKFAST_CEREAL, "breakfast_cereal.stl", position=[1.4, 1, 0.95])
+cereal = Object("cereal", ObjectType.BREAKFAST_CEREAL, "breakfast_cereal.stl", pose=Pose([1.4, 1, 0.95]))
 
 cereal_desig = ObjectDesignatorDescription(names=["cereal"])
 kitchen_desig = ObjectDesignatorDescription(names=["kitchen"])
@@ -56,7 +57,7 @@ with simulated_robot:
 
     NavigateAction(target_locations=[pickup_pose.pose]).resolve().perform()
 
-    PickUpAction(object_designator_description=cereal_desig, arms=[pickup_arm], grasps=["front"]).resolve().perform()
+    PickUpAction(object_designator_description=cereal_desig, arms=[pickup_arm], grasps=[Grasp.FRONT]).resolve().perform()
 
     ParkArmsAction([Arms.BOTH]).resolve().perform()
 
@@ -69,6 +70,8 @@ with simulated_robot:
     PlaceAction(cereal_desig, target_locations=[place_island.pose], arms=[pickup_arm]).resolve().perform()
 
     ParkArmsAction([Arms.BOTH]).resolve().perform()
+
+world.exit()
 ```
 
 

binder/README.md

@@ -99,8 +99,7 @@ RUN cd pycram \
   && cd src/neem_interface_python \
   && git clone https://github.com/benjaminalt/neem-interface.git src/neem-interface
 
-RUN pip install --requirement ${PYCRAM_WS}/src/pycram/requirements.txt --user 
-RUN pip install --requirement ${PYCRAM_WS}/src/pycram/src/neem_interface_python/requirements.txt --user \
+RUN pip install --requirement ${PYCRAM_WS}/src/pycram/requirements.txt --user \
   && pip cache purge
 ```
 
@@ -448,15 +447,3 @@ with simulated_robot:
                  arms=["left"],
                  grasps=["left", "right"]).resolve().perform()
 ```
-
-
-
-
-
-
-
-
-
-
-
-

binder/pycram-http.rosinstall

@@ -7,18 +7,10 @@ repositories:
     type: git
     url: http://github.com/code-iai/iai_robots.git
     version: master
-  pr2_common:
-    type: git
-    url: https://github.com/PR2/pr2_common.git
-    version: b34703bcca2b07cadbc3777d3c504c232a0c0c28
   kdl_ik_services:
     type: git
     url: https://github.com/cram2/kdl_ik_service.git
     verison: master
-  pr2_kinematics:
-    type: git
-    url: https://github.com/PR2/pr2_kinematics.git
-    version: kinetic-devel
   orocos_kinematics_dynamics:
     type: git
     url: https://github.com/orocos/orocos_kinematics_dynamics.git

config/multiverse_conf.py

@@ -0,0 +1,72 @@
+import datetime
+
+from typing_extensions import Type
+
+from .world_conf import WorldConfig
+from pycram.description import ObjectDescription
+from pycram.helper import find_multiverse_resources_path
+from pycram.object_descriptors.mjcf import ObjectDescription as MJCF
+
+
+class MultiverseConfig(WorldConfig):
+    # Multiverse Configuration
+    resources_path = find_multiverse_resources_path()
+    """
+    The path to the Multiverse resources directory.
+    """
+
+    # Multiverse Socket Configuration
+    HOST: str = "tcp://127.0.0.1"
+    SERVER_HOST: str = HOST
+    SERVER_PORT: str = 7000
+    BASE_CLIENT_PORT: int = 9000
+
+    # Multiverse Client Configuration
+    READER_MAX_WAIT_TIME_FOR_DATA: datetime.timedelta = datetime.timedelta(milliseconds=1000)
+    """
+    The maximum wait time for the data in seconds.
+    """
+
+    # Multiverse Simulation Configuration
+    simulation_time_step: datetime.timedelta = datetime.timedelta(milliseconds=10)
+    simulation_frequency: int = int(1 / simulation_time_step.total_seconds())
+    """
+    The time step of the simulation in seconds and the frequency of the simulation in Hz.
+    """
+
+    simulation_wait_time_factor: float = 1.0
+    """
+    The factor to multiply the simulation wait time with, this is used to adjust the simulation wait time to account for
+    the time taken by the simulation to process the request, this depends on the computational power of the machine
+    running the simulation.
+    """
+
+    use_static_mode: bool = True
+    """
+    If True, the simulation will always be in paused state unless the simulate() function is called, this behaves 
+    similar to bullet_world which uses the bullet physics engine.
+    """
+
+    use_controller: bool = False
+    use_controller = use_controller and not use_static_mode
+    """
+    Only used when use_static_mode is False. This turns on the controller for the robot joints.
+    """
+
+    default_description_type: Type[ObjectDescription] = MJCF
+    """
+    The default description type for the objects.
+    """
+
+    use_physics_simulator_state: bool = True
+    """
+    Whether to use the physics simulator state when restoring or saving the world state.
+    """
+
+    clear_cache_at_start = False
+
+    let_pycram_move_attached_objects = False
+    let_pycram_handle_spawning = False
+
+    position_tolerance = 2e-2
+    prismatic_joint_position_tolerance = 2e-2

config/world_conf.py

@@ -0,0 +1,93 @@
+import math
+import os
+
+from typing_extensions import Tuple, Type
+from pycram.description import ObjectDescription
+from pycram.object_descriptors.urdf import ObjectDescription as URDF
+
+
+class WorldConfig:
+
+    """
+    A class to store the configuration of the world, this can be inherited to create a new configuration class for a
+    specific world (e.g. multiverse has MultiverseConfig which inherits from this class).
+    """
+
+    resources_path = os.path.join(os.path.dirname(__file__), '..', '..', '..', 'resources')
+    resources_path = os.path.abspath(resources_path)
+    """
+    Global reference for the resources path, this is used to search for the description files of the robot and
+     the objects.
+    """
+
+    cache_dir_name: str = 'cached'
+    """
+    The name of the cache directory.
+    """
+
+    cache_dir: str = os.path.join(resources_path, cache_dir_name)
+    """
+    Global reference for the cache directory, this is used to cache the description files of the robot and the objects.
+    """
+
+    clear_cache_at_start: bool = True
+    """
+    Whether to clear the cache directory at the start.
+    """
+
+    prospection_world_prefix: str = "prospection_"
+    """
+    The prefix for the prospection world name.
+    """
+
+    simulation_frequency: int = 240
+    """
+    The simulation frequency (Hz), used for calculating the equivalent real time in the simulation.
+    """
+
+    update_poses_from_sim_on_get: bool = True
+    """
+    Whether to update the poses from the simulator when getting the object poses.
+    """
+
+    default_description_type: Type[ObjectDescription] = URDF
+    """
+    The default description type for the objects.
+    """
+
+    use_physics_simulator_state: bool = False
+    """
+    Whether to use the physics simulator state when restoring or saving the world state.
+    Currently with PyBullet, this causes a bug where ray_test does not work correctly after restoring the state using the
+    simulator, so it is recommended to set this to False in PyBullet.
+    """
+
+    let_pycram_move_attached_objects: bool = True
+    let_pycram_handle_spawning: bool = True
+    let_pycram_handle_world_sync: bool = True
+    """
+    Whether to let PyCRAM handle the movement of attached objects, the spawning of objects,
+     and the world synchronization.
+    """
+
+    position_tolerance: float = 1e-2
+    orientation_tolerance: float = 10 * math.pi / 180
+    prismatic_joint_position_tolerance: float = 1e-2
+    revolute_joint_position_tolerance: float = 5 * math.pi / 180
+    """
+    The acceptable error for the position and orientation of an object/link, and the joint positions.
+    """
+
+    use_percentage_of_goal: bool = False
+    acceptable_percentage_of_goal: float = 0.5
+    """
+    Whether to use a percentage of the goal as the acceptable error.
+    """
+
+    raise_goal_validator_error: bool = False
+    """
+    Whether to raise an error if the goals are not achieved.
+    """
+    @classmethod
+    def get_pose_tolerance(cls) -> Tuple[float, float]:
+        return cls.position_tolerance, cls.orientation_tolerance

demos/pycram_bullet_world_demo/demo.py

@@ -8,10 +8,12 @@
 from pycram.object_descriptors.urdf import ObjectDescription
 from pycram.world_concepts.world_object import Object
 from pycram.datastructures.dataclasses import Color
+from pycram.ros.viz_marker_publisher import VizMarkerPublisher
 
 extension = ObjectDescription.get_file_extension()
 
 world = BulletWorld(WorldMode.GUI)
+
 robot = Object("pr2", ObjectType.ROBOT, f"pr2{extension}", pose=Pose([1, 2, 0]))
 apartment = Object("apartment", ObjectType.ENVIRONMENT, f"apartment{extension}")
 
@@ -94,3 +96,5 @@ def move_and_detect(obj_type):
     PlaceAction(spoon_desig, [spoon_target_pose], [pickup_arm]).resolve().perform()
 
     ParkArmsAction([Arms.BOTH]).resolve().perform()
+
+world.exit()

demos/pycram_multiverse_demo/demo.py

@@ -0,0 +1,100 @@
+from pycram.datastructures.dataclasses import Color
+from pycram.datastructures.enums import ObjectType, Arms, Grasp
+from pycram.datastructures.pose import Pose
+from pycram.designators.action_designator import ParkArmsAction, MoveTorsoAction, TransportAction, NavigateAction, \
+    LookAtAction, DetectAction, OpenAction, PickUpAction, CloseAction, PlaceAction
+from pycram.designators.location_designator import CostmapLocation, AccessingLocation
+from pycram.designators.object_designator import BelieveObject, ObjectPart
+from pycram.object_descriptors.urdf import ObjectDescription
+from pycram.process_module import simulated_robot, with_simulated_robot
+from pycram.world_concepts.world_object import Object
+from pycram.worlds.multiverse import Multiverse
+
+
+@with_simulated_robot
+def move_and_detect(obj_type: ObjectType, pick_pose: Pose):
+    NavigateAction(target_locations=[Pose([1.7, 2, 0])]).resolve().perform()
+
+    LookAtAction(targets=[pick_pose]).resolve().perform()
+
+    object_desig = DetectAction(BelieveObject(types=[obj_type])).resolve().perform()
+
+    return object_desig
+
+world = Multiverse(simulation_name='pycram_test')
+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}")
+
+milk = Object("milk", ObjectType.MILK, f"milk.stl", pose=Pose([2.4, 2, 1.02]),
+              color=Color(1, 0, 0, 1))
+
+spoon = Object("spoon", ObjectType.SPOON, "spoon.stl", pose=Pose([2.5, 2.2, 0.85]),
+               color=Color(0, 0, 1, 1))
+apartment.attach(spoon, 'cabinet10_drawer1')
+
+robot_desig = BelieveObject(names=[robot.name])
+apartment_desig = BelieveObject(names=[apartment.name])
+
+with simulated_robot:
+
+    # Transport the milk
+    ParkArmsAction([Arms.BOTH]).resolve().perform()
+
+    MoveTorsoAction([0.25]).resolve().perform()
+
+    NavigateAction(target_locations=[Pose([1.7, 2, 0])]).resolve().perform()
+
+    LookAtAction(targets=[Pose([2.6, 2.15, 1])]).resolve().perform()
+
+    milk_desig = DetectAction(BelieveObject(types=[milk.obj_type])).resolve().perform()
+
+    TransportAction(milk_desig, [Arms.LEFT], [Pose([2.4, 3, 1.02])]).resolve().perform()
+
+    # Find and navigate to the drawer containing the spoon
+    handle_desig = ObjectPart(names=["cabinet10_drawer1_handle"], part_of=apartment_desig.resolve())
+    drawer_open_loc = AccessingLocation(handle_desig=handle_desig.resolve(),
+                                        robot_desig=robot_desig.resolve()).resolve()
+
+    NavigateAction([drawer_open_loc.pose]).resolve().perform()
+
+    OpenAction(object_designator_description=handle_desig, arms=[drawer_open_loc.arms[0]]).resolve().perform()
+    spoon.detach(apartment)
+
+    # Detect and pickup the spoon
+    LookAtAction([apartment.get_link_pose("cabinet10_drawer1_handle")]).resolve().perform()
+
+    spoon_desig = DetectAction(BelieveObject(types=[ObjectType.SPOON])).resolve().perform()
+
+    pickup_arm = Arms.LEFT if drawer_open_loc.arms[0] == Arms.RIGHT else Arms.RIGHT
+    PickUpAction(spoon_desig, [pickup_arm], [Grasp.TOP]).resolve().perform()
+
+    ParkArmsAction([Arms.LEFT if pickup_arm == Arms.LEFT else Arms.RIGHT]).resolve().perform()
+
+    CloseAction(object_designator_description=handle_desig, arms=[drawer_open_loc.arms[0]]).resolve().perform()
+
+    ParkArmsAction([Arms.BOTH]).resolve().perform()
+
+    MoveTorsoAction([0.15]).resolve().perform()
+
+    # Find a pose to place the spoon, move and then place it
+    spoon_target_pose = Pose([2.35, 2.6, 0.95], [0, 0, 0, 1])
+    placing_loc = CostmapLocation(target=spoon_target_pose, reachable_for=robot_desig.resolve()).resolve()
+
+    NavigateAction([placing_loc.pose]).resolve().perform()
+
+    PlaceAction(spoon_desig, [spoon_target_pose], [pickup_arm]).resolve().perform()
+
+    ParkArmsAction([Arms.BOTH]).resolve().perform()
+
+world.exit()
+
+
+def debug_place_spoon():
+    robot.set_pose(Pose([1.66, 2.56, 0.01], [0.0, 0.0, -0.04044101807153309, 0.9991819274072855]))
+    spoon.set_pose(Pose([1.9601757566599975, 2.06718019258626, 1.0427727691273496],
+                        [0.11157527804553227, -0.7076243776942466, 0.12773644958149588, 0.685931554070963]))
+    robot.attach(spoon, 'r_gripper_tool_frame')
+    pickup_arm = Arms.RIGHT
+    spoon_desig = BelieveObject(names=[spoon.name])
+    return pickup_arm, spoon_desig

demos/pycram_ur5_demo/demo.py

@@ -7,9 +7,9 @@
 from pycram.worlds.bullet_world import BulletWorld
 from pycram.datastructures.world import Object
 from pycram.datastructures.pose import Pose
-from pycram.ros.force_torque_sensor import ForceTorqueSensor
-from pycram.ros.joint_state_publisher import JointStatePublisher
-from pycram.ros.tf_broadcaster import TFBroadcaster
+from pycram.ros_utils.force_torque_sensor import ForceTorqueSensor
+from pycram.ros_utils.joint_state_publisher import JointStatePublisher
+from pycram.ros_utils.tf_broadcaster import TFBroadcaster
 
 SCRIPT_DIR = os.path.abspath(os.path.dirname(__file__))
 PYCRAM_DIR = os.path.join(SCRIPT_DIR, os.pardir, os.pardir)