Merge pull request #144 from LucaKro/luca_master_thesis

[armar6] reintegrated armar into pycram

binder/01-import.py

@@ -3,7 +3,7 @@
 sys.path.insert(0, '/home/jovyan/workspace/ros/src/pycram')
 
 from demos.pycram_virtual_building_demos.setup.setup_launch_robot import launch_pr2, launch_hsrb, launch_stretch, \
-    launch_tiago, launch_justin, launch_donbot
+    launch_tiago, launch_justin, launch_donbot, launch_armar, launch_icub
 from pycram.datastructures.enums import ObjectType, WorldMode
 from pycram.designators.object_designator import *
 from pycram.object_descriptors.urdf import ObjectDescription
@@ -27,3 +27,7 @@
     launch_justin()
 elif robot_param == 'donbot':
     launch_donbot()
+elif robot_param == 'armar':
+    launch_armar()
+elif robot_param == 'icub':
+    launch_icub()

binder/pycram-http.rosinstall

@@ -5,8 +5,8 @@ repositories:
     version: master
   iai_robots:
     type: git
-    url: http://github.com/code-iai/iai_robots.git
-    version: master
+    url: http://github.com/LucaKro/iai_robots.git
+    version: armar6
   pr2_common:
     type: git
     url: https://github.com/PR2/pr2_common.git

demos/pycram_bullet_world_demo/demo.py

@@ -1,10 +1,13 @@
-from pycram.ros.viz_marker_publisher import VizMarkerPublisher, AxisMarkerPublisher
+from pycram.external_interfaces.ik import request_ik
+from pycram.plan_failures import IKError
+from pycram.ros.viz_marker_publisher import VizMarkerPublisher, AxisMarkerPublisher, CostmapPublisher
+from pycram.utils import _apply_ik
 from pycram.worlds.bullet_world import BulletWorld
 from pycram.designators.action_designator import *
 from pycram.designators.location_designator import *
 from pycram.designators.object_designator import *
 from pycram.datastructures.enums import ObjectType, WorldMode, TorsoState
-from pycram.datastructures.pose import Pose
+from pycram.datastructures.pose import Pose, Transform
 from pycram.process_module import simulated_robot, with_simulated_robot
 from pycram.object_descriptors.urdf import ObjectDescription
 from pycram.world_concepts.world_object import Object
@@ -19,25 +22,54 @@
 
 apartment = Object("apartment", ObjectType.ENVIRONMENT, f"apartment-small{extension}")
 
-milk = Object("milk", ObjectType.MILK, "milk.stl", pose=Pose([2.5, 2, 1.02]),
-              color=Color(1, 0, 0, 1))
-cereal = Object("cereal", ObjectType.BREAKFAST_CEREAL, "breakfast_cereal.stl",
-                pose=Pose([2.5, 2.5, 1.05]), color=Color(0, 1, 0, 1))
-bowl = Object("bowl", ObjectType.BOWL, "bowl.stl", pose=Pose([2.4, 2.2, 0.98]),
-              color=Color(1, 1, 0, 1))
-spoon = Object("spoon", ObjectType.SPOON, "spoon.stl", pose=Pose([2.4, 2.2, 0.85]),
-               color=Color(0, 0, 1, 1))
-apartment.attach(spoon, 'cabinet10_drawer_top')
+if robot.name == "iCub":
+    milk = Object("milk", ObjectType.MILK, "milk.stl", pose=Pose([4.7, 4.65, 0.8]),
+                  color=Color(1, 0, 0, 1))
+    milk_target_pose = Pose([4.8, 3.45, 0.8])
 
