From 46389792efe0cd80d969f1da86e55fc3e0a5a5f0 Mon Sep 17 00:00:00 2001
From: Jonas Dech <j.dech@gmx.net>
Date: Thu, 18 Apr 2024 10:08:53 +0200
Subject: [PATCH 01/11] [giskard/ik] Added full body ik using giskard
 projection

---
 src/pycram/external_interfaces/giskard.py | 41 +++++++++++-----
 src/pycram/external_interfaces/ik.py      | 60 +++++++++++++++++++++--
 2 files changed, 86 insertions(+), 15 deletions(-)

diff --git a/src/pycram/external_interfaces/giskard.py b/src/pycram/external_interfaces/giskard.py
index bd88af990..168684a2e 100644
--- a/src/pycram/external_interfaces/giskard.py
+++ b/src/pycram/external_interfaces/giskard.py
@@ -1,5 +1,6 @@
 import json
 import threading
+import time
 
 import rospy
 import sys
@@ -17,7 +18,7 @@
 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:
@@ -257,7 +258,7 @@ def _manage_par_motion_goals(goal_func, *args) -> Optional['MoveResult']:
                 if key in par_motion_goal.keys():
                     del par_motion_goal[key]
                 del par_threads[key]
-                res = giskard_wrapper.plan_and_execute()
+                res = giskard_wrapper.execute()
                 giskard_wrapper.cmd_seq = tmp
                 return res
             else:
@@ -282,7 +283,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 +305,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 +328,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 +350,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 +373,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 +395,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 +421,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 +440,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 +460,25 @@ 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_cartisian_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()
 
 
 # Managing collisions
diff --git a/src/pycram/external_interfaces/ik.py b/src/pycram/external_interfaces/ik.py
index a6c73f328..ae8aea4b4 100644
--- a/src/pycram/external_interfaces/ik.py
+++ b/src/pycram/external_interfaces/ik.py
@@ -1,4 +1,6 @@
-from typing_extensions import List, Union
+import rosnode
+import tf
+from typing_extensions import List, Union, Tuple, Dict
 
 import rospy
 from moveit_msgs.msg import PositionIKRequest
@@ -6,12 +8,14 @@
 from moveit_msgs.srv import GetPositionIK
 from sensor_msgs.msg import JointState
 
-from pycram.world_concepts.world_object import Object
+from ..world import World, UseProspectionWorld
+from ..world_concepts.world_object import Object
 from ..helper import calculate_wrist_tool_offset, _apply_ik
-from pycram.local_transformer import LocalTransformer
-from pycram.datastructures.pose import Pose
+from ..local_transformer import LocalTransformer
+from ..datastructures.pose import Pose
 from ..robot_descriptions import robot_description
 from ..plan_failures import IKError
