[external-int] Minor changes and doc

demos/pycram_bullet_world_demo/demo.py

@@ -49,48 +49,48 @@ def move_and_detect(obj_type):
 
     milk_desig = move_and_detect(ObjectType.MILK)
 
-    # TransportAction(milk_desig, ["left"], [Pose([4.8, 3.55, 0.8])]).resolve().perform()
-    #
-    # cereal_desig = move_and_detect(ObjectType.BREAKFAST_CEREAL)
-    #
-    # TransportAction(cereal_desig, ["right"], [Pose([5.2, 3.4, 0.8], [0, 0, 1, 1])]).resolve().perform()
-    #
-    # bowl_desig = move_and_detect(ObjectType.BOWL)
-    #
-    # TransportAction(bowl_desig, ["left"], [Pose([5, 3.3, 0.8], [0, 0, 1, 1])]).resolve().perform()
-    #
-    # # Finding and navigating to the drawer holding the spoon
-    # handle_desig = ObjectPart(names=["handle_cab10_t"], 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("handle_cab10_t")]).resolve().perform()
-    #
-    # spoon_desig = DetectAction(BelieveObject(types=[ObjectType.SPOON])).resolve().perform()
-    #
-    # pickup_arm = "left" if drawer_open_loc.arms[0] == "right" else "right"
-    # PickUpAction(spoon_desig, [pickup_arm], ["top"]).resolve().perform()
-    #
-    # ParkArmsAction([Arms.LEFT if pickup_arm == "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([4.85, 3.3, 0.8], [0, 0, 1, 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()
+    TransportAction(milk_desig, ["left"], [Pose([4.8, 3.55, 0.8])]).resolve().perform()
+
+    cereal_desig = move_and_detect(ObjectType.BREAKFAST_CEREAL)
+
+    TransportAction(cereal_desig, ["right"], [Pose([5.2, 3.4, 0.8], [0, 0, 1, 1])]).resolve().perform()
+
+    bowl_desig = move_and_detect(ObjectType.BOWL)
+
+    TransportAction(bowl_desig, ["left"], [Pose([5, 3.3, 0.8], [0, 0, 1, 1])]).resolve().perform()
+
+    # Finding and navigating to the drawer holding the spoon
+    handle_desig = ObjectPart(names=["handle_cab10_t"], 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("handle_cab10_t")]).resolve().perform()
+
+    spoon_desig = DetectAction(BelieveObject(types=[ObjectType.SPOON])).resolve().perform()
+
+    pickup_arm = "left" if drawer_open_loc.arms[0] == "right" else "right"
+    PickUpAction(spoon_desig, [pickup_arm], ["top"]).resolve().perform()
+
+    ParkArmsAction([Arms.LEFT if pickup_arm == "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([4.85, 3.3, 0.8], [0, 0, 1, 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()

src/pycram/external_interfaces/giskard.py

@@ -6,11 +6,11 @@
 import sys
 import rosnode
 
-from pycram.datastructures.pose import Pose
+from ..datastructures.pose import Pose
 from ..robot_descriptions import robot_description
-from pycram.world import World
-from pycram.datastructures.dataclasses import MeshVisualShape
-from pycram.world_concepts.world_object import Object
+from ..world import World
+from ..datastructures.dataclasses import MeshVisualShape
+from ..world_concepts.world_object import Object
 from ..robot_description import ManipulatorDescription
 
 from typing_extensions import List, Dict, Callable, Optional
@@ -24,7 +24,7 @@
 except ModuleNotFoundError as e:
     rospy.logwarn("Failed to import Giskard messages, the real robot will not be available")
 
-giskard_wrapper: GiskardWrapper = None
+giskard_wrapper = None
 giskard_update_service = None
 is_init = False
 
@@ -69,6 +69,7 @@ def wrapper(*args, **kwargs):
         elif is_init and "/giskard" not in rosnode.get_node_names():
             rospy.logwarn("Giskard node is not available anymore, could not initialize giskard interface")
             is_init = False
+            giskard_wrapper = None
             return
 
         if "giskard_msgs" not in sys.modules:

src/pycram/external_interfaces/ik.py

@@ -195,7 +195,9 @@ def request_kdl_ik(target_pose: Pose, robot: Object, joints: List[str], gripper:
 
 def request_giskard_ik(target_pose: Pose, robot: Object, gripper: str) -> Tuple[Pose, Dict[str, float]]:
     """
-    Calls giskard in projection mode and queries the ik solution for a full body ik solution.
+    Calls giskard in projection mode and queries the ik solution for a full body ik solution. This method will
+    try to drive the robot directly to a pose from which the target_pose is reachable for the end effector. If there
+    are obstacles in the way this method will fail. In this case please use the GiskardLocation designator. 
 
     :param target_pose: Pose at which the end effector should be moved.
     :param robot: Robot object which should be used.