Merge pull request cram2#227 from AbdelrhmanBassiouny/fallschool_mult…

…iverse_demo

Fallschool multiverse demo + Minor Changes

config/multiverse_conf.py

@@ -47,8 +47,7 @@ class MultiverseConfig(WorldConfig):
     similar to bullet_world which uses the bullet physics engine.
     """
 
-    use_controller: bool = False
-    use_controller = use_controller and not use_static_mode
+    use_controller: bool = True
     """
     Only used when use_static_mode is False. This turns on the controller for the robot joints.
     """
@@ -58,12 +57,14 @@ class MultiverseConfig(WorldConfig):
     The default description type for the objects.
     """
 
-    use_physics_simulator_state: bool = True
+    use_physics_simulator_state: bool = False
     """
     Whether to use the physics simulator state when restoring or saving the world state.
     """
 
-    clear_cache_at_start = False
+    validate_goals = True
+
+    clear_cache_at_start = True
 
     let_pycram_move_attached_objects = False
     let_pycram_handle_spawning = False
@@ -72,4 +73,8 @@ class MultiverseConfig(WorldConfig):
     prismatic_joint_position_tolerance = 2e-2
 
     use_giskard_monitor = False
-    allow_gripper_collision = False
+    allow_gripper_collision = True
+
+    use_multiverse_process_modules = True
+
+    depth_images_are_in_meter = True

config/world_conf.py

@@ -35,7 +35,7 @@ class WorldConfig:
     Whether to clear the cache directory at the start.
     """
 
-    prospection_world_prefix: str = "prospection_"
+    prospection_world_prefix: str = "prospection"
     """
     The prefix for the prospection world name.
     """
@@ -84,6 +84,11 @@ class WorldConfig:
     Whether to use a percentage of the goal as the acceptable error.
     """
 
+    validate_goals: bool = False
+    """
+    Whether to validate the goals when executing them.
+    """
+
     raise_goal_validator_error: bool = False
     """
     Whether to raise an error if the goals are not achieved.
@@ -99,6 +104,11 @@ class WorldConfig:
     Whether to allow the gripper to collide with the objects when planning for the goals.
     """
 
+    depth_images_are_in_meter: bool = False
+    """
+    Whether the depth images produced by :meth:`datastructures.world.World.get_images_for_target` are in meters.
+    """
+
     @classmethod
     def get_pose_tolerance(cls) -> Tuple[float, float]:
         return cls.position_tolerance, cls.orientation_tolerance

demos/pycram_bullet_world_demo/demo.py

@@ -8,7 +8,7 @@
 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
+from pycram.ros_utils.viz_marker_publisher import VizMarkerPublisher
 from pycrap import Robot, Apartment, Milk, Cereal, Spoon, Bowl
 import numpy as np
 
@@ -101,4 +101,5 @@ def move_and_detect(obj_type):
 
     ParkArmsAction([Arms.BOTH]).resolve().perform()
 
+
 world.exit()

demos/pycram_multiverse_demo/demo.py

@@ -1,35 +1,31 @@
+from typing_extensions import Type
+
+import pycrap
 from pycram.datastructures.dataclasses import Color
-from pycram.datastructures.enums import ObjectType, Arms, Grasp
+from pycram.datastructures.enums import ObjectType, Arms, Grasp, DetectionTechnique
 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.motion_designator import MoveArmJointsMotion, MoveTCPMotion
 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.robot_description import RobotDescription
 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):
-    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')
+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.stl", 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.stl", 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')
 
@@ -47,9 +43,10 @@ def move_and_detect(obj_type: ObjectType, pick_pose: Pose):
 
     LookAtAction(targets=[Pose([2.6, 2.15, 1])]).resolve().perform()
 
-    milk_desig = DetectAction(BelieveObject(types=[milk.obj_type])).resolve().perform()
+    milk_desig = DetectAction(DetectionTechnique.TYPES,
+                              object_designator_description=BelieveObject(types=[pycrap.Milk])).resolve().perform()[0]
 
-    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())
@@ -58,19 +55,34 @@ def move_and_detect(obj_type: ObjectType, pick_pose: Pose):
 
     NavigateAction([drawer_open_loc.pose]).resolve().perform()
 
-    OpenAction(object_designator_description=handle_desig, arms=[drawer_open_loc.arms[0]]).resolve().perform()
-    spoon.detach(apartment)
+    OpenAction(object_designator_description=handle_desig,
+               arms=[drawer_open_loc.arms[0]]).resolve().perform()
+
+    arm_ee = RobotDescription.current_robot_description.get_arm_chain(drawer_open_loc.arms[0]).get_tool_frame()
+    closing_arm_pose = robot.get_link_pose(arm_ee)
 
     # Detect and pickup the spoon
+    spoon.detach(apartment)
     LookAtAction([apartment.get_link_pose("cabinet10_drawer1_handle")]).resolve().perform()
 
-    spoon_desig = DetectAction(BelieveObject(types=[ObjectType.SPOON])).resolve().perform()
+    spoon_desig = DetectAction(DetectionTechnique.TYPES,
+                               object_designator_description=BelieveObject(types=[pycrap.Spoon])).resolve().perform()[0]
+
+    ParkArmsAction([Arms.BOTH]).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()
+    MoveTCPMotion(closing_arm_pose, drawer_open_loc.arms[0]).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()
@@ -79,7 +91,8 @@ def move_and_detect(obj_type: ObjectType, pick_pose: Pose):
 
     # 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()