-pick_pose = Pose([2.7, 2.15, 1])
+    cereal = Object("cereal", ObjectType.BREAKFAST_CEREAL, "breakfast_cereal.stl",
+                    pose=Pose([4.65, 4.75, 0.8]), color=Color(0, 1, 0, 1))
+    cereal_target_pose = Pose([4.85, 3.3, 0.8], [0, 0, 1, 1])
+
+    bowl = Object("bowl", ObjectType.BOWL, "bowl.stl", pose=Pose([4.7, 4.5, 0.75], [0, 0, -1, 1]),
+                  color=Color(1, 1, 0, 1))
+    bowl_target_pose = Pose([5, 3.3, 0.75], [0, 0, 0, 1])
+
+    spoon = Object("spoon", ObjectType.SPOON, "spoon.stl", pose=Pose([4.7, 4.3, 0.75], [0, 0, -1, 1]),
+                   color=Color(0, 0, 1, 1))
+    spoon_target_pose = Pose([5.2, 3.3, 0.8], [0, 0, 1, 1])
+
+    pick_pose = Pose([4.7, 4.5, 0.8])
+    nav_pose = Pose([4, 4.5, 0])
+else:
+    milk = Object("milk", ObjectType.MILK, "milk.stl", pose=Pose([2.5, 2, 1.02], [0, 0, 1, 1]),
+                  color=Color(1, 0, 0, 1))
+    milk_target_pose = Pose([4.8, 3.55, 0.8])
+
+    cereal = Object("cereal", ObjectType.BREAKFAST_CEREAL, "breakfast_cereal.stl",
+                    pose=Pose([2.5, 2.5, 1.05]), color=Color(0, 1, 0, 1))
+    cereal_target_pose = Pose([5.2, 3.4, 0.8], [0, 0, 1, 1])
+
+    bowl = Object("bowl", ObjectType.BOWL, "bowl.stl", pose=Pose([2.4, 2.2, 0.98]),
+                  color=Color(1, 1, 0, 1))
+    bowl_target_pose = Pose([5, 3.3, 0.8], [0, 0, 1, 1])
+
+    spoon = Object("spoon", ObjectType.SPOON, "spoon.stl", pose=Pose([2.5, 2.2, 0.85]),
+                   color=Color(0, 0, 1, 1))
+    spoon_target_pose = Pose([4.85, 3.3, 0.8], [0, 0, 1, 1])
+
+    apartment.attach(spoon, 'cabinet10_drawer_top')
+
+    pick_pose = Pose([2.7, 2.15, 1])
+    nav_pose = Pose([1.5, 2, 0])
 
 robot_desig = BelieveObject(names=[robot_name])
 apartment_desig = BelieveObject(names=["apartment"])
 
 
 @with_simulated_robot
 def move_and_detect(obj_type):
-    NavigateAction(target_locations=[Pose([1.7, 2, 0])]).resolve().perform()
+    NavigateAction(target_locations=[nav_pose]).resolve().perform()
 
     LookAtAction(targets=[pick_pose]).resolve().perform()
 
@@ -47,89 +79,109 @@ def move_and_detect(obj_type):
 
 
 with simulated_robot:
+    NavigateAction([Pose([0, 0, 0])]).resolve().perform()
+
     ParkArmsAction([Arms.BOTH]).resolve().perform()
 
     MoveTorsoAction([TorsoState.HIGH]).resolve().perform()
 
+    # handle_desig = ObjectPart(names=["handle_cab3_door_top"], part_of=apartment_desig.resolve())
+    # closed_location, opened_location = AccessingLocation(handle_desig=handle_desig.resolve(),
+    #                                                      robot_desig=robot_desig.resolve()).resolve()
+    # OpenAction(object_designator_description=handle_desig, arms=[closed_location.arms[0]], start_goal_location=[closed_location, opened_location]).resolve().perform()
+    # NavigateAction([Pose([0, 0, 0])]).resolve().perform()
+    # CloseAction(object_designator_description=handle_desig, arms=[closed_location.arms[0]], start_goal_location=[opened_location, closed_location]).resolve().perform()
+    # ParkArmsAction([Arms.BOTH]).resolve().perform()
+
     milk_desig = move_and_detect(ObjectType.MILK)
 
-    TransportAction(milk_desig, [Arms.LEFT], [Pose([4.8, 3.55, 0.8])]).resolve().perform()
+    TransportAction(milk_desig, [Arms.LEFT], [milk_target_pose]).resolve().perform()
 
     cereal_desig = move_and_detect(ObjectType.BREAKFAST_CEREAL)
 
-    TransportAction(cereal_desig, [Arms.LEFT], [Pose([5.2, 3.4, 0.8], [0, 0, 1, 1])]).resolve().perform()
-
-    bowl_desig = move_and_detect(ObjectType.BOWL)
+    TransportAction(cereal_desig, [Arms.LEFT], [cereal_target_pose]).resolve().perform()
 
-    TransportAction(bowl_desig, [Arms.LEFT], [Pose([5, 3.3, 0.8], [0, 0, 1, 1])]).resolve().perform()
+    if not robot.name == "tiago_dual":
+        bowl_desig = move_and_detect(ObjectType.BOWL)
 
