Hsr world config with env

CMakeLists.txt

@@ -169,6 +169,7 @@ catkin_install_python(PROGRAMS
 	scripts/iai_robots/hsr/hsr_standalone.py
 	scripts/iai_robots/hsr/iai_hsr.py
 	scripts/iai_robots/hsr/iai_hsr_real_time.py
+	scripts/iai_robots/hsr/iai_hsr_real_time_with_kitchen.py
 	scripts/iai_robots/hsr/iai_hsr_mujoco.py
 	scripts/iai_robots/pr2/iai_pr2.py
 	scripts/iai_robots/pr2/iai_pr2_vel_controllers.py

launch/giskardpy_hsr_real_vel_with_kitchen.launch

@@ -0,0 +1,28 @@
+<launch>
+    <arg name="debug_mode" default="False" />
+
+    <include file="$(find hsr_velocity_controller)/launch/switch_to_velocity_controllers.launch" />
+
+    <node pkg="giskardpy" type="iai_hsr_real_time_with_kitchen.py" name="giskard" output="screen">
+        <param name="debug_mode" value="$(arg debug_mode)" />
+    </node>
+
+    <node pkg="giskardpy" type="joystick_e_stop.py" name="giskard_e_stop" output="screen">
+        <remap from="/joy" to="/hsrb/joy"/>
+        <rosparam param="button_ids">
+            [0, 1, 2, 3, 4, 5, 8, 9, 10, 11, 12, 13, 14, 15, 16]
+        </rosparam>
+    </node>
+
+    <node pkg="giskardpy" type="switch_controllers.py" name="giskard_joy_switch_controllers" output="screen" />
+
+    <node pkg="giskardpy" type="interactive_marker.py" name="giskard_interactive_marker" output="screen">
+        <rosparam param="enable_self_collision">False</rosparam>
+        <rosparam param="interactive_marker_chains">
+          - [odom, base_footprint]
+          - [odom, head_rgbd_sensor_link]
+          - [odom, hand_gripper_tool_frame]
+        </rosparam>
+    </node>
+
+</launch>

scripts/iai_robots/hsr/iai_hsr_real_time_with_kitchen.py

@@ -0,0 +1,17 @@
+#!/usr/bin/env python
+import rospy
+
+from giskardpy.configs.behavior_tree_config import ClosedLoopBTConfig
+from giskardpy.configs.giskard import Giskard
+from giskardpy.configs.iai_robots.hsr import WorldWithHSRConfig, HSRCollisionAvoidanceConfig, \
+    HSRVelocityInterface, SuturoArenaWithHSRConfig
+
+if __name__ == '__main__':
+    rospy.init_node('giskard')
+    debug_mode = rospy.get_param('~debug_mode', False)
+    giskard = Giskard(world_config=SuturoArenaWithHSRConfig(),
+                      collision_avoidance_config=HSRCollisionAvoidanceConfig(),
+                      robot_interface_config=HSRVelocityInterface(),
+                      behavior_tree_config=ClosedLoopBTConfig(publish_free_variables=True, debug_mode=debug_mode,
+                                                              add_tf_pub=True))
+    giskard.live()

src/giskardpy/casadi_wrapper.pyi

@@ -909,4 +909,4 @@ def is_false(expr: Expression) -> bool: ...
 
 def is_constant(expr: Expression) -> bool: ...
 
-def det(expr: Expression) -> Expression: ...
+def det(expr: Expression) -> Expression: ...

src/giskardpy/configs/iai_robots/hsr.py

@@ -1,10 +1,14 @@
 import numpy as np
+import rospy
 
 from giskardpy.configs.collision_avoidance_config import CollisionAvoidanceConfig
 from giskardpy.configs.robot_interface_config import StandAloneRobotInterfaceConfig, RobotInterfaceConfig
 from giskardpy.configs.world_config import WorldConfig
 from giskardpy.data_types import PrefixName, Derivatives
+from giskardpy.god_map import god_map
+from giskardpy.model.utils import robot_name_from_urdf_string
 from giskardpy.utils import logging
+import giskardpy.utils.tfwrapper as tf
 
 
 class WorldWithHSRConfig(WorldConfig):
@@ -68,6 +72,28 @@ def setup(self):
             joint_name='arm_lift_joint')
 
 
+class SuturoArenaWithHSRConfig(WorldWithHSRConfig):
+
+    def __init__(self, map_name: str = 'map', localization_joint_name: str = 'localization',
+                 odom_link_name: str = 'odom', drive_joint_name: str = 'brumbrum',
+                 description_name: str = 'robot_description',
+                 environment_name: str = 'kitchen_description'):
+        super().__init__(map_name, localization_joint_name, odom_link_name, drive_joint_name, description_name)
+        self.environment_name = environment_name
+
+    def setup(self):
+        super().setup()
+        urdf = rospy.get_param(self.environment_name)
+        self.kitchen_name = robot_name_from_urdf_string(urdf)
+        god_map.world.add_urdf(urdf=urdf,
+                               group_name=self.kitchen_name,
+                               actuated=False)
+        root_link_name = self.get_root_link_of_group(self.kitchen_name)
+        kitchen_pose = tf.lookup_pose(self.map_name, 'iai_kitchen/urdf_main')
+        self.add_fixed_joint(parent_link=self.map_name, child_link=root_link_name,
+                             homogenous_transform=tf.pose_to_np(kitchen_pose.pose))
+
+
 class HSRCollisionAvoidanceConfig(CollisionAvoidanceConfig):
     def __init__(self, drive_joint_name: str = 'brumbrum'):
         super().__init__()

