fixed bug, where control/mpc dt couldn't be properly set and set all …

…jerk limits to None, such that they are computed by giskard

src/giskardpy_ros/configs/behavior_tree_config.py

@@ -13,17 +13,12 @@
 
 class BehaviorTreeConfig(ABC):
 
-    def __init__(self, mode: ControlModes, control_loop_max_hz: float = 80, simulation_max_hz: Optional[float] = None):
+    def __init__(self, mode: ControlModes):
         """
 
         :param mode: Defines the default setup of the behavior tree.
-        :param control_loop_max_hz: if mode == ControlModes.standalone: limits the simulation speed
-                       if mode == ControlModes.open_loop: limits the control loop of the base tracker
-                       if mode == ControlModes.close_loop: limits the control loop
         """
         self._control_mode = mode
-        GiskardBlackboard().control_loop_max_hz = control_loop_max_hz
-        GiskardBlackboard().simulation_max_hz = simulation_max_hz
 
     @abstractmethod
     def setup(self):
@@ -195,14 +190,12 @@ def __init__(self,
                  visualization_mode: VisualizationMode = VisualizationMode.VisualsFrameLocked,
                  publish_free_variables: bool = False,
                  publish_tf: bool = True,
-                 include_prefix: bool = False,
-                 simulation_max_hz: Optional[float] = None):
+                 include_prefix: bool = False):
         """
         The default behavior tree for Giskard in standalone mode. Make sure to set up the robot interface accordingly.
         :param debug_mode: enable various debugging tools.
         :param publish_js: publish current world state.
         :param publish_tf: publish all link poses in tf.
-        :param simulation_max_hz: if not None, will limit the frequency of the simulation.
         :param include_prefix: whether to include the robot name prefix when publishing joint states or tf
         """
         self.include_prefix = include_prefix
@@ -212,9 +205,8 @@ def __init__(self,
                 publish_js = False
                 publish_tf = True
                 debug_mode = False
-                simulation_max_hz = None
                 self.visualization_mode = VisualizationMode.Nothing
-        super().__init__(ControlModes.standalone, simulation_max_hz=simulation_max_hz)
+        super().__init__(ControlModes.standalone)
         self.debug_mode = debug_mode
         self.publish_js = publish_js
         self.publish_free_variables = publish_free_variables

src/giskardpy_ros/configs/giskard.py

@@ -72,9 +72,9 @@ def __init__(self,
         god_map.hack = 0
 
     def set_defaults(self) -> None:
+        self.qp_controller_config.set_defaults()
         self.world_config.set_defaults()
         self.robot_interface_config.set_defaults()
-        self.qp_controller_config.set_defaults()
         self.collision_avoidance_config.set_defaults()
         self.behavior_tree_config.set_defaults()
 
@@ -95,11 +95,6 @@ def grow(self):
         GiskardBlackboard().tree.setup(30)
 
     def sanity_check(self):
-        hz = GiskardBlackboard().control_loop_max_hz
-        if god_map.qp_controller.sample_period < 1/hz:
-            raise GiskardException(f'control_loop_max_hz (1/{hz}hz = {1/hz}) '
-                                   f'must be smaller than sample period of controller '
-                                   f'({god_map.qp_controller.sample_period}).')
         self._controlled_joints_sanity_check()
 
     def _controlled_joints_sanity_check(self):

src/giskardpy_ros/configs/iai_robots/donbot.py

@@ -19,7 +19,7 @@ def __init__(self,
     def setup(self):
         self.set_default_limits({Derivatives.velocity: 0.5,
                                  Derivatives.acceleration: np.inf,
-                                 Derivatives.jerk: 15})
+                                 Derivatives.jerk: None})
         self.add_empty_link(PrefixName(self.map_name))
         self.add_robot_urdf(urdf=rospy.get_param('robot_description'))
         root_link_name = self.get_root_link_of_group(self.robot_group_name)

src/giskardpy_ros/configs/iai_robots/hsr.py

@@ -30,7 +30,7 @@ def setup(self):
         self.set_default_color(1, 1, 1, 1)
         self.set_default_limits({Derivatives.velocity: 1,
                                  Derivatives.acceleration: np.inf,
-                                 Derivatives.jerk: 711})
+                                 Derivatives.jerk: None})
         self.add_empty_link(PrefixName(self.map_name))
         self.add_6dof_joint(parent_link=self.map_name, child_link=self.odom_link_name,
                             joint_name=self.localization_joint_name)
@@ -46,16 +46,16 @@ def setup(self):
                                   translation_limits={
                                       Derivatives.velocity: 0.2,
                                       Derivatives.acceleration: np.inf,
-                                      Derivatives.jerk: 142,
+                                      Derivatives.jerk: None,
                                   },
                                   rotation_limits={
                                       Derivatives.velocity: 0.2,
                                       Derivatives.acceleration: np.inf,
-                                      Derivatives.jerk: 142
+                                      Derivatives.jerk: None
                                   },
                                   robot_group_name=self.robot_group_name)
         self.set_joint_limits(limit_map={
-            Derivatives.jerk: 107,
+            Derivatives.jerk: None,
         },
             joint_name='arm_lift_joint')
 

src/giskardpy_ros/configs/iai_robots/pr2.py

@@ -21,10 +21,10 @@ def __init__(self, map_name: str = 'map', localization_joint_name: str = 'locali
     def setup(self, robot_name: Optional[str] = None):
         super().setup(robot_name)
         self.set_joint_limits(limit_map={Derivatives.velocity: 2,
-                                         Derivatives.jerk: 1422},
+                                         Derivatives.jerk: None},
                               joint_name='head_pan_joint')
         self.set_joint_limits(limit_map={Derivatives.velocity: 3.5,
-                                         Derivatives.jerk: 2489},
+                                         Derivatives.jerk: None},
                               joint_name='head_tilt_joint')
 
 

src/giskardpy_ros/configs/iai_robots/tracy.py

@@ -11,7 +11,7 @@ class TracyWorldConfig(WorldWithFixedRobot):
     def __init__(self):
         super().__init__({Derivatives.velocity: 0.2,
                           Derivatives.acceleration: np.inf,
-                          Derivatives.jerk: 15})
+                          Derivatives.jerk: None})
 
 
 class TracyCollisionAvoidanceConfig(LoadSelfCollisionMatrixConfig):

src/giskardpy_ros/configs/other_robots/armar.py

@@ -27,7 +27,7 @@ def setup(self):
         self.set_default_color(1, 1, 1, 1)
         self.set_default_limits({Derivatives.velocity: 1,
                                  Derivatives.acceleration: np.inf,
-                                 Derivatives.jerk: 30})
+                                 Derivatives.jerk: None})
         self.add_empty_link(self.map_name)
         self.add_6dof_joint(parent_link=self.map_name, child_link=self.odom_link_name,
                             joint_name=self.localization_joint_name)
@@ -43,12 +43,12 @@ def setup(self):
                                   translation_limits={
                                       Derivatives.velocity: 0.2,
                                       Derivatives.acceleration: 1,
-                                      Derivatives.jerk: 6,
+                                      Derivatives.jerk: None,
                                   },
                                   rotation_limits={
                                       Derivatives.velocity: 0.2,
                                       Derivatives.acceleration: 1,
-                                      Derivatives.jerk: 6
+                                      Derivatives.jerk: None
                                   },
                                   robot_group_name=self.robot_group_name)
 

src/giskardpy_ros/tree/behaviors/kinematic_sim.py

@@ -19,7 +19,7 @@ def f(joint_symbol):
     @profile
     def update(self):
         next_cmds = god_map.qp_solver_solution
