fixed some real robot configs

src/giskardpy_ros/configs/behavior_tree_config.py

@@ -237,18 +237,15 @@ def setup(self):
 class OpenLoopBTConfig(BehaviorTreeConfig):
     def __init__(self,
                  debug_mode: bool = False,
-                 control_loop_max_hz: float = 50,
-                 visualization_mode: VisualizationMode = VisualizationMode.CollisionsDecomposed,
-                 simulation_max_hz: Optional[float] = None):
+                 visualization_mode: VisualizationMode = VisualizationMode.CollisionsDecomposed):
         """
         The default behavior tree for Giskard in open-loop mode. It will first plan the trajectory in simulation mode
         and then publish it to connected joint trajectory followers. The base trajectory is tracked with a closed-loop
         controller.
         :param debug_mode:  enable various debugging tools.
         :param control_loop_max_hz: if not None, limits the frequency of the base trajectory controller.
         """
-        super().__init__(ControlModes.open_loop, control_loop_max_hz=control_loop_max_hz,
-                         simulation_max_hz=simulation_max_hz)
+        super().__init__(ControlModes.open_loop)
         if god_map.is_in_github_workflow():
             debug_mode = False
         self.debug_mode = debug_mode
@@ -273,16 +270,13 @@ def setup(self):
 
 class ClosedLoopBTConfig(BehaviorTreeConfig):
     def __init__(self, debug_mode: bool = False,
-                 control_loop_max_hz: float = 50,
-                 visualization_mode: VisualizationMode = VisualizationMode.CollisionsDecomposed,
-                 simulation_max_hz: Optional[float] = None):
+                 visualization_mode: VisualizationMode = VisualizationMode.CollisionsDecomposed):
         """
         The default configuration for Giskard in closed loop mode. Make use to set up the robot interface accordingly.
         :param debug_mode: If True, will publish debug data on topics. This will significantly slow down the control loop.
         :param control_loop_max_hz: Limits the control loop frequency. If None, it will go as fast as possible.
         """
-        super().__init__(ControlModes.close_loop, control_loop_max_hz=control_loop_max_hz,
-                         simulation_max_hz=simulation_max_hz)
+        super().__init__(ControlModes.close_loop)
         if god_map.is_in_github_workflow():
             debug_mode = False
         self.debug_mode = debug_mode

src/giskardpy_ros/configs/robot_interface_config.py

@@ -41,13 +41,13 @@ def tree(self) -> GiskardBT:
     def control_mode(self) -> ControlModes:
         return GiskardBlackboard().tree.control_mode
 
-    def sync_odometry_topic(self, odometry_topic: str, joint_name: str):
+    def sync_odometry_topic(self, odometry_topic: str, joint_name: str, sync_in_control_loop: bool = True):
         """
         Tell Giskard to sync an odometry joint added during by the world config.
         """
         joint_name = self.world.search_for_joint_name(joint_name)
         self.tree.wait_for_goal.synchronization.sync_odometry_topic(odometry_topic, joint_name)
-        if GiskardBlackboard().tree.is_closed_loop():
+        if sync_in_control_loop and GiskardBlackboard().tree.is_closed_loop():
             self.tree.control_loop_branch.closed_loop_synchronization.sync_odometry_topic_no_lock(
                 odometry_topic,
                 joint_name)

src/giskardpy_ros/ros1/ros_timer.py

@@ -81,7 +81,7 @@ def sleep(self):
         if elapsed_time > self.sleep_dur * 2:
             self.last_time = curr_time
             if self.print_warning:
-                get_middleware().logwarn(f'Control loop can\'t keep up with {GiskardBlackboard().control_loop_max_hz} hz. '
+                get_middleware().logwarn(f'Control loop can\'t keep up with {GiskardBlackboard().giskard.qp_controller_config.control_dt} hz. '
                                 f'This loop took {elapsed_time.to_sec():.5f}s')
 
 

src/giskardpy_ros/tree/behaviors/real_kinematic_sim.py

@@ -24,9 +24,10 @@ def update(self):
             self.last_time = next_time
             return Status.RUNNING
         next_cmds = god_map.qp_solver_solution
-        dt = next_time - self.last_time
-        if dt > god_map.qp_controller.mpc_dt:
-            dt = god_map.qp_controller.mpc_dt
+        # dt = next_time - self.last_time
+        # if dt > god_map.qp_controller.mpc_dt:
+        #     dt = god_map.qp_controller.mpc_dt
+        dt = god_map.qp_controller.control_dt
         god_map.world.update_state(next_cmds, dt, max_derivative=god_map.qp_controller.max_derivative)
         self.last_time = next_time
         return Status.RUNNING