Merge pull request cram2#193 from cram2/multiverse-dev

Multiverse dev

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,101 @@
+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

examples/cram_plan_tutorial.md

@@ -28,7 +28,7 @@ from pycram.designators.location_designator import *
 from pycram.process_module import simulated_robot
 from pycram.designators.object_designator import *
 import anytree
-import pycram.plan_failures
+import pycram.failures
 ```
 
 Next we will create a bullet world with a PR2 in a kitchen containing milk and cereal.

examples/improving_actions.md

@@ -44,7 +44,7 @@ from random_events.product_algebra import Event, SimpleEvent
 
 import pycram.orm.base
 from pycram.designators.action_designator import MoveTorsoActionPerformable
-from pycram.plan_failures import PlanFailure
+from pycram.failures import PlanFailure
 from pycram.designators.object_designator import ObjectDesignatorDescription
 from pycram.worlds.bullet_world import BulletWorld
 from pycram.world_concepts.world_object import Object

examples/language.md

@@ -254,7 +254,7 @@ We will see how exceptions are handled at a simple example.
 from pycram.designators.action_designator import *
 from pycram.process_module import simulated_robot
 from pycram.language import Code
-from pycram.plan_failures import PlanFailure
+from pycram.failures import PlanFailure
 
 
 def code_test():

examples/minimal_task_tree.md

@@ -31,7 +31,7 @@ from pycram.designators.object_designator import *
 from pycram.datastructures.pose import Pose
 from pycram.datastructures.enums import ObjectType, WorldMode
 import anytree
-import pycram.plan_failures
+import pycram.failures
 ```
 
 Next we will create a bullet world with a PR2 in a kitchen containing milk and cereal.

examples/orm_querying_examples.md

@@ -30,9 +30,10 @@ import tqdm
 import pycram.orm.base
 from pycram.worlds.bullet_world import BulletWorld
 from pycram.world_concepts.world_object import Object as BulletWorldObject
-from pycram.designators.action_designator import MoveTorsoAction, PickUpAction, NavigateAction, ParkArmsAction, ParkArmsActionPerformable, MoveTorsoActionPerformable
+from pycram.designators.action_designator import MoveTorsoAction, PickUpAction, NavigateAction, ParkArmsAction,
+    ParkArmsActionPerformable, MoveTorsoActionPerformable
 from pycram.designators.object_designator import ObjectDesignatorDescription
-from pycram.plan_failures import PlanFailure
+from pycram.failures import PlanFailure
 from pycram.process_module import ProcessModule
 from pycram.datastructures.enums import Arms, ObjectType, Grasp, WorldMode
 from pycram.process_module import simulated_robot

requirements.txt

@@ -24,6 +24,9 @@ pynput~=1.7.7
 playsound~=1.3.0
 pydub~=0.25.1
 gTTS~=2.5.3
+dm_control
+trimesh
+deprecated
 probabilistic_model>=5.1.0
 random_events>=3.0.7
 sympy

src/pycram/__init__.py

@@ -1,4 +1,5 @@
-import pycram.process_modules
+from . import process_modules
+from . import robot_descriptions
 # from .specialized_designators import *
 
 from .datastructures.world import World