-        god_map.world.update_state(next_cmds, god_map.qp_controller.sample_period,
+        god_map.world.update_state(next_cmds, god_map.qp_controller.mpc_dt,
                                    max_derivative=god_map.qp_controller.max_derivative)
         # god_map.world.notify_state_change()
         return Status.SUCCESS

src/giskardpy_ros/tree/behaviors/plot_debug_expressions.py

@@ -48,7 +48,7 @@ def split_traj(self, traj) -> Trajectory:
     def plot(self):
         trajectory = god_map.debug_expression_manager.raw_traj_to_traj()
         if trajectory and len(trajectory.items()) > 0:
-            sample_period = god_map.qp_controller.sample_period
+            sample_period = god_map.qp_controller.mpc_dt
             traj = self.split_traj(trajectory)
             try:
                 traj.plot_trajectory(path_to_data_folder=self.path_to_data_folder,

src/giskardpy_ros/tree/behaviors/plot_goal_gantt_chart.py

@@ -104,7 +104,7 @@ def get_new_history(self) \
         god_map.monitor_manager.evaluate_monitors()
 
         # add Nones to make sure all bars gets "ended"
-        new_end_time = god_map.time + god_map.qp_controller.sample_period
+        new_end_time = god_map.time + god_map.qp_controller.mpc_dt
 
         monitor_history = copy(god_map.monitor_manager.state_history)
         monitor_history.append((new_end_time, ([None] * len(monitor_history[0][1][0]), [None] * len(monitor_history[0][1][0]))))

src/giskardpy_ros/tree/behaviors/plot_trajectory.py

@@ -29,7 +29,7 @@ def initialise(self):
     def plot(self):
         trajectory = god_map.trajectory
         if trajectory:
-            sample_period = god_map.qp_controller.sample_period
+            sample_period = god_map.qp_controller.mpc_dt
             try:
                 trajectory.plot_trajectory(path_to_data_folder=self.path_to_data_folder,
                                            sample_period=sample_period,

src/giskardpy_ros/tree/behaviors/real_kinematic_sim.py

@@ -25,8 +25,8 @@ def update(self):
             return Status.RUNNING
         next_cmds = god_map.qp_solver_solution
         dt = next_time - self.last_time
-        if dt > god_map.qp_controller.sample_period:
-            dt = god_map.qp_controller.sample_period
+        if dt > god_map.qp_controller.mpc_dt:
+            dt = god_map.qp_controller.mpc_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

src/giskardpy_ros/tree/behaviors/send_trajectory.py

@@ -105,7 +105,7 @@ def initialise(self):
         if fill_velocity_values is None:
             fill_velocity_values = self.fill_velocity_values
         goal.trajectory = msg_converter.trajectory_to_ros_trajectory(trajectory,
-                                                                     god_map.qp_controller.sample_period,
+                                                                     god_map.qp_controller.mpc_dt,
                                                                      start_time,
                                                                      self.controlled_joints,
                                                                      fill_velocity_values)

src/giskardpy_ros/tree/behaviors/set_move_result.py

@@ -32,7 +32,7 @@ def update(self):
 
         trajectory = god_map.trajectory
         joints = [god_map.world.joints[joint_name] for joint_name in god_map.world.movable_joint_names]
-        sample_period = god_map.qp_controller.sample_period
+        sample_period = god_map.qp_controller.mpc_dt
         move_result.trajectory = msg_converter.trajectory_to_ros_trajectory(trajectory,
                                                                             sample_period=sample_period,
                                                                             start_time=0,

src/giskardpy_ros/tree/behaviors/time.py

@@ -14,7 +14,7 @@ def __init__(self, name: Optional[str] = 'time'):
 
     @profile
     def update(self):
-        god_map.time += god_map.qp_controller.sample_period
+        god_map.time += god_map.qp_controller.mpc_dt
         return Status.SUCCESS
 
 

src/giskardpy_ros/tree/blackboard_utils.py

@@ -24,8 +24,6 @@ class GiskardBlackboard(Blackboard):
     world_action_server: ActionServerHandler
     ros_visualizer: ROSMsgVisualization
     fill_trajectory_velocity_values: bool
-    control_loop_max_hz: float
-    simulation_max_hz: float
 
 
 def raise_to_blackboard(exception):

src/giskardpy_ros/tree/branches/control_loop.py

@@ -37,7 +37,8 @@ class ControlLoop(AsyncBehavior):
     log_traj: LogTrajPlugin
     controller_plugin: ControllerPlugin
 
-    def __init__(self, name: str = 'control_loop', log_traj: bool = True, max_hz: Optional[float] = None):
+    def __init__(self, name: str = 'control_loop', log_traj: bool = True):
+        max_hz = 1/GiskardBlackboard().giskard.qp_controller_config.control_dt
         name = f'{name}\nmax_hz: {max_hz}'
         super().__init__(name, max_hz=max_hz)
         self.publish_state = success_is_running(PublishState)('publish state 2')

src/giskardpy_ros/tree/branches/giskard_bt.py

@@ -51,11 +51,7 @@ def __init__(self, control_mode: ControlModes):
         self.root = Sequence('Giskard')
         self.wait_for_goal = WaitForGoal()
         self.prepare_control_loop = failure_is_success(PrepareControlLoop)()
-        if self.is_closed_loop():
-            max_hz = GiskardBlackboard().control_loop_max_hz
-        else:
-            max_hz = GiskardBlackboard().simulation_max_hz
-        self.control_loop_branch = failure_is_success(ControlLoop)(max_hz=max_hz)
+        self.control_loop_branch = failure_is_success(ControlLoop)()
         if self.is_closed_loop():
             self.control_loop_branch.add_closed_loop_behaviors()
         else:

test/test_integration_hsr.py

@@ -40,7 +40,7 @@ def __init__(self, giskard=None):
                               behavior_tree_config=StandAloneBTConfig(debug_mode=True,
                                                                       publish_tf=True,
                                                                       publish_js=False),
-                              qp_controller_config=QPControllerConfig())
+                              qp_controller_config=QPControllerConfig(mpc_dt=0.05))
         super().__init__(giskard)
         self.gripper_group = 'gripper'
         # self.r_gripper = rospy.ServiceProxy('r_gripper_simulator/set_joint_states', SetJointState)

test/test_integration_pr2.py

@@ -165,10 +165,9 @@ def __init__(self, giskard: Optional[Giskard] = None):
                                                                                # collision_checker=CollisionCheckerLib.none),
                                                                                ),
                               behavior_tree_config=StandAloneBTConfig(debug_mode=True,
-                                                                      publish_tf=True,
-                                                                      simulation_max_hz=None),
+                                                                      publish_tf=True),
                               # qp_controller_config=QPControllerConfig(qp_solver=SupportedQPSolver.gurobi))