+    placing_loc = CostmapLocation(target=spoon_target_pose,
+                                  reachable_for=robot_desig.resolve()).resolve()
 
     NavigateAction([placing_loc.pose]).resolve().perform()
 

demos/pycram_multiverse_demo/demo_euROBIN_industrial_robotics.py

@@ -0,0 +1,34 @@
+import pycrap
+from pycram.datastructures.enums import GripperState, Arms
+from pycram.datastructures.world import UseProspectionWorld
+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.worlds.multiverse import Multiverse
+from pycram.designators.action_designator import SetGripperAction
+from pycram.ros_utils.robot_state_updater import WorldStateUpdater
+
+
+if __name__ == '__main__':
+    # Create a new world
+    world = Multiverse()
+    WorldStateUpdater(tf_topic="/tf", joint_state_topic="/real/ur5e/joint_states")
+
+    # Load the robot and the gripper
+    robot = Object("ur5e", pycrap.Robot, "universal_robot/ur5e/urdf/ur5e.urdf")
+    gripper = Object("gripper-2F-85", pycrap.Gripper, "robotiq/gripper-2F-85/gripper-2F-85.urdf")
+
+    # Attach the gripper to the robot at the wrist_3_link with the correct pose
+    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")
+
+    # Get the robot arms
+    robot_arms = [chain.arm_type for chain in robot.robot_description.get_manipulator_chains()]
+
+    # Perform the plan
+    with real_robot:
+        SetGripperAction(robot_arms, [GripperState.CLOSE]).resolve().perform()
+
+
+    world.exit()

demos/pycram_multiverse_demo/fallschool_demo.py

@@ -0,0 +1,87 @@
+import logging
+from datetime import timedelta
+
+import rospy
+from tf.transformations import quaternion_from_euler
+from typing_extensions import Type
+
+import pycrap
+from pycram.datastructures.dataclasses import Color
+from pycram.datastructures.enums import Arms
+from pycram.datastructures.pose import Pose
+from pycram.datastructures.world import UseProspectionWorld, World
+from pycram.designators.action_designator import ParkArmsAction, MoveTorsoAction, TransportAction, NavigateAction, \
+    LookAtAction, DetectAction
+from pycram.designators.object_designator import BelieveObject
+from pycram.process_module import simulated_robot, with_simulated_robot, real_robot
+from pycram.ros_utils.robot_state_updater import WorldStateUpdater
+from pycram.world_concepts.world_object import Object
+from pycram.worlds.bullet_world import BulletWorld
+from pycram.worlds.multiverse import Multiverse
+from pycram.ros_utils.viz_marker_publisher import VizMarkerPublisher
+from pycrap import PhysicalObject
+
+
+@with_simulated_robot
+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()
+
+    object_desig = DetectAction(BelieveObject(types=[obj_type])).resolve().perform()
+
+    return object_desig
+
+
+use_bullet_world = False
+
+if use_bullet_world:
+    world = BulletWorld(use_multiverse_for_real_world_simulation=True)
+    vis_publisher = VizMarkerPublisher()
+    milk_path = "milk.stl"
+else:
+    world = Multiverse()
+    vis_publisher = None
+    milk_path = "milk.xml"
+
+robot = Object('pr2', pycrap.Robot, f'pr2.urdf', pose=Pose([1.3, 2.6, 0.01]))
+WorldStateUpdater(tf_topic="/tf", joint_state_topic="/real/pr2/joint_states", update_rate=timedelta(seconds=2),
+                  world=world)
+apartment = Object("apartment", pycrap.Apartment, f"apartment.urdf")
+milk = Object("milk", pycrap.Milk, milk_path, pose=Pose([0.4, 2.6, 1.34],
+                                                            [1, 0, 0, 0]),
+              color=Color(1, 0, 0, 1))
+
+# apartment.set_joint_position("fridge_door1_joint", 1.5707963267948966)
+
+fridge_base_pose = apartment.get_link_pose("fridge_base")
+fridge_base_pose.position.z -= 0.12
+fridge_base_pose.position.x += 0.16
+fridge_base_pose.position.y += -0.1
+milk.set_pose(fridge_base_pose, base=True)
+
+
+robot_desig = BelieveObject(names=[robot.name])
+apartment_desig = BelieveObject(names=[apartment.name])
+
+
+with real_robot:
+
+    # Transport the milkMoveGripperMotion
+    ParkArmsAction([Arms.BOTH]).resolve().perform()
+
+    MoveTorsoAction([0.2]).resolve().perform()
+
+    NavigateAction(target_locations=[Pose([1.4, 3.15, 0.01], quaternion_from_euler(0, 0, 3.14))]).resolve().perform()
+
+    LookAtAction(targets=[Pose(milk.get_position_as_list())]).resolve().perform()
+
+    milk_desig = DetectAction(BelieveObject(types=[milk.obj_type])).resolve().perform()
+
+    TransportAction(milk_desig, [Pose([2.4, 3, 1.02])], [Arms.LEFT]).resolve().perform()
+
+    ParkArmsAction([Arms.BOTH]).resolve().perform()
+
+if vis_publisher is not None:
+    vis_publisher._stop_publishing()
+world.exit()