src/giskardpy/goals/joint_goals.py

@@ -49,9 +49,10 @@ def __init__(self,
 
 
 class SetOdometry(NonMotionGoal):
+    odom_joints = (OmniDrive, DiffDrive, OmniDrivePR22)
     def __init__(self,
-                 group_name: str,
                  base_pose: PoseStamped,
+                 group_name: Optional[str] = None,
                  name: Optional[str] = None,
                  start_condition: cas.Expression = cas.TrueSymbol,
                  hold_condition: cas.Expression = cas.FalseSymbol,
@@ -62,10 +63,19 @@ def __init__(self,
         super().__init__(name)
         if god_map.is_goal_msg_type_execute() and not god_map.is_standalone():
             raise GoalInitalizationException(f'It is not allowed to combine {str(self)} with plan and execute.')
-        brumbrum_joint_name = god_map.world.groups[group_name].root_link.child_joint_names[0]
-        brumbrum_joint = god_map.world.joints[brumbrum_joint_name]
-        if not isinstance(brumbrum_joint, (OmniDrive, DiffDrive, OmniDrivePR22)):
-            raise GoalInitalizationException(f'Group {group_name} has no odometry joint.')
+        if self.group_name is None:
+            drive_joints = god_map.world.search_for_joint_of_type(self.odom_joints)
+            if len(drive_joints) == 0:
+                raise GoalInitalizationException('No drive joints in world')
+            elif len(drive_joints) == 1:
+                brumbrum_joint = drive_joints[0]
+            else:
+                raise GoalInitalizationException('Multiple drive joint found in world, please set \'group_name\'')
+        else:
+            brumbrum_joint_name = god_map.world.groups[group_name].root_link.child_joint_names[0]
+            brumbrum_joint = god_map.world.joints[brumbrum_joint_name]
+            if not isinstance(brumbrum_joint, self.odom_joints):
+                raise GoalInitalizationException(f'Group {self.group_name} has no odometry joint.')
         base_pose = transform_msg(brumbrum_joint.parent_link_name, base_pose).pose
         god_map.world.state[brumbrum_joint.x.name].position = base_pose.position.x
         god_map.world.state[brumbrum_joint.y.name].position = base_pose.position.y

src/giskardpy/model/utils.py

@@ -1,3 +1,6 @@
+from typing import Tuple
+from xml.etree import ElementTree as ET
+
 import numpy as np
 from shape_msgs.msg import SolidPrimitive
 
@@ -8,22 +11,20 @@ def robot_name_from_urdf_string(urdf_string):
     return urdf_string.split('robot name="')[1].split('"')[0]
 
 
-def hacky_urdf_parser_fix(urdf_str):
-    # this function is inefficient but the tested urdfs's aren't big enough for it to be a problem
-    fixed_urdf = ''
-    delete = False
-    black_list = ['transmission', 'gazebo']
-    black_open = ['<{}'.format(x) for x in black_list]
-    black_close = ['</{}'.format(x) for x in black_list]
-    for line in urdf_str.split('\n'):
-        if len([x for x in black_open if x in line]) > 0:
-            delete = True
-        if len([x for x in black_close if x in line]) > 0:
-            delete = False
-            continue
-        if not delete:
-            fixed_urdf += line + '\n'
-    return fixed_urdf
+def hacky_urdf_parser_fix(urdf: str, blacklist: Tuple[str] = ('transmission', 'gazebo')) -> str:
+    # Parse input string
+    root = ET.fromstring(urdf)
+
+    # Iterate through each section in the blacklist
+    for section_name in blacklist:
+        # Find all sections with the given name and remove them
+        for elem in root.findall(f".//{section_name}"):
+            parent = root.find(f".//{section_name}/..")
+            if parent is not None:
+                parent.remove(elem)
+
+    # Turn back to string
+    return ET.tostring(root, encoding='unicode')
 
 
 def make_world_body_box(x_length: float = 1, y_length: float = 1, z_length: float = 1) -> WorldBody:

src/giskardpy/model/world.py

@@ -593,7 +593,7 @@ def add_urdf(self,
         with suppress_stderr():
             try:
                 parsed_urdf: up.Robot = up.URDF.from_xml_string(hacky_urdf_parser_fix(urdf))
-            except ParseError as e:
+            except Exception as e:
                 raise CorruptURDFException(str(e))
         if group_name in self.groups:
             raise DuplicateNameException(

src/giskardpy/monitors/monitor_manager.py

@@ -12,7 +12,7 @@
 from giskardpy.data_types import TaskState
 from giskardpy.exceptions import GiskardException, MonitorInitalizationException, UnknownMonitorException
 from giskardpy.god_map import god_map
-from giskardpy.monitors.monitors import ExpressionMonitor, Monitor
+from giskardpy.monitors.monitors import ExpressionMonitor, Monitor, EndMotion
 from giskardpy.monitors.payload_monitors import PayloadMonitor, CancelMotion
 from giskardpy.symbol_manager import symbol_manager
 from giskardpy.utils import logging
@@ -337,3 +337,15 @@ def parse_monitors(self, monitor_msgs: List[giskard_msgs.Monitor]):
                 if not isinstance(e, GiskardException):
                     raise MonitorInitalizationException(error_msg)
                 raise e
+
+    def has_end_motion_monitor(self) -> bool:
+        for m in self.monitors:
+            if isinstance(m, EndMotion):
+                return True
+        return False
+
+    def has_cancel_motion_monitor(self) -> bool:
+        for m in self.monitors:
+            if isinstance(m, CancelMotion):
+                return True
+        return False

src/giskardpy/qp/pos_in_vel_limits.py

@@ -3,6 +3,7 @@
 import numpy as np
 import giskardpy.casadi_wrapper as cas
 import giskardpy.utils.math as gm
+from giskardpy.utils.decorators import memoize
 
 
 def shifted_velocity_profile(vel_profile, acc_profile, distance, dt):
@@ -84,6 +85,8 @@ def implicit_vel_profile(acc_limit: float, jerk_limit: float, dt: float, ph: int
     return list(reversed(vel_profile))
 
 
+@memoize
+@profile
 def b_profile(current_pos, current_vel, current_acc,
               pos_limits, vel_limits, acc_limits, jerk_limits, dt, ph, eps=0.00001):
     vel_limit = vel_limits[1]

src/giskardpy/qp/qp_controller.py

@@ -1020,6 +1020,7 @@ class QPProblemBuilder:
     qp_solver: QPSolver
     prediction_horizon: int = None
 
+    @profile
     def __init__(self,
                  sample_period: float,
                  prediction_horizon: int,

src/giskardpy/tree/behaviors/ros_msg_to_goal.py

@@ -5,7 +5,7 @@
 from giskard_msgs.msg import MoveGoal
 from giskardpy.exceptions import InvalidGoalException
 from giskardpy.goals.base_traj_follower import BaseTrajFollower
-from giskardpy.monitors.monitors import TimeAbove, LocalMinimumReached, EndMotion
+from giskardpy.monitors.monitors import TimeAbove, LocalMinimumReached, EndMotion, CancelMotion
 from giskardpy.god_map import god_map
 from giskardpy.model.joints import OmniDrive, DiffDrive
 from giskardpy.monitors.monitors import TimeAbove, LocalMinimumReached
@@ -38,7 +38,6 @@ def __init__(self, name):
     def update(self):
         move_goal = god_map.move_action_server.goal_msg
         loginfo(f'Parsing goal #{god_map.move_action_server.goal_id} message.')
-        self.sanity_check(move_goal)
         try:
             god_map.monitor_manager.parse_monitors(move_goal.monitors)
             god_map.motion_goal_manager.parse_motion_goals(move_goal.goals)
@@ -47,14 +46,21 @@ def update(self):
             raise InvalidGoalException('Couldn\'t transform goal')
         except Exception as e:
             raise e
+        self.sanity_check()
         # if god_map.is_collision_checking_enabled():
         #     god_map.motion_goal_manager.parse_collision_entries(move_goal.collisions)
         loginfo('Done parsing goal message.')
         return Status.SUCCESS
 
-    def sanity_check(self, move_goal: MoveGoal):
-        if not end_motion_in_move_goal(move_goal):
-            logging.logwarn(f'No {EndMotion.__name__} monitor.')
+    def sanity_check(self) -> None:
+        if (not god_map.monitor_manager.has_end_motion_monitor()
+                and not god_map.monitor_manager.has_cancel_motion_monitor()):
+            logging.logwarn(f'No {EndMotion.__name__} or {CancelMotion.__name__} monitor specified. '
+                            f'Motion will not stop unless cancelled externally.')
+            return
+        if not god_map.monitor_manager.has_end_motion_monitor():
+            logging.logwarn(f'No {EndMotion.__name__} monitor specified. Motion can\'t end successfully.')
+
 
 
 class SetExecutionMode(GiskardBehavior):
@@ -67,7 +73,8 @@ def __init__(self, name: str = 'set execution mode'):
     @record_time
     @profile
     def update(self):
-        loginfo(f'Goal is of type {get_ros_msgs_constant_name_by_value(type(god_map.move_action_server.goal_msg), god_map.move_action_server.goal_msg.type)}')
+        loginfo(
+            f'Goal is of type {get_ros_msgs_constant_name_by_value(type(god_map.move_action_server.goal_msg), god_map.move_action_server.goal_msg.type)}')
         if god_map.is_goal_msg_type_projection():
             god_map.tree.switch_to_projection()
         elif god_map.is_goal_msg_type_execute():