Merge pull request #158 from SemRoCo/monitor_end_conditions

Monitor hold and end conditions

scripts/examples/python_interface_example.py

@@ -2,7 +2,7 @@
 from geometry_msgs.msg import PoseStamped
 
 from giskardpy.goals.joint_goals import JointPositionList
-from giskardpy.monitors.joint_monitors import JointGoalReached
+from giskardpy.motion_graph.monitors import JointGoalReached
 from giskardpy.python_interface.python_interface import GiskardWrapper
 
 # %% Define goals for later
@@ -97,10 +97,9 @@
 
 # %% Define when the motion should end.
 # Usually you'd use the local minimum reached monitor for this.
-# Most monitors also have a stay_true parameter (when it makes sense), with reasonable default values.
 # In this case, we don't want the local minimum reached monitor to stay True, because it might get triggered during
 # the sleeps and therefore set it to False.
-local_min = giskard_wrapper.monitors.add_local_minimum_reached(stay_true=False)
+local_min = giskard_wrapper.monitors.add_local_minimum_reached(end_condition='')
 
 # Giskard will only end the motion generation and return Success, if an end monitor becomes True.
 # We do this by defining one that gets triggered, when a local minimum was reached, sleep2 is done and the motion goals

src/giskardpy/casadi_wrapper.py

@@ -2,6 +2,7 @@
 
 import builtins
 from copy import copy
+from enum import IntEnum
 from typing import Union, List, TypeVar
 import math
 
@@ -11,8 +12,6 @@
 import geometry_msgs.msg as geometry_msgs
 import rospy
 from scipy import sparse as sp
-from giskardpy.data_types import PrefixName, Derivatives
-from giskardpy.utils import logging
 
 builtin_max = builtins.max
 builtin_min = builtins.min
@@ -68,7 +67,7 @@ def __init__(self, expression, parameters=None, sparse=False):
         else:
             try:
                 self.compiled_f = ca.Function('f', parameters, [ca.densify(expression.s)])
-            except Exception:
+            except Exception as e:
                 self.compiled_f = ca.Function('f', parameters, ca.densify(expression.s))
             self.buf, self.f_eval = self.compiled_f.buffer()
             if expression.shape[1] <= 1:
@@ -1351,7 +1350,7 @@ def dot(self, other):
 
 all_expressions = Union[Symbol_, Symbol, Expression, Point3, Vector3, RotationMatrix, TransMatrix, Quaternion]
 all_expressions_float = Union[Symbol, Expression, Point3, Vector3, RotationMatrix, TransMatrix, float, Quaternion]
-symbol_expr_float = Union[Symbol, Expression, float]
+symbol_expr_float = Union[Symbol, Expression, float, int, IntEnum]
 symbol_expr = Union[Symbol, Expression]
 PreservedCasType = TypeVar('PreservedCasType', Point3, Vector3, TransMatrix, RotationMatrix, Quaternion, Expression)
 
@@ -1493,9 +1492,9 @@ def limit(x, lower_limit, upper_limit):
 
 def if_else(condition, if_result, else_result):
     condition = Expression(condition).s
-    if isinstance(if_result, float):
+    if isinstance(if_result, (float, int)):
         if_result = Expression(if_result)
-    if isinstance(else_result, float):
+    if isinstance(else_result, (float, int)):
         else_result = Expression(else_result)
     if isinstance(if_result, (Point3, Vector3, TransMatrix, RotationMatrix, Quaternion)):
         assert type(if_result) == type(else_result), \

src/giskardpy/casadi_wrapper.pyi

@@ -1,6 +1,7 @@
 from __future__ import annotations
 
 import functools
+from enum import IntEnum
 from typing import overload, Union, Iterable, Tuple, Optional, Callable, List, Any, Sequence, Dict, TypeVar
 import numpy as np
 import casadi as ca  # type: ignore
@@ -11,7 +12,7 @@ from scipy import sparse as sp
 
 all_expressions = Union[Symbol_, Symbol, Expression, Point3, Vector3, RotationMatrix, TransMatrix, Quaternion]
 all_expressions_float = Union[Symbol, Expression, Point3, Vector3, RotationMatrix, TransMatrix, float, Quaternion]
-symbol_expr_float = Union[Symbol, Expression, float]
+symbol_expr_float = Union[Symbol, Expression, float, int, IntEnum]
 symbol_expr = Union[Symbol, Expression]
 PreservedCasType = TypeVar('PreservedCasType', Point3, Vector3, TransMatrix, RotationMatrix, Quaternion, Expression)
 

src/giskardpy/configs/giskard.py

@@ -10,7 +10,7 @@
 from giskardpy.configs.world_config import WorldConfig
 from giskardpy.exceptions import GiskardException, SetupException
 from giskardpy.goals.goal import Goal
-from giskardpy.monitors.monitors import Monitor
+from giskardpy.motion_graph.monitors.monitors import Monitor
 from giskardpy.utils import logging
 from giskardpy.utils.utils import resolve_ros_iris, get_all_classes_in_package
 
@@ -23,7 +23,7 @@ class Giskard:
     qp_controller_config: QPControllerConfig = None
     tmp_folder: str = resolve_ros_iris('package://giskardpy/tmp/')
     goal_package_paths = {'giskardpy.goals'}
-    monitor_package_paths = {'giskardpy.monitors'}
+    monitor_package_paths = {'giskardpy.motion_graph.monitors'}
     action_server_name: str = '~command'
 
     def __init__(self,

src/giskardpy/data_types.py

@@ -15,7 +15,7 @@ class PrefixName:
     primary_separator = '/'
     secondary_separator = '_'
 
-    def __init__(self, name: str, prefix: Optional[Union[str, PrefixName]]):
+    def __init__(self, name: str, prefix: Optional[Union[str, PrefixName]] = None):
         if isinstance(prefix, PrefixName):
             self.prefix = prefix.prefix
             old_suffix = prefix.short_name

src/giskardpy/goals/align_planes.py

@@ -1,13 +1,12 @@
-from typing import Optional, List
+from typing import Optional
 
 from geometry_msgs.msg import Vector3Stamped
 from std_msgs.msg import ColorRGBA
 
 import giskardpy.utils.tfwrapper as tf
 import giskardpy.casadi_wrapper as cas
 from giskardpy.goals.goal import Goal
-from giskardpy.monitors.monitors import ExpressionMonitor
-from giskardpy.tasks.task import WEIGHT_ABOVE_CA, Task
+from giskardpy.motion_graph.tasks.task import WEIGHT_ABOVE_CA
 from giskardpy.god_map import god_map
 from giskardpy.utils.expression_definition_utils import transform_msg
 from giskardpy.utils.logging import logwarn

src/giskardpy/goals/align_to_push_door.py

@@ -1,14 +1,13 @@
 from typing import Optional
 import numpy as np
 
-from geometry_msgs.msg import Vector3Stamped, PointStamped, Quaternion
+from geometry_msgs.msg import Vector3Stamped
 from std_msgs.msg import ColorRGBA
 
 from giskardpy import casadi_wrapper as cas
 from giskardpy.goals.goal import Goal
 from giskardpy.god_map import god_map
-from giskardpy.symbol_manager import symbol_manager
-from giskardpy.tasks.task import WEIGHT_BELOW_CA
+from giskardpy.motion_graph.tasks.task import WEIGHT_BELOW_CA
 from giskardpy.utils import tfwrapper as tf
 from giskardpy.exceptions import GoalInitalizationException
 

src/giskardpy/goals/base_traj_follower.py

@@ -1,6 +1,6 @@
 from __future__ import division, annotations
 
-from typing import Optional, List, Dict, Tuple
+from typing import Optional, List, Dict
 
 import numpy as np
 # import matplotlib.pyplot as plt
@@ -11,10 +11,10 @@
 from visualization_msgs.msg import MarkerArray, Marker
 
 import giskardpy.casadi_wrapper as cas
-from giskardpy.exceptions import GiskardException, GoalInitalizationException, ExecutionException
+from giskardpy.exceptions import GoalInitalizationException, ExecutionException
 from giskardpy.goals.goal import Goal
-from giskardpy.monitors.monitors import ExpressionMonitor, EndMotion
-from giskardpy.tasks.task import WEIGHT_ABOVE_CA, WEIGHT_BELOW_CA, WEIGHT_COLLISION_AVOIDANCE, Task
+from giskardpy.motion_graph.monitors.monitors import ExpressionMonitor, EndMotion
+from giskardpy.motion_graph.tasks.task import WEIGHT_ABOVE_CA, WEIGHT_BELOW_CA, WEIGHT_COLLISION_AVOIDANCE
 from giskardpy.god_map import god_map
 from giskardpy.model.joints import OmniDrive, OmniDrivePR22
 from giskardpy.data_types import my_string, Derivatives, PrefixName

src/giskardpy/goals/cartesian_goals.py

@@ -13,9 +13,9 @@
 from giskardpy.goals.goal import Goal
 from giskardpy.god_map import god_map
 from giskardpy.model.joints import DiffDrive
-from giskardpy.monitors.monitors import ExpressionMonitor
+from giskardpy.motion_graph.monitors.monitors import ExpressionMonitor
 from giskardpy.symbol_manager import symbol_manager
-from giskardpy.tasks.task import WEIGHT_BELOW_CA, WEIGHT_ABOVE_CA
+from giskardpy.motion_graph.tasks.task import WEIGHT_BELOW_CA, WEIGHT_ABOVE_CA
 from giskardpy.utils.expression_definition_utils import transform_msg_and_turn_to_expr, transform_msg
 from giskardpy.utils.tfwrapper import normalize
 from giskardpy.utils.utils import split_pose_stamped
@@ -591,14 +591,14 @@ def __init__(self,
         root_P_goal1 = cas.Point3(self.root_P_goal1)
 
         error1 = cas.euclidean_distance(root_P_goal1, root_P_current)
-        error1_monitor = ExpressionMonitor(name='p1',
-                                           stay_true=True)
+        error1_monitor = ExpressionMonitor(name='p1')
         self.add_monitor(error1_monitor)
+        error1_monitor.end_condition = error1_monitor.get_state_expression()
         error1_monitor.expression = cas.less(cas.abs(error1), 0.01)
 
-        error2_monitor = ExpressionMonitor(name='p2',
-                                           stay_true=True)
+        error2_monitor = ExpressionMonitor(name='p2')
         self.add_monitor(error2_monitor)
+        error2_monitor.end_condition = error2_monitor.get_state_expression()
 
         root_P_goal2_cached = transform_msg_and_turn_to_expr(self.root_link,
                                                              self.tip_P_goal2,

src/giskardpy/goals/caster.py

@@ -1,7 +1,7 @@
 from geometry_msgs.msg import PointStamped
 import giskardpy.casadi_wrapper as cas
 from giskardpy.goals.goal import Goal
-from giskardpy.tasks.task import WEIGHT_BELOW_CA, WEIGHT_ABOVE_CA
+from giskardpy.motion_graph.tasks.task import WEIGHT_BELOW_CA, WEIGHT_ABOVE_CA
 from giskardpy.god_map import god_map
 from giskardpy.data_types import PrefixName
 

src/giskardpy/goals/collision_avoidance.py

@@ -8,9 +8,9 @@
 import giskardpy.utils.tfwrapper as tf
 from giskard_msgs.msg import CollisionEntry
 from giskardpy.goals.goal import Goal
-from giskardpy.monitors.monitors import ExpressionMonitor
-from giskardpy.monitors.payload_monitors import CollisionMatrixUpdater
-from giskardpy.tasks.task import WEIGHT_ABOVE_CA, WEIGHT_COLLISION_AVOIDANCE, Task
+from giskardpy.motion_graph.monitors.monitors import ExpressionMonitor
+from giskardpy.motion_graph.monitors.payload_monitors import CollisionMatrixUpdater
+from giskardpy.motion_graph.tasks.task import WEIGHT_ABOVE_CA, WEIGHT_COLLISION_AVOIDANCE
 from giskardpy.god_map import god_map
 from giskardpy.data_types import my_string
 from giskardpy.symbol_manager import symbol_manager

src/giskardpy/goals/diff_drive_goals.py

@@ -1,17 +1,14 @@
 from __future__ import division
 
-from typing import Optional, List
+from typing import Optional
 
 from geometry_msgs.msg import Vector3Stamped, PointStamped
 
 import giskardpy.utils.tfwrapper as tf
 import giskardpy.casadi_wrapper as cas
 from giskardpy.goals.goal import Goal
-from giskardpy.monitors.monitors import ExpressionMonitor
-from giskardpy.tasks.task import WEIGHT_ABOVE_CA, Task
+from giskardpy.motion_graph.tasks.task import WEIGHT_ABOVE_CA
 from giskardpy.god_map import god_map
-from giskardpy.model.joints import OmniDrivePR22
-from giskardpy.data_types import Derivatives
 from giskardpy.utils.expression_definition_utils import transform_msg
 
 

src/giskardpy/goals/goal.py

@@ -2,19 +2,15 @@
 
 import abc
 from abc import ABC
-from collections import OrderedDict
-from typing import Optional, Tuple, Dict, List, Union
+from typing import List, Union
 
-from giskardpy.monitors.monitors import ExpressionMonitor, Monitor
+from giskardpy.motion_graph.monitors.monitors import ExpressionMonitor, Monitor
 from giskardpy.god_map import god_map
-from giskardpy.symbol_manager import symbol_manager
-from giskardpy.tasks.task import Task
+from giskardpy.motion_graph.tasks.task import Task
 from giskardpy.utils.utils import string_shortener
-import giskardpy.casadi_wrapper as cas
 from giskardpy.exceptions import GoalInitalizationException
 from giskardpy.model.joints import OneDofJoint
 from giskardpy.data_types import PrefixName, Derivatives
-from giskardpy.qp.constraint import InequalityConstraint, EqualityConstraint, DerivativeInequalityConstraint
 import giskardpy.casadi_wrapper as cas
 
 

src/giskardpy/goals/goals_tests.py

@@ -1,10 +1,9 @@
-from typing import Optional, List
+from typing import Optional
 
 import numpy as np
 from geometry_msgs.msg import QuaternionStamped, PointStamped, PoseStamped, Vector3Stamped
 import giskardpy.casadi_wrapper as cas
 from giskardpy.goals.goal import Goal
-from giskardpy.monitors.monitors import ExpressionMonitor
 from giskardpy.god_map import god_map
 from giskardpy.symbol_manager import symbol_manager
 

src/giskardpy/goals/grasp_bar.py

@@ -1,14 +1,13 @@
 from __future__ import division
 
-from typing import Optional, List
+from typing import Optional
 
 from geometry_msgs.msg import Vector3Stamped, PointStamped
 
 import giskardpy.utils.tfwrapper as tf
 import giskardpy.casadi_wrapper as cas
 from giskardpy.goals.goal import Goal
-from giskardpy.monitors.monitors import ExpressionMonitor
-from giskardpy.tasks.task import WEIGHT_ABOVE_CA, Task
+from giskardpy.motion_graph.tasks.task import WEIGHT_ABOVE_CA
 from giskardpy.god_map import god_map
 from giskardpy.utils.expression_definition_utils import transform_msg
 

src/giskardpy/goals/joint_goals.py

@@ -9,7 +9,7 @@
 from giskardpy.god_map import god_map
 from giskardpy.model.joints import OneDofJoint
 from giskardpy.symbol_manager import symbol_manager
-from giskardpy.tasks.task import WEIGHT_BELOW_CA
+from giskardpy.motion_graph.tasks.task import WEIGHT_BELOW_CA
 
 
 class JointVelocityLimit(Goal):

src/giskardpy/goals/motion_goal_manager.py

@@ -12,9 +12,9 @@
 from giskardpy.goals.goal import Goal
 import giskard_msgs.msg as giskard_msgs
 from giskardpy.god_map import god_map
+from giskardpy.motion_graph.helpers import compile_graph_node_state_updater
 from giskardpy.qp.constraint import EqualityConstraint, InequalityConstraint, DerivativeInequalityConstraint
-from giskardpy.symbol_manager import symbol_manager
-from giskardpy.tasks.task import Task
+from giskardpy.motion_graph.tasks.task import Task
 from giskardpy.utils import logging
 from giskardpy.utils.utils import get_all_classes_in_package, convert_dictionary_to_ros_message, \
     json_str_to_kwargs, ImmutableDict
@@ -88,50 +88,11 @@ def add_motion_goal(self, goal: Goal) -> None:
             if task.name in self.tasks:
                 raise DuplicateNameException(f'Task names {task.name} already exists.')
             self.tasks[task.name] = task
-            task.set_id(len(self.tasks) - 1)
+            task.id = len(self.tasks) - 1
 
     @profile
     def compile_task_state_updater(self):
-        symbols = []
-        for task in sorted(self.tasks.values(), key=lambda x: x.id):
-            symbols.append(task.get_state_expression())
-        task_state = cas.Expression(symbols)
-        symbols = []
-        for i, monitor in enumerate(god_map.monitor_manager.monitors):
-            symbols.append(monitor.get_state_expression())
-        monitor_state = cas.Expression(symbols)
-
-        state_updater = []
-        for task in sorted(self.tasks.values(), key=lambda x: x.id):
-            state_symbol = task_state[task.id]
-
-            if not cas.is_true(task.start_condition):
-                start_if = cas.if_else(task.start_condition,
-                                       if_result=int(TaskState.running),
-                                       else_result=state_symbol)
-            else:
-                start_if = state_symbol
-            if not cas.is_true(task.hold_condition):
-                hold_if = cas.if_else(task.hold_condition,
-                                      if_result=int(TaskState.on_hold),
-                                      else_result=int(TaskState.running))
-            else:
-                hold_if = state_symbol
-            if not cas.is_false(task.end_condition):
-                else_result = cas.if_else(task.end_condition,
-                                          if_result=int(TaskState.succeeded),
-                                          else_result=hold_if)
-            else:
-                else_result = hold_if
-
-            state_f = cas.if_eq_cases(a=state_symbol,
-                                      b_result_cases=[(int(TaskState.not_started), start_if),
-                                                      (int(TaskState.succeeded), int(TaskState.succeeded))],
-                                      else_result=else_result)  # running or on_hold
-            state_updater.append(state_f)
-        state_updater = cas.Expression(state_updater)
-        symbols = task_state.free_symbols() + monitor_state.free_symbols()
-        self.compiled_state_updater = state_updater.compile(symbols)
+        self.compiled_state_updater = compile_graph_node_state_updater(self.tasks)
 
     @profile
     def update_task_state(self, new_state: np.ndarray) -> None: