Merge pull request #161 from Tigul/full-body-giskard

Full body giskard

demos/pycram_bullet_world_demo/demo.py

@@ -2,7 +2,7 @@
 from pycram.designators.action_designator import *
 from pycram.designators.location_designator import *
 from pycram.designators.object_designator import *
-from pycram.datastructures.enums import ObjectType
+from pycram.datastructures.enums import ObjectType, WorldMode
 from pycram.datastructures.pose import Pose
 from pycram.process_module import simulated_robot, with_simulated_robot
 from pycram.object_descriptors.urdf import ObjectDescription
@@ -11,7 +11,7 @@
 
 extension = ObjectDescription.get_file_extension()
 
-world = BulletWorld()
+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}")
 

examples/location_designator.ipynb

@@ -457,6 +457,32 @@
     "access_location = AccessingLocation(handle_desig.resolve(), robot_desig.resolve()).resolve()\n",
     "print(access_location.pose)"
    ]
+  },
+  {
+   "metadata": {},
+   "cell_type": "markdown",
+   "source": [
+    "## Giskard Location\n",
+    "Some robots like the HSR or the Stretch2 need a full-body ik solver to utilize the whole body. For this case robots the ```GiskardLocation``` can be used. This location designator uses giskard as an ik solver to find a pose for the robot to reach a target pose. \n",
+    "\n",
+    "**Note:** The GiskardLocation relies on Giskard, therefore Giskard needs to run in order for this Location Designator to work."
+   ],
+   "id": "382afb4196242da8"
+  },
+  {
+   "metadata": {},
+   "cell_type": "code",
+   "outputs": [],
+   "execution_count": null,
+   "source": [
+    "from pycram.resolver.location.giskard_location import GiskardLocation\n",
+    "\n",
+    "robot_desig = BelieveObject(names=[\"pr2\"]).resolve()\n",
+    "\n",
+    "loc = GiskardLocation(target=Pose([1, 1, 1]), reachable_for=robot_desig).resolve()\n",
+    "print(loc.pose)"
+   ],
+   "id": "8154fec6aeebad00"
   }
  ],
  "metadata": {

src/pycram/external_interfaces/giskard.py

@@ -1,27 +1,28 @@
 import json
 import threading
+import time
 
 import rospy
 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
 from geometry_msgs.msg import PoseStamped, PointStamped, QuaternionStamped, Vector3Stamped
 from threading import Lock, RLock
 
 try:
-    from giskardpy.python_interface import GiskardWrapper
+    from giskardpy.python_interface.old_python_interface import OldGiskardWrapper as GiskardWrapper
     from giskard_msgs.msg import WorldBody, MoveResult, CollisionEntry
     from giskard_msgs.srv import UpdateWorldRequest, UpdateWorld, UpdateWorldResponse, RegisterGroupResponse
 except ModuleNotFoundError as e:
-    rospy.logwarn("Unable to import Giskard messages. Real robot not available")
+    rospy.logwarn("Failed to import Giskard messages, the real robot will not be available")
 
 giskard_wrapper = None
 giskard_update_service = None
@@ -68,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:
@@ -97,7 +99,7 @@ def initial_adding_objects() -> None:
     """
     groups = giskard_wrapper.get_group_names()
     for obj in World.current_world.objects:
-        if obj is World.robot:
+        if obj is World.robot or obj is World.current_world.get_prospection_object_for_object(World.robot):
             continue
         name = obj.name + "_" + str(obj.id)
         if name not in groups:
@@ -218,51 +220,65 @@ def _manage_par_motion_goals(goal_func, *args) -> Optional['MoveResult']:
     :param args: Arguments for the ``goal_func`` function
     :return: MoveResult of the execution if there was an execution, True if a new motion goal was added to the giskard_wrapper and None in any other case
     """
+    # key is the instance of the parallel language element, value is a list of threads that should be executed in
+    # parallel
     for key, value in par_threads.items():
+        # if the current thread is in the list of threads that should be executed in parallel backup the current list
+        # of motion goals and monitors
         if threading.get_ident() in value:
-            tmp = giskard_wrapper.cmd_seq
+            tmp_goals = giskard_wrapper.motion_goals.get_goals()
+            tmp_monitors = giskard_wrapper.monitors.get_monitors()
 
             if key in par_motion_goal.keys():
-                giskard_wrapper.cmd_seq = par_motion_goal[key]
+                # giskard_wrapper.cmd_seq = par_motion_goal[key]
+                giskard_wrapper.motion_goals._goals = par_motion_goal[key][0]
+                giskard_wrapper.monitors._monitors = par_motion_goal[key][1]
             else:
-                giskard_wrapper.clear_cmds()
+                giskard_wrapper.clear_motion_goals_and_monitors()
 
             goal_func(*args)
 
             # Check if there are multiple constraints that use the same joint, if this is the case the
             used_joints = set()
-            for cmd in giskard_wrapper.cmd_seq:
-                for con in cmd.constraints:
-                    par_value_pair = json.loads(con.parameter_value_pair)
-                    if "tip_link" in par_value_pair.keys() and "root_link" in par_value_pair.keys():
-                        if par_value_pair["tip_link"] == robot_description.base_link:
-                            continue
-                        chain = World.robot.description.get_chain(par_value_pair["root_link"],
-                                                                  par_value_pair["tip_link"])
-                        if set(chain).intersection(used_joints) != set():
-                            giskard_wrapper.cmd_seq = tmp
-                            raise AttributeError(f"The joint(s) {set(chain).intersection(used_joints)} is used by multiple Designators")
-                        else:
-                            [used_joints.add(joint) for joint in chain]
-
-                    elif "goal_state" in par_value_pair.keys():
-                        if set(par_value_pair["goal_state"].keys()).intersection(used_joints) != set():
-                            giskard_wrapper.cmd_seq = tmp
-                            raise AttributeError(f"The joint(s) {set(par_value_pair['goal_state'].keys()).intersection(used_joints)} is used by multiple Designators")
-                        else:
-                            [used_joints.add(joint) for joint in par_value_pair["goal_state"].keys()]
+            for cmd in giskard_wrapper.motion_goals.get_goals():
+                par_value_pair = json.loads(cmd.kwargs)
+                if "tip_link" in par_value_pair.keys() and "root_link" in par_value_pair.keys():
+                    if par_value_pair["tip_link"] == robot_description.base_link:
+                        continue
+                    chain = World.robot.description.get_chain(par_value_pair["root_link"],
+                                                              par_value_pair["tip_link"])
+                    if set(chain).intersection(used_joints) != set():
+                        giskard_wrapper.motion_goals._goals = tmp_goals
+                        giskard_wrapper.monitors._monitors = tmp_monitors
+                        raise AttributeError(f"The joint(s) {set(chain).intersection(used_joints)} is used by multiple Designators")
+                    else:
+                        [used_joints.add(joint) for joint in chain]
+
+                elif "goal_state" in par_value_pair.keys():
+                    if set(par_value_pair["goal_state"].keys()).intersection(used_joints) != set():
+                        giskard_wrapper.motion_goals._goals = tmp_goals
+                        giskard_wrapper.monitors._monitors = tmp_monitors
+                        raise AttributeError(f"The joint(s) {set(par_value_pair['goal_state'].keys()).intersection(used_joints)} is used by multiple Designators")
+                    else:
+                        [used_joints.add(joint) for joint in par_value_pair["goal_state"].keys()]
 
             par_threads[key].remove(threading.get_ident())
+            # If this is the last thread that should be executed in parallel, execute the complete sequence of motion
+            # goals
             if len(par_threads[key]) == 0:
                 if key in par_motion_goal.keys():
                     del par_motion_goal[key]
                 del par_threads[key]
-                res = giskard_wrapper.plan_and_execute()
-                giskard_wrapper.cmd_seq = tmp
+                res = giskard_wrapper.execute()
+                giskard_wrapper.motion_goals._goals = tmp_goals
+                giskard_wrapper.monitors._monitors = tmp_monitors
                 return res
+            # If there are still threads that should be executed in parallel, save the current state of motion goals and
+            # monitors.
             else:
-                par_motion_goal[key] = giskard_wrapper.cmd_seq
-                giskard_wrapper.cmd_seq = tmp
+                par_motion_goal[key] = [giskard_wrapper.motion_goals.get_goals(), giskard_wrapper.monitors.get_monitors()]
+                giskard_wrapper.motion_goals._goals = tmp_goals
+                giskard_wrapper.monitors._monitors = tmp_monitors
                 return True
 
 
@@ -282,7 +298,7 @@ def achieve_joint_goal(goal_poses: Dict[str, float]) -> 'MoveResult':
         return par_return
 
     giskard_wrapper.set_joint_goal(goal_poses)
-    return giskard_wrapper.plan_and_execute()
+    return giskard_wrapper.execute()
 
 
 @init_giskard_interface
@@ -304,7 +320,7 @@ def achieve_cartesian_goal(goal_pose: Pose, tip_link: str, root_link: str) -> 'M
         return par_return
 
     giskard_wrapper.set_cart_goal(_pose_to_pose_stamped(goal_pose), tip_link, root_link)
-    return giskard_wrapper.plan_and_execute()
+    return giskard_wrapper.execute()
 
 
 @init_giskard_interface
@@ -327,7 +343,7 @@ def achieve_straight_cartesian_goal(goal_pose: Pose, tip_link: str,
         return par_return
 
     giskard_wrapper.set_straight_cart_goal(_pose_to_pose_stamped(goal_pose), tip_link, root_link)
-    return giskard_wrapper.plan_and_execute()
+    return giskard_wrapper.execute()
 
 
 @init_giskard_interface
@@ -349,7 +365,7 @@ def achieve_translation_goal(goal_point: List[float], tip_link: str, root_link:
         return par_return
 
     giskard_wrapper.set_translation_goal(make_point_stamped(goal_point), tip_link, root_link)
-    return giskard_wrapper.plan_and_execute()
+    return giskard_wrapper.execute()
 
 
 @init_giskard_interface
@@ -372,7 +388,7 @@ def achieve_straight_translation_goal(goal_point: List[float], tip_link: str, ro
         return par_return
 
     giskard_wrapper.set_straight_translation_goal(make_point_stamped(goal_point), tip_link, root_link)
-    return giskard_wrapper.plan_and_execute()
+    return giskard_wrapper.execute()
 
 
 @init_giskard_interface
@@ -394,7 +410,7 @@ def achieve_rotation_goal(quat: List[float], tip_link: str, root_link: str) -> '
         return par_return
 
     giskard_wrapper.set_rotation_goal(make_quaternion_stamped(quat), tip_link, root_link)
-    return giskard_wrapper.plan_and_execute()
+    return giskard_wrapper.execute()
 
 
 @init_giskard_interface
@@ -420,7 +436,7 @@ def achieve_align_planes_goal(goal_normal: List[float], tip_link: str, tip_norma
     giskard_wrapper.set_align_planes_goal(make_vector_stamped(goal_normal), tip_link,
                                           make_vector_stamped(tip_normal),
                                           root_link)
-    return giskard_wrapper.plan_and_execute()
+    return giskard_wrapper.execute()
 
 
 @init_giskard_interface
@@ -439,7 +455,7 @@ def achieve_open_container_goal(tip_link: str, environment_link: str) -> 'MoveRe
     if par_return:
         return par_return
     giskard_wrapper.set_open_container_goal(tip_link, environment_link)
-    return giskard_wrapper.plan_and_execute()
+    return giskard_wrapper.execute()
 
 
 @init_giskard_interface
@@ -459,7 +475,65 @@ def achieve_close_container_goal(tip_link: str, environment_link: str) -> 'MoveR
         return par_return
 
     giskard_wrapper.set_close_container_goal(tip_link, environment_link)
-    return giskard_wrapper.plan_and_execute()
+    return giskard_wrapper.execute()
+
+# Projection Goals
+
+
+@init_giskard_interface
+def projection_cartesian_goal(goal_pose: Pose, tip_link: str, root_link: str) -> 'MoveResult':
+    """
+    Tries to move the tip_link to the position defined by goal_pose using the chain defined by tip_link and root_link.
+    The goal_pose is projected to the closest point on the robot's workspace.
+
+    :param goal_pose: The position which should be achieved with tip_link
+    :param tip_link: The end link of the chain as well as the link which should achieve the goal_pose
+    :param root_link: The starting link of the chain which should be used to achieve this goal
+    :return: MoveResult message for this goal
+    """
+    sync_worlds()
+    giskard_wrapper.set_cart_goal(_pose_to_pose_stamped(goal_pose), tip_link, root_link)
+    return giskard_wrapper.projection()
+
+
+@init_giskard_interface
+def projection_cartesian_goal_with_approach(approach_pose: Pose, goal_pose: Pose, tip_link: str, root_link: str,
+                                            robot_base_link: str) -> 'MoveResult':
+    """
+    Tries to achieve the goal_pose using the chain defined by tip_link and root_link. The approach_pose is used to drive
+    the robot to a pose close the actual goal pose, the robot_base_link is used to define the base link of the robot.
+
+    :param approach_pose: Pose near the goal_pose
+    :param goal_pose: Pose to which the tip_link should be moved
+    :param tip_link: The link which should be moved to goal_pose, usually the tool frame
+    :param root_link: The start of the link chain which should be used for planning
+    :param robot_base_link: The base link of the robot
+    :return: A trajectory calculated to move the tip_link to the goal_pose
+    """
+    sync_worlds()
+    giskard_wrapper.allow_all_collisions()
+    giskard_wrapper.set_cart_goal(_pose_to_pose_stamped(approach_pose), robot_base_link, "map")
+    giskard_wrapper.projection()
+    giskard_wrapper.avoid_all_collisions()
+    giskard_wrapper.set_cart_goal(_pose_to_pose_stamped(goal_pose), tip_link, root_link)
+    return giskard_wrapper.projection()
+
+
+@init_giskard_interface
+def projection_joint_goal(goal_poses: Dict[str, float], allow_collisions: bool = False) -> 'MoveResult':
+    """
+    Tries to achieve the joint goal defined by goal_poses, the goal_poses are projected to the closest point on the
+    robot's workspace.
+
+    :param goal_poses: Dictionary with joint names and position goals
+    :param allow_collisions: If all collisions should be allowed for this goal
+    :return: MoveResult message for this goal
+    """
+    sync_worlds()
+    if allow_collisions:
+        giskard_wrapper.allow_all_collisions()
+    giskard_wrapper.set_joint_goal(goal_poses)
+    return giskard_wrapper.projection()
 
 
 # Managing collisions