diff --git a/demos/pycram_bullet_world_demo/demo.py b/demos/pycram_bullet_world_demo/demo.py
index ab15c4483..92542cbec 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}")
 
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": {
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 a5fc75a52..76ff722c7 100644
--- a/src/pycram/external_interfaces/giskard.py
+++ b/src/pycram/external_interfaces/giskard.py
@@ -1,15 +1,16 @@
 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
@@ -17,11 +18,11 @@
 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
diff --git a/src/pycram/external_interfaces/ik.py b/src/pycram/external_interfaces/ik.py
index a6c73f328..add3b621d 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_cartesian_goal, allow_gripper_collision
 
 
 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)
@@ -129,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"
@@ -139,12 +152,28 @@ 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
 
 
-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 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) -> 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.
@@ -169,4 +198,48 @@ 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
+    return dict(zip(joints, inv))
+
+
+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. 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.
+    :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")
+
+    allow_gripper_collision("all")
+    result = projection_cartesian_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
+
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/hsrb_process_modules.py b/src/pycram/process_modules/hsrb_process_modules.py
index 3f02d466c..12bf0fb14 100644
--- a/src/pycram/process_modules/hsrb_process_modules.py
+++ b/src/pycram/process_modules/hsrb_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..d7eca5231 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):
@@ -35,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):
@@ -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)
 
 
 ###########################################################
@@ -163,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,
diff --git a/src/pycram/resolver/location/giskard_location.py b/src/pycram/resolver/location/giskard_location.py
index 3d8ee79ec..3bf4aa42a 100644
--- a/src/pycram/resolver/location/giskard_location.py
+++ b/src/pycram/resolver/location/giskard_location.py
@@ -1,54 +1,65 @@
-from ...external_interfaces.giskard import achieve_cartesian_goal
+import tf
+
+from ...datastructures.pose import Pose
 from ...designators.location_designator import CostmapLocation
-from pycram.world import UseProspectionWorld, World
-from pycram.datastructures.pose import Pose
+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 pycram.pose_generator_and_validator import reachability_validator
-from typing_extensions import Tuple, Dict
-
-import tf
+from ...local_transformer import LocalTransformer
+from ...costmaps import OccupancyCostmap, GaussianCostmap
+from ...pose_generator_and_validator import PoseGenerator
 
 
 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
+        :yield: 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]:
-        """
-        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.
+        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, 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, 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
+                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)
+
+                    if gripper_pose.dist(target_map) <= 0.02:
+                        yield CostmapLocation.Location(pose, [name])
+
 
-        :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