Merge branch 'follow_point_path' into follow_nav_path

SUTURO · Jun 26, 2024 · 59b4a9f · 59b4a9f
2 parents 7ec1662 + b50e4e1
commit 59b4a9f
Show file tree

Hide file tree

Showing 9 changed files with 688 additions and 19 deletions.
diff --git a/src/giskardpy/configs/behavior_tree_config.py b/src/giskardpy/configs/behavior_tree_config.py
@@ -141,6 +141,10 @@ def add_tf_publisher(self, include_prefix: bool = True, tf_topic: str = 'tf',
         self.tree.wait_for_goal.publish_state.add_tf_publisher(include_prefix=include_prefix,
                                                                tf_topic=tf_topic,
                                                                mode=mode)
+        if god_map.is_standalone():
+            self.tree.control_loop_branch.publish_state.add_tf_publisher(include_prefix=include_prefix,
+                                                                   tf_topic=tf_topic,
+                                                                   mode=mode)
 
     def add_evaluate_debug_expressions(self):
         self.tree.prepare_control_loop.add_compile_debug_expressions()
@@ -193,9 +197,9 @@ def __init__(self,
         """
         self.include_prefix = include_prefix
         if is_running_in_pytest():
-            publish_tf = False
-            publish_js = False
             if god_map.is_in_github_workflow():
+                publish_js = False
+                publish_tf = False
                 debug_mode = False
                 simulation_max_hz = None
         super().__init__(ControlModes.standalone, simulation_max_hz=simulation_max_hz)

diff --git a/src/giskardpy/configs/collision_avoidance_config.py b/src/giskardpy/configs/collision_avoidance_config.py
@@ -1,6 +1,7 @@
 from __future__ import annotations
 
 import abc
+from copy import copy
 from typing import Dict, Optional, List, Union, DefaultDict, Tuple
 
 from giskardpy.exceptions import SetupException
@@ -98,13 +99,19 @@ def set_default_external_collision_avoidance(self,
         :param hard_threshold: distance threshold not allowed to be violated
         :param max_velocity: how fast it will move away from collisions
         """
+        new_default = CollisionAvoidanceThresholds(
+            number_of_repeller=number_of_repeller,
+            soft_threshold=soft_threshold,
+            hard_threshold=hard_threshold,
+            max_velocity=max_velocity
+        )
+        # overwrite the default of default
+        old_default = self.collision_scene.collision_avoidance_configs.default_factory()
+        old_default.external_collision_avoidance.default_factory = lambda: copy(new_default)
+        self.collision_scene.collision_avoidance_configs.default_factory = lambda: copy(old_default)
+        # overwrite the defaults of existing entries
         for config in self.collision_scene.collision_avoidance_configs.values():
-            config.external_collision_avoidance.default_factory = lambda: CollisionAvoidanceThresholds(
-                number_of_repeller=number_of_repeller,
-                soft_threshold=soft_threshold,
-                hard_threshold=hard_threshold,
-                max_velocity=max_velocity
-            )
+            config.external_collision_avoidance.default_factory = lambda: copy(new_default)
 
     def overwrite_external_collision_avoidance(self,
                                                joint_name: str,
@@ -173,7 +180,8 @@ def load_self_collision_matrix(self, path_to_srdf: str, group_name: Optional[str
         if group_name not in self.collision_scene.self_collision_matrix_cache:
             self.collision_scene.load_self_collision_matrix_from_srdf(path_to_srdf, group_name)
         else:
-            path_to_srdf, self_collision_matrix, disabled_links = self.collision_scene.self_collision_matrix_cache[group_name]
+            path_to_srdf, self_collision_matrix, disabled_links = self.collision_scene.self_collision_matrix_cache[
+                group_name]
             self.collision_scene.self_collision_matrix = self_collision_matrix
             self.collision_scene.disabled_links = disabled_links
 
@@ -209,6 +217,7 @@ def setup(self):
     def _sanity_check(self):
         pass
 
+
 class DisableCollisionAvoidanceConfig(CollisionAvoidanceConfig):
     def __init__(self):
         super().__init__(CollisionCheckerLib.none)

diff --git a/src/giskardpy/exceptions.py b/src/giskardpy/exceptions.py
@@ -242,3 +242,24 @@ class MonitorForceException(ExecutionException):
 @GiskardException.register_error_code(GiskardError.BEHAVIOR_TREE_ERROR)
 class BehaviorTreeException(GiskardException):
     pass
+
+
+# %% force torque exceptions
+@GiskardException.register_error_code(GiskardError.FORCE_TORQUE_MONITOR_ERROR)
+class ForceTorqueExceptions(GiskardException):
+    pass
+
+
+@GiskardException.register_error_code(GiskardError.FORCE_TORQUE_MONITOR_GRASPING_MISSED_OBJECT)
+class ForceTorqueMonitorGraspsingMissedObjectExceptions(GiskardException):
+    pass
+
+
+@GiskardException.register_error_code(GiskardError.FORCE_TORQUE_MONITOR_TRANSPORTING_LOST_OBJECT)
+class ForceTorqueTransportingLostObjectExceptions(GiskardException):
+    pass
+
+
+@GiskardException.register_error_code(GiskardError.FORCE_TORQUE_MONITOR_PLACING_MISSED_PLACING_LOCATION)
+class ForceTorquePlacingMissedPlacingLocationExceptions(GiskardException):
+    pass