+from ..external_interfaces.giskard import projection_cartisian_goal, achieve_cartesian_goal
 
 
 def _make_request_msg(root_link: str, tip_link: str, target_pose: Pose, robot_object: Object,
@@ -70,6 +74,7 @@ def call_ik(root_link: str, tip_link: str, target_pose: Pose, robot_object: Obje
     else:
         ik_service = "/kdl_ik_service/get_ik"
 
+    rospy.loginfo_once(f"Waiting for IK service: {ik_service}")
     rospy.wait_for_service(ik_service)
 
     req = _make_request_msg(root_link, tip_link, target_pose, robot_object, joints)
@@ -145,6 +150,12 @@ def try_to_reach(pose_or_object: Union[Pose, Object], prospection_robot: Object,
 
 
 def request_ik(target_pose: Pose, robot: Object, joints: List[str], gripper: str) -> List[float]:
+    if "/giskard" not in rosnode.get_node_names():
+        return request_kdl_ik(target_pose, robot, joints, gripper)
+    return request_giskard_ik(target_pose, robot, joints, gripper)
+
+
+def request_kdl_ik(target_pose: Pose, robot: Object, joints: List[str], gripper: str) -> List[float]:
     """
     Top-level method to request ik solution for a given pose. Before calling the ik service the links directly before
     and after the joint chain will be queried and the target_pose will be transformed into the frame of the root_link.
@@ -170,3 +181,44 @@ def request_ik(target_pose: Pose, robot: Object, joints: List[str], gripper: str
     inv = call_ik(base_link, end_effector, target_diff, robot, joints)
 
     return inv
+
+
+def request_giskard_ik(target_pose: Pose, robot: Object, joints: List[str], gripper: str) -> Tuple[Pose, Dict[str, float]]:
+    """
+    Calls giskard in projection mode and queries the ik solution for a full body ik solution.
+
+    :param target_pose: Pose at which the end effector should be moved.
+    :param robot: Robot object which should be used.
+    :param joints: List of joints that should be used in computation.
+    :param gripper: Name of the tool frame which should grasp, this should be at the end of the given joint chain.
+    :return: A list of joint values.
+    """
+    local_transformer = LocalTransformer()
+    target_map = local_transformer.transform_pose(target_pose, "map")
+
+    result = projection_cartisian_goal(target_map, gripper, "map")
+    last_point = result.trajectory.points[-1]
+    joint_names = result.trajectory.joint_names
+
+    joint_states = dict(zip(joint_names, last_point.positions))
+    prospection_robot = World.current_world.get_prospection_object_for_object(robot)
+
+    orientation = list(tf.transformations.quaternion_from_euler(0, 0, joint_states["brumbrum_yaw"], axes="sxyz"))
+    pose = Pose([joint_states["brumbrum_x"], joint_states["brumbrum_y"], 0], orientation)
+
+    robot_joint_states = {}
+    for joint_name, state in joint_states.items():
+        if joint_name in robot.joints.keys():
+            robot_joint_states[joint_name] = state
+
+    with UseProspectionWorld():
+        prospection_robot.set_joint_positions(robot_joint_states)
+        prospection_robot.set_pose(pose)
+
+        tip_pose = prospection_robot.get_link_pose(gripper)
+        dist = tip_pose.dist(target_map)
+
+        if dist > 0.01:
+            raise IKError(target_pose, "map")
+        return pose, robot_joint_states
+

From 1619991d36156bef4f62362248fdc5635eadb039 Mon Sep 17 00:00:00 2001
From: Jonas Dech <j.dech@gmx.net>
Date: Fri, 19 Apr 2024 09:49:57 +0200
Subject: [PATCH 02/11] [ik] Changed ik interface to full body

---
 src/neem_interface_python                     |  2 +-
 src/pycram/external_interfaces/giskard.py     |  4 +-
 src/pycram/external_interfaces/ik.py          | 24 +++++--
 src/pycram/helper.py                          | 16 ++---
 src/pycram/pose_generator_and_validator.py    | 12 ++--
 .../process_modules/boxy_process_modules.py   |  2 +-
 .../default_process_modules.py                |  2 +-
 .../process_modules/hsr_process_modules.py    |  5 +-
 .../process_modules/pr2_process_modules.py    |  2 +-
 .../stretch_process_modules.py                |  9 +--
 .../resolver/location/giskard_location.py     | 68 ++++++++-----------
 11 files changed, 72 insertions(+), 74 deletions(-)

diff --git a/src/neem_interface_python b/src/neem_interface_python
index 51aa0f9a3..05ad42c41 160000
--- a/src/neem_interface_python
+++ b/src/neem_interface_python
@@ -1 +1 @@
-Subproject commit 51aa0f9a3b38fde3156bdf29be7aad7bb9547926
+Subproject commit 05ad42c41042335dd2287d865f6a37c115ea8ffe
diff --git a/src/pycram/external_interfaces/giskard.py b/src/pycram/external_interfaces/giskard.py
index 168684a2e..b7e721c5c 100644
--- a/src/pycram/external_interfaces/giskard.py
+++ b/src/pycram/external_interfaces/giskard.py
@@ -24,7 +24,7 @@
 except ModuleNotFoundError as e:
     rospy.logwarn("Failed to import Giskard messages")
 
-giskard_wrapper = None
+giskard_wrapper: GiskardWrapper = None
 giskard_update_service = None
 is_init = False
 
@@ -98,7 +98,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:
diff --git a/src/pycram/external_interfaces/ik.py b/src/pycram/external_interfaces/ik.py
index ae8aea4b4..bccdf3cde 100644
--- a/src/pycram/external_interfaces/ik.py
+++ b/src/pycram/external_interfaces/ik.py
@@ -149,13 +149,23 @@ def try_to_reach(pose_or_object: Union[Pose, Object], prospection_robot: Object,
     return input_pose
 
 
-def request_ik(target_pose: Pose, robot: Object, joints: List[str], gripper: str) -> List[float]:
+def request_ik(target_pose: Pose, robot: Object, joints: List[str], gripper: str) -> Tuple[Pose, Dict[str, float]]:
+    """
+    Top-level method to request ik solution for a given pose. This method will check if the giskard node is running
+    and if so will call the giskard service. If the giskard node is not running the kdl_ik_service will be called.
+
+    :param target_pose: Pose of the end-effector for which an ik solution should be found
+    :param robot: The robot object which should be used
+    :param joints: A list of joints that should be used in computation, this is only relevant for the kdl_ik_service
+    :param gripper: Name of the tool frame which should grasp, this should be at the end of the given joint chain
+    :return: A Pose at which the robt should stand as well as a dictionary of joint values
+    """
     if "/giskard" not in rosnode.get_node_names():
-        return request_kdl_ik(target_pose, robot, joints, gripper)
-    return request_giskard_ik(target_pose, robot, joints, gripper)
+        return robot.pose, request_kdl_ik(target_pose, robot, joints, gripper)
+    return request_giskard_ik(target_pose, robot, gripper)
 
 
-def request_kdl_ik(target_pose: Pose, robot: Object, joints: List[str], gripper: str) -> List[float]:
+def request_kdl_ik(target_pose: Pose, robot: Object, joints: List[str], gripper: str) -> Dict[str, float]:
     """
     Top-level method to request ik solution for a given pose. Before calling the ik service the links directly before
     and after the joint chain will be queried and the target_pose will be transformed into the frame of the root_link.
@@ -180,19 +190,19 @@ def request_kdl_ik(target_pose: Pose, robot: Object, joints: List[str], gripper:
 
     inv = call_ik(base_link, end_effector, target_diff, robot, joints)
 
-    return inv
+    return dict(zip(joints, inv))
 
 
-def request_giskard_ik(target_pose: Pose, robot: Object, joints: List[str], gripper: str) -> Tuple[Pose, Dict[str, float]]:
+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.
 
     :param target_pose: Pose at which the end effector should be moved.
     :param robot: Robot object which should be used.
-    :param joints: List of joints that should be used in computation.
     :param gripper: Name of the tool frame which should grasp, this should be at the end of the given joint chain.
     :return: A list of joint values.
     """
+    rospy.loginfo_once(f"Using Giskard for full body IK")
     local_transformer = LocalTransformer()
     target_map = local_transformer.transform_pose(target_pose, "map")
 
diff --git a/src/pycram/helper.py b/src/pycram/helper.py
index 672d9951f..931ebb736 100644
--- a/src/pycram/helper.py
+++ b/src/pycram/helper.py
@@ -7,7 +7,7 @@
 GeneratorList -- implementation of generator list wrappers.
 """
 from inspect import isgeneratorfunction
-from typing_extensions import List, Tuple, Callable
+from typing_extensions import List, Tuple, Callable, Dict
 
 import numpy as np
 from pytransform3d.rotations import quaternion_wxyz_from_xyzw, quaternion_xyzw_from_wxyz
@@ -37,21 +37,17 @@ class bcolors:
     UNDERLINE = '\033[4m'
 
 
-def _apply_ik(robot: WorldObject, joint_poses: List[float], joints: List[str]) -> None:
+def _apply_ik(robot: WorldObject, pose_and_joint_poses: Tuple[Pose, Dict[str, float]]) -> None:
     """
     Apllies a list of joint poses calculated by an inverse kinematics solver to a robot
 
     :param robot: The robot the joint poses should be applied on
-    :param joint_poses: The joint poses to be applied
-    :param gripper: specifies the gripper for which the ik solution should be applied
+    :param pose_and_joint_poses: The base pose and joint states as returned by the ik solver
     :return: None
     """
-    # arm ="left" if gripper == robot_description.get_tool_frame("left") else "right"
-    # ik_joints = [robot_description.torso_joint] + robot_description._safely_access_chains(arm).joints
-    # ik_joints = robot_description._safely_access_chains(arm).joints
-    robot.set_joint_positions(dict(zip(joints, joint_poses)))
-    # for i in range(0, len(joints)):
-    #     robot.set_joint_state(joints[i], joint_poses[i])
+    pose, joint_states = pose_and_joint_poses
+    robot.set_pose(pose)
+    robot.set_joint_positions(joint_states)
 
 
 def calculate_wrist_tool_offset(wrist_frame: str, tool_frame: str, robot: WorldObject) -> Transform:
diff --git a/src/pycram/pose_generator_and_validator.py b/src/pycram/pose_generator_and_validator.py
index 4582699cc..ebee20ded 100644
--- a/src/pycram/pose_generator_and_validator.py
+++ b/src/pycram/pose_generator_and_validator.py
@@ -201,13 +201,17 @@ def reachability_validator(pose: Pose,
         joint_state_before_ik = robot.get_positions_of_all_joints()
         try:
             # test the possible solution and apply it to the robot
-            resp = request_ik(target, robot, joints, tool_frame)
-            _apply_ik(robot, resp, joints)
+            pose, joint_states = request_ik(target, robot, joints, tool_frame)
+            robot.set_pose(pose)
+            robot.set_joint_positions(joint_states)
+            # _apply_ik(robot, resp, joints)
 
             in_contact = collision_check(robot, allowed_collision)
             if not in_contact:  # only check for retract pose if pose worked
-                resp = request_ik(retract_target_pose, robot, joints, tool_frame)
-                _apply_ik(robot, resp, joints)
+                pose, joint_states = request_ik(retract_target_pose, robot, joints, tool_frame)
+                robot.set_pose(pose)
+                robot.set_joint_positions(joint_states)
+                # _apply_ik(robot, resp, joints)
                 in_contact = collision_check(robot, allowed_collision)
             if not in_contact:
                 arms.append(name)
diff --git a/src/pycram/process_modules/boxy_process_modules.py b/src/pycram/process_modules/boxy_process_modules.py
index e33570e52..825db1dc7 100644
--- a/src/pycram/process_modules/boxy_process_modules.py
+++ b/src/pycram/process_modules/boxy_process_modules.py
@@ -196,7 +196,7 @@ def _move_arm_tcp(target: Pose, robot: Object, arm: str) -> None:
     joints = robot_description.chains[arm].joints
 
     inv = request_ik(target, robot, joints, gripper)
-    helper._apply_ik(robot, inv, joints)
+    helper._apply_ik(robot, inv)
 
 
 class BoxyManager(ProcessModuleManager):
diff --git a/src/pycram/process_modules/default_process_modules.py b/src/pycram/process_modules/default_process_modules.py
index fcd10ed05..c2bd8fe85 100644
--- a/src/pycram/process_modules/default_process_modules.py
+++ b/src/pycram/process_modules/default_process_modules.py
@@ -175,7 +175,7 @@ def _move_arm_tcp(target: Pose, robot: Object, arm: str) -> None:
     joints = robot_description.chains[arm].joints
 
     inv = request_ik(target, robot, joints, gripper)
-    _apply_ik(robot, inv, joints)
+    _apply_ik(robot, inv)
 
 
 class DefaultManager(ProcessModuleManager):
diff --git a/src/pycram/process_modules/hsr_process_modules.py b/src/pycram/process_modules/hsr_process_modules.py
index 3f02d466c..12bf0fb14 100644
--- a/src/pycram/process_modules/hsr_process_modules.py
+++ b/src/pycram/process_modules/hsr_process_modules.py
@@ -25,8 +25,7 @@ def calculate_and_apply_ik(robot, gripper: str, target_position: Point, max_iter
     arm = "right" if gripper == robot_description.get_tool_frame("right") else "left"
     inv = request_ik(Pose(target_position_l, [0, 0, 0, 1]),
                      robot, robot_description.chains[arm].joints, gripper)
-    joints = robot_description.chains[arm].joints
-    _apply_ik(robot, inv, joints)
+    _apply_ik(robot, inv)
 
 
 def _park_arms(arm):
@@ -221,7 +220,7 @@ def _move_arm_tcp(target: Pose, robot: Object, arm: str) -> None:
     joints = robot_description.chains[arm].joints
 
     inv = request_ik(target, robot, joints, gripper)
-    _apply_ik(robot, inv, joints)
+    _apply_ik(robot, inv)
 
 
 ###########################################################
diff --git a/src/pycram/process_modules/pr2_process_modules.py b/src/pycram/process_modules/pr2_process_modules.py
index 0b0d0bede..0f6dbe357 100644
--- a/src/pycram/process_modules/pr2_process_modules.py
+++ b/src/pycram/process_modules/pr2_process_modules.py
@@ -205,7 +205,7 @@ def _move_arm_tcp(target: Pose, robot: Object, arm: str) -> None:
     joints = robot_description.chains[arm].joints
 
     inv = request_ik(target, robot, joints, gripper)
-    _apply_ik(robot, inv, joints)
+    _apply_ik(robot, inv)
 
 
 ###########################################################
diff --git a/src/pycram/process_modules/stretch_process_modules.py b/src/pycram/process_modules/stretch_process_modules.py
index e5aea17d3..dc02f1970 100644
--- a/src/pycram/process_modules/stretch_process_modules.py
+++ b/src/pycram/process_modules/stretch_process_modules.py
@@ -9,6 +9,7 @@
 from .default_process_modules import *
 from ..world import World
 from ..designators.motion_designator import *
+from ..external_interfaces.ik import request_giskard_ik
 
 
 class StretchNavigate(DefaultNavigation):
@@ -127,10 +128,10 @@ def _execute(self, desig: ClosingMotion):
 def _move_arm_tcp(target: Pose, robot: Object, arm: str) -> None:
     gripper = robot_description.get_tool_frame(arm)
 
-    joints = robot_description.chains[arm].joints
-
-    inv = request_ik(target, robot, joints, gripper)
-    _apply_ik(robot, inv, joints)
+    # inv = request_ik(target, robot, joints, gripper)
+    pose, joint_states = request_giskard_ik(target, robot, gripper)
+    robot.set_pose(pose)
+    robot.set_joint_positions(joint_states)
 
 
 ###########################################################
diff --git a/src/pycram/resolver/location/giskard_location.py b/src/pycram/resolver/location/giskard_location.py
index 3d8ee79ec..b5793bd0a 100644
--- a/src/pycram/resolver/location/giskard_location.py
+++ b/src/pycram/resolver/location/giskard_location.py
@@ -1,54 +1,42 @@
-from ...external_interfaces.giskard import achieve_cartesian_goal
+import rospy
+
 from ...designators.location_designator import CostmapLocation
-from pycram.world import UseProspectionWorld, World
-from pycram.datastructures.pose import Pose
+from ...external_interfaces.ik import request_giskard_ik
+from ...robot_description import ManipulatorDescription
+from ...world import UseProspectionWorld, World
+from ...datastructures.pose import Pose
 from ...robot_descriptions import robot_description
-from pycram.pose_generator_and_validator import reachability_validator
+from ...pose_generator_and_validator import reachability_validator
+from ...local_transformer import LocalTransformer
+from ...external_interfaces.giskard import projection_cartisian_goal
+from ...plan_failures import IKError
 from typing_extensions import Tuple, Dict
 
 import tf
 
 
 class GiskardLocation(CostmapLocation):
+    """'
+    Specialization version of the CostmapLocation which uses Giskard to solve for a full-body IK solution. This
+    designator is especially useful for robots which lack a degree of freedom and therefore need to use the base to
+    manipulate the environment effectively.
+    """
 
     def __iter__(self) -> CostmapLocation.Location:
         """
-        Resolves a CostmapLocation for reachability to a specific Location using Giskard. Since Giskard is able to perform
-        full body IK solving we can use this to get the Pose of a robot at which it is able to reach a certain point.
-        This resolver only supports reachable_for and not visible_for
+        Uses Giskard to perform full body ik solving to get the pose of a robot at which it is able to reach a certain point.
 
-        :param desig: A CostmapLocation Designator description
-        :return: An instance of CostmapLocation.Location with a pose from which the robot can reach the target
-        """
-        if self.reachable_for:
-            pose_right, end_config_right = self._get_reachable_pose_for_arm(self.target,
-                                                                            robot_description.get_tool_frame("right"))
-            pose_left, end_config_left = self._get_reachable_pose_for_arm(self.target,
-                                                                          robot_description.get_tool_frame("left"))
-
-            test_robot = World.current_world.get_prospection_object_for_object(World.robot)
-            with UseProspectionWorld():
-                valid, arms = reachability_validator(pose_right, test_robot, self.target, {})
-                if valid:
-                    yield CostmapLocation.Location(pose_right, arms)
-                valid, arms = reachability_validator(pose_left, test_robot, self.target, {})
-                if valid:
-                    yield self.Location(pose_left, arms)
-
-    def _get_reachable_pose_for_arm(self, target: Pose, end_effector_link: str) -> Tuple[Pose, Dict]:
+        :yield: An instance of CostmapLocation.Location with a pose from which the robot can reach the target
         """
-        Calls Giskard to perform full body ik solving between the map and the given end effector link. The end joint
-        configuration of the robot as well as its end pose are then returned.
+        local_transformer = LocalTransformer()
+        target_map = local_transformer.transform_pose(self.target, "map")
+
+        manipulator_descs = list(
+            filter(lambda chain: isinstance(chain[1], ManipulatorDescription), robot_description.chains.items()))
+        for name, chain in manipulator_descs:
+            try:
+                pose, joint_states = request_giskard_ik(target_map, World.robot, chain.tool_frame)
+                yield self.Location(pose, chain.name)
+            except IKError as e:
+                pass
 
-        :param target: The pose which the robots end effector should reach
-        :param end_effector_link: The name of the end effector which should reach the target
-        :return: The end pose of the robot as well as its final joint configuration
-        """
-        giskard_result = achieve_cartesian_goal(target, end_effector_link, "map")
-        joints = giskard_result.trajectory.joint_names
-        trajectory_points = giskard_result.trajectory.points
-
-        end_config = dict(zip(joints, trajectory_points[-1].positions))
-        orientation = list(tf.transformations.quaternion_from_euler(0, 0, end_config["yaw"], axes="sxyz"))
-        pose = Pose([end_config["x"], end_config["y"], 0], orientation)
-        return pose, end_config

From f0f6aa5106e76675a9415f113ef182397987f2e8 Mon Sep 17 00:00:00 2001
From: Jonas Dech <j.dech@gmx.net>
Date: Mon, 22 Apr 2024 14:20:30 +0200
Subject: [PATCH 03/11] [giskard-location] Fixed imports

---
 demos/pycram_bullet_world_demo/demo.py        | 94 +++++++++----------
 src/pycram/external_interfaces/ik.py          |  3 +-
 .../resolver/location/giskard_location.py     |  6 --
 3 files changed, 49 insertions(+), 54 deletions(-)

diff --git a/demos/pycram_bullet_world_demo/demo.py b/demos/pycram_bullet_world_demo/demo.py
index ab15c4483..3c32d9c37 100644
--- a/demos/pycram_bullet_world_demo/demo.py
+++ b/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}")
 
@@ -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()
diff --git a/src/pycram/external_interfaces/ik.py b/src/pycram/external_interfaces/ik.py
index bccdf3cde..b7b44e4bc 100644
--- a/src/pycram/external_interfaces/ik.py
+++ b/src/pycram/external_interfaces/ik.py
@@ -15,7 +15,7 @@
 from ..datastructures.pose import Pose
 from ..robot_descriptions import robot_description
 from ..plan_failures import IKError
-from ..external_interfaces.giskard import projection_cartisian_goal, achieve_cartesian_goal
+from ..external_interfaces.giskard import projection_cartisian_goal, allow_gripper_collision
 
 
 def _make_request_msg(root_link: str, tip_link: str, target_pose: Pose, robot_object: Object,
@@ -206,6 +206,7 @@ def request_giskard_ik(target_pose: Pose, robot: Object, gripper: str) -> Tuple[
     local_transformer = LocalTransformer()
     target_map = local_transformer.transform_pose(target_pose, "map")
 
+    allow_gripper_collision("all")
     result = projection_cartisian_goal(target_map, gripper, "map")
     last_point = result.trajectory.points[-1]
     joint_names = result.trajectory.joint_names
diff --git a/src/pycram/resolver/location/giskard_location.py b/src/pycram/resolver/location/giskard_location.py
index b5793bd0a..7039e34e7 100644
--- a/src/pycram/resolver/location/giskard_location.py
+++ b/src/pycram/resolver/location/giskard_location.py
@@ -4,15 +4,9 @@
 from ...external_interfaces.ik import request_giskard_ik
 from ...robot_description import ManipulatorDescription
 from ...world import UseProspectionWorld, World
-from ...datastructures.pose import Pose
 from ...robot_descriptions import robot_description
-from ...pose_generator_and_validator import reachability_validator
 from ...local_transformer import LocalTransformer
-from ...external_interfaces.giskard import projection_cartisian_goal
 from ...plan_failures import IKError
-from typing_extensions import Tuple, Dict
-
-import tf
 
 
 class GiskardLocation(CostmapLocation):

From 61af10d7d7d1a9ea9a73fcc23e0ec0d9e3b6bdeb Mon Sep 17 00:00:00 2001
From: Jonas Dech <j.dech@gmx.net>
Date: Tue, 7 May 2024 10:49:30 +0200
Subject: [PATCH 04/11] [resolver/giskardLoc] New implementation for giskard
 location

---
 src/pycram/external_interfaces/giskard.py     | 42 ++++++++++++++++-
 src/pycram/external_interfaces/ik.py          |  4 +-
 .../resolver/location/giskard_location.py     | 47 ++++++++++++++++---
 3 files changed, 84 insertions(+), 9 deletions(-)

diff --git a/src/pycram/external_interfaces/giskard.py b/src/pycram/external_interfaces/giskard.py
index b7e721c5c..b2e42a1d0 100644
--- a/src/pycram/external_interfaces/giskard.py
+++ b/src/pycram/external_interfaces/giskard.py
@@ -466,7 +466,7 @@ def achieve_close_container_goal(tip_link: str, environment_link: str) -> 'MoveR
 
 
 @init_giskard_interface
-def projection_cartisian_goal(goal_pose: Pose, tip_link: str, root_link: str) -> 'MoveResult':
+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.
@@ -481,6 +481,46 @@ def projection_cartisian_goal(goal_pose: Pose, tip_link: str, root_link: str) ->
     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
 
 @init_giskard_interface
diff --git a/src/pycram/external_interfaces/ik.py b/src/pycram/external_interfaces/ik.py
index b7b44e4bc..28ee4ef1e 100644
--- a/src/pycram/external_interfaces/ik.py
+++ b/src/pycram/external_interfaces/ik.py
@@ -15,7 +15,7 @@
 from ..datastructures.pose import Pose
 from ..robot_descriptions import robot_description
 from ..plan_failures import IKError
-from ..external_interfaces.giskard import projection_cartisian_goal, allow_gripper_collision
+from ..external_interfaces.giskard import projection_cartesian_goal, allow_gripper_collision
 
 
 def _make_request_msg(root_link: str, tip_link: str, target_pose: Pose, robot_object: Object,
@@ -207,7 +207,7 @@ def request_giskard_ik(target_pose: Pose, robot: Object, gripper: str) -> Tuple[
     target_map = local_transformer.transform_pose(target_pose, "map")
 
     allow_gripper_collision("all")
-    result = projection_cartisian_goal(target_map, gripper, "map")
+    result = projection_cartesian_goal(target_map, gripper, "map")
     last_point = result.trajectory.points[-1]
     joint_names = result.trajectory.joint_names
 
diff --git a/src/pycram/resolver/location/giskard_location.py b/src/pycram/resolver/location/giskard_location.py
index 7039e34e7..0403c6e86 100644
--- a/src/pycram/resolver/location/giskard_location.py
+++ b/src/pycram/resolver/location/giskard_location.py
@@ -1,12 +1,19 @@
+import time
+
 import rospy
+import tf
 
+from ...datastructures.pose import Pose
 from ...designators.location_designator import CostmapLocation
 from ...external_interfaces.ik import request_giskard_ik
+from ...external_interfaces.giskard import projection_cartesian_goal_with_approach, projection_joint_goal
 from ...robot_description import ManipulatorDescription
 from ...world import UseProspectionWorld, World
 from ...robot_descriptions import robot_description
 from ...local_transformer import LocalTransformer
 from ...plan_failures import IKError
+from ...costmaps import OccupancyCostmap, GaussianCostmap
+from ...pose_generator_and_validator import PoseGenerator
 
 
 class GiskardLocation(CostmapLocation):
@@ -27,10 +34,38 @@ def __iter__(self) -> CostmapLocation.Location:
 
         manipulator_descs = list(
             filter(lambda chain: isinstance(chain[1], ManipulatorDescription), robot_description.chains.items()))
-        for name, chain in manipulator_descs:
-            try:
-                pose, joint_states = request_giskard_ik(target_map, World.robot, chain.tool_frame)
-                yield self.Location(pose, chain.name)
-            except IKError as e:
-                pass
+
+        near_costmap = (OccupancyCostmap(0.35, False, 200, 0.02, self.target)
+                        + GaussianCostmap(200, 15, 0.02, self.target))
+        for maybe_pose in PoseGenerator(near_costmap, 200):
+            for name, chain in manipulator_descs:
+                projection_joint_goal(robot_description.get_static_joint_chain(chain.name, "park"), allow_collisions=True)
+                time.sleep(5)
+                reachable_arms = []
+                reachable = False
+
+                trajectory = projection_cartesian_goal_with_approach(maybe_pose, self.target, chain.tool_frame,
+                                                                     "map", robot_description.base_link)
+                last_point_positions = trajectory.trajectory.points[-1].positions
+                last_point_names = trajectory.trajectory.joint_names
+                last_joint_states = dict(zip(last_point_names, last_point_positions))
+                orientation = list(
+                    tf.transformations.quaternion_from_euler(0, 0, last_joint_states["brumbrum_yaw"], axes="sxyz"))
+                pose = Pose([last_joint_states["brumbrum_x"], last_joint_states["brumbrum_y"], 0], orientation)
+
+                robot_joint_states = {}
+                for joint_name, state in last_joint_states.items():
+                    if joint_name in World.robot.joints.keys():
+                        robot_joint_states[joint_name] = state
+
+                prospection_robot = World.current_world.get_prospection_object_for_object(World.robot)
+
+                with UseProspectionWorld():
+                    prospection_robot.set_joint_positions(robot_joint_states)
+                    prospection_robot.set_pose(pose)
+                    gripper_pose = prospection_robot.get_link_pose(chain.tool_frame)
+                    print(gripper_pose.dist(self.target))
+                    if gripper_pose.dist(self.target) <= 0.02:
+                        yield CostmapLocation.Location(pose, [name])
+
 

From bd7d21ceb85f7f9867c2d5f326603effaae0984b Mon Sep 17 00:00:00 2001
From: Jonas Dech <j.dech@gmx.net>
Date: Tue, 7 May 2024 10:53:57 +0200
Subject: [PATCH 05/11] [resolver/GiskardLoc] Removed debug statements

---
 src/pycram/resolver/location/giskard_location.py | 5 +----
 1 file changed, 1 insertion(+), 4 deletions(-)

diff --git a/src/pycram/resolver/location/giskard_location.py b/src/pycram/resolver/location/giskard_location.py
index 0403c6e86..2d4f78935 100644
--- a/src/pycram/resolver/location/giskard_location.py
+++ b/src/pycram/resolver/location/giskard_location.py
@@ -40,9 +40,6 @@ def __iter__(self) -> CostmapLocation.Location:
         for maybe_pose in PoseGenerator(near_costmap, 200):
             for name, chain in manipulator_descs:
                 projection_joint_goal(robot_description.get_static_joint_chain(chain.name, "park"), allow_collisions=True)
-                time.sleep(5)
-                reachable_arms = []
-                reachable = False
 
                 trajectory = projection_cartesian_goal_with_approach(maybe_pose, self.target, chain.tool_frame,
                                                                      "map", robot_description.base_link)
@@ -64,7 +61,7 @@ def __iter__(self) -> CostmapLocation.Location:
                     prospection_robot.set_joint_positions(robot_joint_states)
                     prospection_robot.set_pose(pose)
                     gripper_pose = prospection_robot.get_link_pose(chain.tool_frame)
-                    print(gripper_pose.dist(self.target))
+
                     if gripper_pose.dist(self.target) <= 0.02:
                         yield CostmapLocation.Location(pose, [name])
 

From 8c5f8a1bb79243d90d27f72d699f428643562615 Mon Sep 17 00:00:00 2001
From: Jonas Dech <j.dech@gmx.net>
Date: Tue, 7 May 2024 11:52:00 +0200
Subject: [PATCH 06/11] [stretch-pms] Fixed api and math for move head

---
 .../process_modules/stretch_process_modules.py    | 15 ++++++++-------
 1 file changed, 8 insertions(+), 7 deletions(-)

diff --git a/src/pycram/process_modules/stretch_process_modules.py b/src/pycram/process_modules/stretch_process_modules.py
index dc02f1970..d7eca5231 100644
--- a/src/pycram/process_modules/stretch_process_modules.py
+++ b/src/pycram/process_modules/stretch_process_modules.py
@@ -36,11 +36,11 @@ def _execute(self, designator: MoveMotion) -> Any:
         new_tilt = np.arctan2(-pose_in_tilt.position.y,
                               pose_in_tilt.position.z ** 2 + pose_in_tilt.position.x ** 2) * -1
 
-        current_pan = robot.get_joint_state("joint_head_pan")
-        current_tilt = robot.get_joint_state("joint_head_tilt")
+        current_pan = robot.get_joint_position("joint_head_pan")
+        current_tilt = robot.get_joint_position("joint_head_tilt")
 
-        robot.set_joint_state("joint_head_pan", new_pan + current_pan)
-        robot.set_joint_state("joint_head_tilt", new_tilt + current_tilt)
+        robot.set_joint_position("joint_head_pan", new_pan + current_pan)
+        robot.set_joint_position("joint_head_tilt", new_tilt + current_tilt)
 
 
 class StretchMoveGripper(DefaultMoveGripper):
@@ -164,10 +164,11 @@ def _execute(self, desig: LookingMotion):
         pose_in_tilt = local_transformer.transform_pose(target, robot.get_link_tf_frame("head_tilt_link"))
 
         new_pan = np.arctan2(pose_in_pan.position.y, pose_in_pan.position.x)
-        new_tilt = np.arctan2(pose_in_tilt.position.z, pose_in_tilt.position.x ** 2 + pose_in_tilt.position.y ** 2) * -1
+        new_tilt = np.arctan2(-pose_in_tilt.position.y,
+                              pose_in_tilt.position.z ** 2 + pose_in_tilt.position.x ** 2) * -1
 
-        current_pan = robot.get_joint_state("head_pan_joint")
-        current_tilt = robot.get_joint_state("head_tilt_joint")
+        current_pan = robot.get_joint_position("joint_head_pan")
+        current_tilt = robot.get_joint_position("joint_head_tilt")
 
         giskard.avoid_all_collisions()
         giskard.achieve_joint_goal({"head_pan_joint": new_pan + current_pan,

From f40a610bdc57af5da8532f42277a9ca41ec9c517 Mon Sep 17 00:00:00 2001
From: Jonas Dech <j.dech@gmx.net>
Date: Wed, 22 May 2024 16:23:14 +0200
Subject: [PATCH 07/11] [ext/giskard] Updated parallel execution for new
 giskard interface

---
 src/pycram/external_interfaces/giskard.py     | 68 +++++++++++--------
 .../resolver/location/giskard_location.py     |  5 --
 2 files changed, 41 insertions(+), 32 deletions(-)

diff --git a/src/pycram/external_interfaces/giskard.py b/src/pycram/external_interfaces/giskard.py
index b2e42a1d0..24f052fd9 100644
--- a/src/pycram/external_interfaces/giskard.py
+++ b/src/pycram/external_interfaces/giskard.py
@@ -22,7 +22,7 @@
     from giskard_msgs.msg import WorldBody, MoveResult, CollisionEntry
     from giskard_msgs.srv import UpdateWorldRequest, UpdateWorld, UpdateWorldResponse, RegisterGroupResponse
 except ModuleNotFoundError as e:
-    rospy.logwarn("Failed to import Giskard messages")
+    rospy.logwarn("Failed to import Giskard messages, the real robot will not be available")
 
 giskard_wrapper: GiskardWrapper = None
 giskard_update_service = None
@@ -219,51 +219,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.execute()
-                giskard_wrapper.cmd_seq = tmp
+                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
 
 
diff --git a/src/pycram/resolver/location/giskard_location.py b/src/pycram/resolver/location/giskard_location.py
index 2d4f78935..70e15b014 100644
--- a/src/pycram/resolver/location/giskard_location.py
+++ b/src/pycram/resolver/location/giskard_location.py
@@ -1,17 +1,12 @@
-import time
-
-import rospy
 import tf
 
 from ...datastructures.pose import Pose
 from ...designators.location_designator import CostmapLocation
-from ...external_interfaces.ik import request_giskard_ik
 from ...external_interfaces.giskard import projection_cartesian_goal_with_approach, projection_joint_goal
 from ...robot_description import ManipulatorDescription
 from ...world import UseProspectionWorld, World
 from ...robot_descriptions import robot_description
 from ...local_transformer import LocalTransformer
-from ...plan_failures import IKError
 from ...costmaps import OccupancyCostmap, GaussianCostmap
 from ...pose_generator_and_validator import PoseGenerator
 

From 890ddddf4400a6c1796abd5a8f82dda8da9053c0 Mon Sep 17 00:00:00 2001
From: Jonas Dech <j.dech@gmx.net>
Date: Wed, 22 May 2024 16:45:44 +0200
Subject: [PATCH 08/11] [examples] updated location_designator with
 GiskardLocation

---
 examples/location_designator.ipynb | 26 ++++++++++++++++++++++++++
 1 file changed, 26 insertions(+)

diff --git a/examples/location_designator.ipynb b/examples/location_designator.ipynb
index a6c368aec..20818f279 100644
--- a/examples/location_designator.ipynb
+++ b/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": {

From e20064127f74dd6fc2ea6f6d0e517bf98adfccd2 Mon Sep 17 00:00:00 2001
From: Jonas Dech <j.dech@gmx.net>
Date: Wed, 22 May 2024 16:53:31 +0200
Subject: [PATCH 09/11] [resolv/giskard_loc] Small fixes and exclusion of
 visile_for

---
 src/pycram/resolver/location/giskard_location.py | 10 ++++++----
 1 file changed, 6 insertions(+), 4 deletions(-)

diff --git a/src/pycram/resolver/location/giskard_location.py b/src/pycram/resolver/location/giskard_location.py
index 70e15b014..3bf4aa42a 100644
--- a/src/pycram/resolver/location/giskard_location.py
+++ b/src/pycram/resolver/location/giskard_location.py
@@ -24,19 +24,21 @@ def __iter__(self) -> CostmapLocation.Location:
 
         :yield: An instance of CostmapLocation.Location with a pose from which the robot can reach the target
         """
+        if self.visible_for:
+            raise ValueError("GiskardLocation does not support the visible_for parameter")
         local_transformer = LocalTransformer()
         target_map = local_transformer.transform_pose(self.target, "map")
 
         manipulator_descs = list(
             filter(lambda chain: isinstance(chain[1], ManipulatorDescription), robot_description.chains.items()))
 
-        near_costmap = (OccupancyCostmap(0.35, False, 200, 0.02, self.target)
-                        + GaussianCostmap(200, 15, 0.02, self.target))
+        near_costmap = (OccupancyCostmap(0.35, False, 200, 0.02, target_map)
+                        + GaussianCostmap(200, 15, 0.02, target_map))
         for maybe_pose in PoseGenerator(near_costmap, 200):
             for name, chain in manipulator_descs:
                 projection_joint_goal(robot_description.get_static_joint_chain(chain.name, "park"), allow_collisions=True)
 
-                trajectory = projection_cartesian_goal_with_approach(maybe_pose, self.target, chain.tool_frame,
+                trajectory = projection_cartesian_goal_with_approach(maybe_pose, target_map, chain.tool_frame,
                                                                      "map", robot_description.base_link)
                 last_point_positions = trajectory.trajectory.points[-1].positions
                 last_point_names = trajectory.trajectory.joint_names
@@ -57,7 +59,7 @@ def __iter__(self) -> CostmapLocation.Location:
                     prospection_robot.set_pose(pose)
                     gripper_pose = prospection_robot.get_link_pose(chain.tool_frame)
 
-                    if gripper_pose.dist(self.target) <= 0.02:
+                    if gripper_pose.dist(target_map) <= 0.02:
                         yield CostmapLocation.Location(pose, [name])
 
 

From 258d185e6f58a289e31f5af4251b8664d44b40de Mon Sep 17 00:00:00 2001
From: Jonas Dech <j.dech@gmx.net>
Date: Thu, 23 May 2024 11:31:24 +0200
Subject: [PATCH 10/11] [external-int] Minor changes and doc

---
 demos/pycram_bullet_world_demo/demo.py    | 90 +++++++++++------------
 src/pycram/external_interfaces/giskard.py | 11 +--
 src/pycram/external_interfaces/ik.py      |  4 +-
 3 files changed, 54 insertions(+), 51 deletions(-)

diff --git a/demos/pycram_bullet_world_demo/demo.py b/demos/pycram_bullet_world_demo/demo.py
index 3c32d9c37..92542cbec 100644
--- a/demos/pycram_bullet_world_demo/demo.py
+++ b/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()
diff --git a/src/pycram/external_interfaces/giskard.py b/src/pycram/external_interfaces/giskard.py
index 24f052fd9..76ff722c7 100644
--- a/src/pycram/external_interfaces/giskard.py
+++ b/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:
diff --git a/src/pycram/external_interfaces/ik.py b/src/pycram/external_interfaces/ik.py
index 28ee4ef1e..8722f1706 100644
--- a/src/pycram/external_interfaces/ik.py
+++ b/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.

From acfa0833f8e3e5fac3cbc1cb87579b79717ebc1d Mon Sep 17 00:00:00 2001
From: Jonas Dech <j.dech@gmx.net>
Date: Thu, 23 May 2024 14:02:05 +0200
Subject: [PATCH 11/11] [ik] doc and fix for try_to_reach_pose

---
 src/pycram/external_interfaces/ik.py | 12 ++++++++++--
 1 file changed, 10 insertions(+), 2 deletions(-)

diff --git a/src/pycram/external_interfaces/ik.py b/src/pycram/external_interfaces/ik.py
index 8722f1706..add3b621d 100644
--- a/src/pycram/external_interfaces/ik.py
+++ b/src/pycram/external_interfaces/ik.py
@@ -134,6 +134,14 @@ def apply_grasp_orientation_to_pose(grasp: str, pose: Pose) -> Pose:
 
 def try_to_reach(pose_or_object: Union[Pose, Object], prospection_robot: Object,
                  gripper_name: str) -> Union[Pose, None]:
+    """
+    Tries to reach a given position with a given robot. This is done by calculating the inverse kinematics.
+
+    :param pose_or_object: The position and rotation or Object for which reachability should be checked.
+    :param prospection_robot: The robot that should be used to check for reachability, should be the one in the prospection world
+    :param gripper_name: Name of the gripper tool frame
+    :return: The pose at which the robot should stand or None if the target is not reachable
+    """
     input_pose = pose_or_object.get_pose() if isinstance(pose_or_object, Object) else pose_or_object
 
     arm = "left" if gripper_name == robot_description.get_tool_frame("left") else "right"
@@ -144,7 +152,7 @@ def try_to_reach(pose_or_object: Union[Pose, Object], prospection_robot: Object,
     except IKError as e:
         rospy.logerr(f"Pose is not reachable: {e}")
         return None
-    _apply_ik(prospection_robot, inv, joints)
+    _apply_ik(prospection_robot, inv)
 
     return input_pose
 
@@ -197,7 +205,7 @@ def request_giskard_ik(target_pose: Pose, robot: Object, gripper: str) -> Tuple[
     """
     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. 
+    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.