-    # Finding and navigating to the drawer holding the spoon
-    handle_desig = ObjectPart(names=["handle_cab10_t"], part_of=apartment_desig.resolve())
+        TransportAction(bowl_desig, [Arms.LEFT], [bowl_target_pose]).resolve().perform()
 
-    if robot.name == "rollin_justin":
-        pose = Pose([1.4, 1.6, 0], [0, 0, 0.2040033016133158, 0.9789702002261697])
-        drawer_open_loc = AccessingLocation.Location(pose, [Arms.LEFT])
+    if robot.name == "iCub":
+        spoon_desig = move_and_detect(ObjectType.SPOON)
+        TransportAction(spoon_desig, [Arms.LEFT], [spoon_target_pose]).resolve().perform()
     else:
-        drawer_open_loc = AccessingLocation(handle_desig=handle_desig.resolve(),
-                                            robot_desig=robot_desig.resolve()).resolve()
+        # Finding and navigating to the drawer holding the spoon
+        handle_desig = ObjectPart(names=["handle_cab10_t"], part_of=apartment_desig.resolve())
+        closed_location, opened_location = AccessingLocation(handle_desig=handle_desig.resolve(),
+                                                             robot_desig=robot_desig.resolve()).resolve()
 
-    NavigateAction([drawer_open_loc.pose]).resolve().perform()
+        NavigateAction([closed_location.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=[closed_location.arms[0]],
+                   start_goal_location=[closed_location, opened_location]).resolve().perform()
+        spoon.detach(apartment)
 
-    # Detect and pickup the spoon
-    LookAtAction([apartment.get_link_pose("handle_cab10_t")]).resolve().perform()
-    ParkArmsAction([Arms.BOTH]).resolve().perform()
+        # Detect and pickup the spoon
+        ParkArmsAction([Arms.BOTH]).resolve().perform()
+        MoveTorsoAction([TorsoState.HIGH]).resolve().perform()
+        LookAtAction([apartment.get_link_pose("handle_cab10_t")]).resolve().perform()
 
-    spoon_desig = DetectAction(BelieveObject(types=[ObjectType.SPOON])).resolve().perform()
+        spoon_desig = DetectAction(BelieveObject(types=[ObjectType.SPOON])).resolve().perform()
 
-    if robot.name == "iai_donbot":
-        ParkArmsAction([Arms.BOTH]).resolve().perform()
-        PickUpAction(spoon_desig, [Arms.LEFT], [Grasp.TOP]).resolve().perform()
+        if robot.name == "iai_donbot":
+            ParkArmsAction([Arms.BOTH]).resolve().perform()
+            PickUpAction(spoon_desig, [Arms.LEFT], [Grasp.TOP]).resolve().perform()
 
-        ParkArmsAction([Arms.BOTH]).resolve().perform()
+            ParkArmsAction([Arms.BOTH]).resolve().perform()
 
-        # Find a pose to place the spoon, move and then place it
-        spoon_target_pose = Pose([4.85, 3.3, 0.8], [0, 0, 1, 1])
-        placing_loc = CostmapLocation(target=spoon_target_pose, reachable_for=robot_desig.resolve()).resolve()
+            # Find a pose to place the spoon, move and then place it
+            placing_loc = CostmapLocation(target=spoon_target_pose, reachable_for=robot_desig.resolve(),
+                                          reachable_arm=Arms.LEFT,
+                                          used_grasps=[Grasp.TOP], object_in_hand=spoon_desig).resolve()
 
-        NavigateAction([placing_loc.pose]).resolve().perform()
+            NavigateAction([placing_loc.pose]).resolve().perform()
 
-        PlaceAction(spoon_desig, [spoon_target_pose], [Arms.LEFT]).resolve().perform()
+            PlaceAction(spoon_desig, [spoon_target_pose], [Arms.LEFT]).resolve().perform()
 
-        ParkArmsAction([Arms.BOTH]).resolve().perform()
+            ParkArmsAction([Arms.BOTH]).resolve().perform()
 
-        NavigateAction([drawer_open_loc.pose]).resolve().perform()
+            NavigateAction([closed_location.pose]).resolve().perform()
 
-        CloseAction(object_designator_description=handle_desig, arms=[drawer_open_loc.arms[0]]).resolve().perform()
+            CloseAction(object_designator_description=handle_desig, arms=[closed_location.arms[0]],
+                        start_goal_location=[opened_location, closed_location]).resolve().perform()
 
-        ParkArmsAction([Arms.BOTH]).resolve().perform()
+            ParkArmsAction([Arms.BOTH]).resolve().perform()
 
-    else:
-        if robot.name == "tiago_dual":
-            NavigateAction([Pose([1.45, 2.7, 0], [0, 0, 0, 1])]).resolve().perform()
+        else:
 
-        pickup_arm = Arms.LEFT if drawer_open_loc.arms[0] == Arms.RIGHT else Arms.RIGHT
-        ParkArmsAction([Arms.BOTH]).resolve().perform()
-        PickUpAction(spoon_desig, [pickup_arm], [Grasp.TOP]).resolve().perform()
+            pickup_arm = Arms.LEFT if closed_location.arms[0] == Arms.RIGHT else Arms.RIGHT
+            try:
+                PickUpAction(spoon_desig, [pickup_arm], [Grasp.TOP]).resolve().perform()
+            except IKError:
+                pickup_loc = CostmapLocation(target=spoon_desig, reachable_for=robot_desig.resolve(),
+                                             reachable_arm=pickup_arm, used_grasps=[Grasp.TOP]).resolve()
+                ParkArmsAction([Arms.BOTH]).resolve().perform()
+                NavigateActionPerformable(pickup_loc.pose).perform()
+                PickUpAction(spoon_desig, [pickup_arm], [Grasp.TOP]).resolve().perform()
 
-        ParkArmsAction([Arms.LEFT if pickup_arm == Arms.LEFT else Arms.RIGHT]).resolve().perform()
+            ParkArmsAction([Arms.LEFT if pickup_arm == Arms.LEFT else Arms.RIGHT]).resolve().perform()
 
-        NavigateAction([drawer_open_loc.pose]).resolve().perform()
+            NavigateAction([opened_location.pose]).resolve().perform()
 
-        CloseAction(object_designator_description=handle_desig, arms=[drawer_open_loc.arms[0]]).resolve().perform()
+            CloseAction(object_designator_description=handle_desig, arms=[closed_location.arms[0]],
+                        start_goal_location=[opened_location, closed_location]).resolve().perform()
 
-        ParkArmsAction([Arms.BOTH]).resolve().perform()
+            ParkArmsAction([Arms.BOTH]).resolve().perform()
 
-        MoveTorsoAction([TorsoState.MID]).resolve().perform()
+            MoveTorsoAction([TorsoState.MID]).resolve().perform()
 
-        # Find a pose to place the spoon, move and then place it
-        spoon_target_pose = Pose([4.85, 3.3, 0.8], [0, 0, 1, 1])
-        placing_loc = CostmapLocation(target=spoon_target_pose, reachable_for=robot_desig.resolve(), reachable_arm=pickup_arm, used_grasps=[Grasp.TOP]).resolve()
+            # Find a pose to place the spoon, move and then place it
+            placing_loc = CostmapLocation(target=spoon_target_pose, reachable_for=robot_desig.resolve(),
+                                          reachable_arm=pickup_arm, used_grasps=[Grasp.TOP],
+                                          object_in_hand=spoon_desig).resolve()
 
-        NavigateAction([placing_loc.pose]).resolve().perform()
+            NavigateAction([placing_loc.pose]).resolve().perform()
 
-        PlaceAction(spoon_desig, [spoon_target_pose], [pickup_arm]).resolve().perform()
+            PlaceAction(spoon_desig, [spoon_target_pose], [pickup_arm]).resolve().perform()
 
-        ParkArmsAction([Arms.BOTH]).resolve().perform()
+            ParkArmsAction([Arms.BOTH]).resolve().perform()

demos/pycram_virtual_building_demos/setup/setup_launch_robot.py

@@ -54,18 +54,35 @@ def launch_donbot():
     args = ["robot:=donbot"]
     launch_robot(executable, args=args)
 
+def launch_armar():
+    # name = 'Armar6'
+    # urdf = 'Armar6.urdf'
+    executable = 'ik_and_description.launch'
+    args = ["robot:=armar"]
+    launch_robot(executable, args=args)
+
+def launch_icub():
+    # name = 'iCub'
+    # urdf = 'iCub.urdf'
+    executable = 'ik_and_description.launch'
+    args = ["robot:=icub"]
+    launch_robot(executable, args=args)
 
 def launch_robot(launch_file, package='pycram', launch_folder='/launch/', args: List[str] = None):
     """
-    General method to start a specified launch file with given parameters.
-    Default location for launch files here is in the folder 'launch' inside the pycram package
+    Starts a specified launch file with given parameters.
+
+    The default location for launch files is the 'launch' folder inside the pycram package.
 
-    :param launch_file: File name of the launch file
-    :param package: Name of the package
-    :param launch_folder: Location of the launch file inside the package
-    :param args: List of arguments to pass onto the launch file
+    Args:
+        launch_file (str): File name of the launch file.
+        package (str): Name of the package.
+        launch_folder (str): Location of the launch file inside the package.
+        args (list): List of arguments to pass to the launch file.
+
+    Returns:
+        None
     """
-    # Suppress all output from the function
     rospath = rospkg.RosPack()
 
     uuid = roslaunch.rlutil.get_or_generate_uuid(None, False)
@@ -82,6 +99,4 @@ def launch_robot(launch_file, package='pycram', launch_folder='/launch/', args:
     launch.start()
 
     rospy.loginfo(f'{launch_file} started')
-
-    # Wait for ik server to launch
     time.sleep(2)

demos/pycram_virtual_building_demos/setup/setup_utils.py

@@ -30,12 +30,26 @@ def update_text(text_widget, new_text):
     text_widget.value = f'<span style="font-size: 40px; color: lightblue;">{new_text}</span>'
 
 
-def get_robot_name(robot_name):
-    if robot_name == 'justin':
-        return 'rollin_justin'
-    elif robot_name == 'donbot':
-        return 'iai_donbot'
-    elif robot_name == 'tiago':
-        return 'tiago_dual'
-
-    return robot_name
+def get_robot_name(robot_name: str):
+    """
+    Retrieves the internal identifier for a specified robot name.
+
+    This function maps a given `robot_name` to a predefined internal identifier.
+    If `robot_name` does not match any known names, it returns the input unchanged.
+
+    Args:
+        robot_name (str): The name of the robot to map to its internal identifier.
+
+    Returns:
+        str: The internal identifier for the specified robot name, or the input
+        `robot_name` if no predefined match is found.
+    """
+    robot_name_map = {
+        'justin': 'rollin_justin',
+        'donbot': 'iai_donbot',
+        'tiago': 'tiago_dual',
+        'armar': 'Armar6',
+        'icub': 'iCub'
+    }
+
+    return robot_name_map.get(robot_name, robot_name)

demos/pycram_virtual_building_demos/setup_demo_manager.py

@@ -6,7 +6,6 @@
 from demos.pycram_virtual_building_demos.src.follow_demo import follow_simple_example
 from demos.pycram_virtual_building_demos.src.generlized_actions_demo import start_generalized_demo
 from demos.pycram_virtual_building_demos.src.transport_demo import transporting_demo
-from pycram.utils import suppress_stdout_stderr
 
 # sys.path.insert(0, '/home/vee/robocup_workspaces/pycram_ws/src/pycram')
 sys.path.insert(0, '/home/jovyan/workspace/ros/src/pycram')
@@ -42,6 +41,10 @@ def start_demo():
     update_text(text_widget, 'Loading process~ Please wait...')
 
     world = BulletWorld(WorldMode.DIRECT)
+
+    # Set this to True to publish costmaps and axis marker during the demo. May slow down the simulation.
+    world.allow_publish_debug_poses = False
+
     VizMarkerPublisher()
     robot = Object(robot_name, ObjectType.ROBOT, f"{robot_name}{extension}", pose=Pose([1, 2, 0]))
     apartment = Object(environment_param, ObjectType.ENVIRONMENT, f"{environment_param}{extension}")
@@ -85,11 +88,12 @@ def demo_selecting(apartment, robot, task_param):
     elif task_param == "follow":
         follow_simple_example(robot)
     elif task_param == "transporting":
-        specialized_task = rospy.get_param('/nbparam_specialized_task')
+        specialized_task = None
+        # specialized_task = rospy.get_param('/nbparam_specialized_task')
         if specialized_task == "clean":
-            transporting_demo(apartment, robot)
-        else:
             cleanup_demo(apartment, robot)
+        else:
+            transporting_demo(apartment, robot)
     elif task_param in ["cutting", "mixing", "pouring"]:
         # object_target = rospy.get_param('/nbparam_object')
         # object_tool = rospy.get_param('/nbparam_object_tool')
@@ -102,7 +106,6 @@ def demo_selecting(apartment, robot, task_param):
         else:
             object_target = "banana"
             object_tool = "butter_knife"
-
         specialized_task = rospy.get_param('/nbparam_specialized_task')
         start_generalized_demo(task_param, object_tool, object_target, specialized_task)