-                              qp_controller_config=QPControllerConfig())
+                              qp_controller_config=QPControllerConfig(mpc_dt=0.05))
         super().__init__(giskard)
         self.robot = god_map.world.groups[self.robot_name]
 
@@ -1280,12 +1279,12 @@ def test_SetMaxTrajLength(self, zero_pose: PR2TestWrapper):
         zero_pose.set_max_traj_length(new_length)
         zero_pose.set_cart_goal(base_goal, tip_link='base_footprint', root_link='map')
         result = zero_pose.execute(expected_error_type=MaxTrajectoryLengthException)
-        dt = god_map.qp_controller.sample_period
+        dt = god_map.qp_controller.mpc_dt
         np.testing.assert_almost_equal(len(result.trajectory.points) * dt, new_length + dt)
 
         zero_pose.set_cart_goal(base_goal, tip_link='base_footprint', root_link='map')
         result = zero_pose.execute(expected_error_type=MaxTrajectoryLengthException)
-        dt = god_map.qp_controller.sample_period
+        dt = god_map.qp_controller.mpc_dt
         assert len(result.trajectory.points) * dt > new_length + 1
 
     def test_CollisionAvoidanceHint(self, kitchen_setup: PR2TestWrapper):

test/utils_for_tests.py

@@ -538,7 +538,7 @@ def send_goal(self,
             diff = time() - time_spend_giskarding
             self.total_time_spend_giskarding += diff
             self.total_time_spend_moving += (len(god_map.trajectory.keys()) *
-                                             god_map.qp_controller.sample_period)
+                                             god_map.qp_controller.mpc_dt)
             get_middleware().logwarn(f'Goal processing took {diff}')
             result_exception = msg_converter.error_msg_to_exception(r.error)
             if expected_error_type is not None: