diff --git a/launch/ik_and_description.launch b/launch/ik_and_description.launch
index d21700b12..628fd7f82 100644
--- a/launch/ik_and_description.launch
+++ b/launch/ik_and_description.launch
@@ -41,7 +41,7 @@
+ textfile="$(find pycram)/resources/robots/stretch_description.urdf"/>
diff --git a/resources/robots/stretch.urdf b/resources/robots/stretch_description.urdf
similarity index 56%
rename from resources/robots/stretch.urdf
rename to resources/robots/stretch_description.urdf
index 78df2173b..67fb04d90 100644
--- a/resources/robots/stretch.urdf
+++ b/resources/robots/stretch_description.urdf
@@ -1,256 +1,266 @@
-
+
+
+
+
+
-
+
-
+
-
+
-
+
-
+
-
-
+
+
-
+
-
+
-
+
-
+
-
+
-
-
+
-
+
+
-
+
-
+
-
+
-
+
-
+
-
-
+
-
+
+
-
+
-
+
-
+
-
+
-
+
-
-
+
+
-
+
-
+
-
+
-
+
-
+
-
-
+
-
+
+
-
+
-
+
-
+
-
+
-
+
-
-
+
+
-
+
-
+
-
+
-
+
-
+
-
-
+
-
+
+
-
+
-
+
-
+
-
+
-
+
-
-
+
+
-
+
+
-
+
+
+
+
+
-
+
-
+
-
+
@@ -258,626 +268,658 @@
-
-
-
+
+
+
-
+
-
+
-
+
-
-
+
-
+
+
-
-
-
+
+
+
-
+
-
+
-
+
-
-
+
-
+
+
-
-
-
-
-
-
+
+
+
+
+ 0
+ 0
+ 1.0
+ 1.0
+
+
+
-
+
-
+
+
+
+
+
+
-
-
+
+
-
-
-
+
+
+
-
+
-
+
-
+
-
-
+
+
-
-
-
+
+
+
-
+
-
+
-
+
-
-
+
-
+
+
+
+
+
-
-
-
+
+
+
-
+
-
+
-
+
-
-
+
+
-
-
-
+
+
+
-
+
-
+
-
+
-
-
+
-
+
+
+
-
-
-
+
+
+
-
+
-
+
-
+
-
-
+
-
+
+
+
-
-
-
+
+
+
-
+
-
+
-
+
-
-
+
-
+
+
+
-
-
-
+
+
+
-
+
-
+
-
+
-
-
+
-
+
+
+
-
-
-
+
+
+
-
+
-
+
-
+
-
-
+
-
+
+
+
-
-
-
+
+
+
-
+
-
+
-
+
-
-
+
+
-
-
-
+
+
+
-
+
-
+
-
+
-
-
+
-
+
+
+
-
-
-
+
+
+
-
+
-
+
-
+
-
-
+
-
+
+
+
-
-
-
+
+
+
-
+
-
+
-
+
-
-
+
+
-
-
-
+
+
+
-
+
-
+
-
+
-
-
+
+
-
-
-
+
+
+
-
+
-
+
-
+
-
-
+
+
-
-
-
+
+
+
-
+
-
+
-
+
-
-
+
+
-
-
-
+
+
+
-
+
-
+
-
+
-
-
+
+
-
+
-
+
+
-
+
-
+
-
-
-
-
-
-
+
+
-
+
+
+
+
+
+
+
+
-
+
-
+
+
-
+
-
+
+
-
+
-
+
+
-
+
-
+
@@ -887,107 +929,108 @@
-
+
-
+
-
+
-
+
-
-
-
+
+
+
-
+
-
+
-
+
-
-
+
+
-
-
-
+
+
+
-
+
-
+
-
+
-
-
+
+
-
-
-
+
+
+
-
+
-
+
-
+
-
-
+
+
+
diff --git a/src/pycram/external_interfaces/giskard.py b/src/pycram/external_interfaces/giskard.py
index 3d6dea4ff..f0e75da7b 100644
--- a/src/pycram/external_interfaces/giskard.py
+++ b/src/pycram/external_interfaces/giskard.py
@@ -6,7 +6,7 @@
import sys
import rosnode
-from ..datastructures.enums import JointType
+from ..datastructures.enums import JointType, ObjectType
from ..datastructures.pose import Pose
from ..robot_descriptions import robot_description
from ..datastructures.world import World
@@ -130,18 +130,17 @@ def sync_worlds() -> None:
add_gripper_groups()
world_object_names = set()
for obj in World.current_world.objects:
- if obj.name != robot_description.name and len(obj.link_name_to_id) != 1:
+ if obj.name != robot_description.name and obj.obj_type != ObjectType.ROBOT and len(obj.link_name_to_id) != 1:
world_object_names.add(obj.name)
- if obj.name == robot_description.name:
+ if obj.name == robot_description.name or obj.obj_type == ObjectType.ROBOT:
joint_config = obj.get_positions_of_all_joints()
non_fixed_joints = list(filter(lambda joint: joint.type != JointType.FIXED, obj.joints.values()))
joint_config_filtered = {joint.name: joint_config[joint.name] for joint in non_fixed_joints}
- giskard_wrapper.motion_goals.set_seed_configuration(joint_config_filtered,
+ giskard_wrapper.monitors.add_set_seed_configuration(joint_config_filtered,
robot_description.name)
- giskard_wrapper.motion_goals.set_seed_odometry(_pose_to_pose_stamped(obj.get_pose()),
+ giskard_wrapper.monitors.add_set_seed_odometry(_pose_to_pose_stamped(obj.get_pose()),
robot_description.name)
-
giskard_object_names = set(giskard_wrapper.get_group_names())
robot_name = {robot_description.name}
if not world_object_names.union(robot_name).issubset(giskard_object_names):
@@ -597,7 +596,6 @@ def add_gripper_groups() -> None:
for name in giskard_wrapper.get_group_names():
if "gripper" in name:
return
-
for name, description in robot_description.chains.items():
if isinstance(description, ManipulatorDescription):
root_link = robot_description.chains[name].gripper.links[-1]
diff --git a/src/pycram/process_modules/stretch_process_modules.py b/src/pycram/process_modules/stretch_process_modules.py
index d3c7fcada..d73eb6642 100644
--- a/src/pycram/process_modules/stretch_process_modules.py
+++ b/src/pycram/process_modules/stretch_process_modules.py
@@ -281,7 +281,7 @@ def _execute(self, designator: ClosingMotion) -> Any:
class StretchManager(ProcessModuleManager):
def __init__(self):
- super().__init__("stretch")
+ super().__init__("stretch_description")
self._navigate_lock = Lock()
self._looking_lock = Lock()
self._detecting_lock = Lock()
diff --git a/src/pycram/robot_descriptions/stretch_description.py b/src/pycram/robot_descriptions/stretch_description.py
index 503df6ede..f298daa70 100644
--- a/src/pycram/robot_descriptions/stretch_description.py
+++ b/src/pycram/robot_descriptions/stretch_description.py
@@ -6,7 +6,7 @@
class StretchDescription(RobotDescription):
def __init__(self):
- super().__init__("stretch", "base_link", "base_link", "link_lift", "joint_lift")
+ super().__init__("stretch_description", "base_link", "base_link", "link_lift", "joint_lift")
realsense_color = CameraDescription('camera_color_optical_frame', horizontal_angle=1.047, vertical_angle=0.785,
minimal_height=1.322, maximal_height=1.322)