From 2050e2acf429c542f0d2135f59c44506343ac0df Mon Sep 17 00:00:00 2001 From: Simon Date: Thu, 6 Jun 2024 15:03:50 +0200 Subject: [PATCH 01/22] moved tasks and monitors into motion_graph package and added graph node --- scripts/examples/python_interface_example.py | 2 +- src/giskardpy/configs/giskard.py | 4 +-- src/giskardpy/goals/align_planes.py | 5 ++- src/giskardpy/goals/align_to_push_door.py | 5 ++- src/giskardpy/goals/base_traj_follower.py | 8 ++--- src/giskardpy/goals/cartesian_goals.py | 4 +-- src/giskardpy/goals/caster.py | 2 +- src/giskardpy/goals/collision_avoidance.py | 6 ++-- src/giskardpy/goals/diff_drive_goals.py | 7 ++--- src/giskardpy/goals/goal.py | 10 ++---- src/giskardpy/goals/goals_tests.py | 3 +- src/giskardpy/goals/grasp_bar.py | 5 ++- src/giskardpy/goals/joint_goals.py | 5 ++- src/giskardpy/goals/motion_goal_manager.py | 3 +- src/giskardpy/goals/open_close.py | 5 ++- src/giskardpy/goals/pointing.py | 5 ++- src/giskardpy/goals/pre_push_door.py | 2 +- src/giskardpy/goals/realtime_goals.py | 5 ++- src/giskardpy/goals/set_prediction_horizon.py | 3 +- src/giskardpy/goals/tracebot.py | 4 +-- src/giskardpy/goals/weight_scaling_goals.py | 1 - src/giskardpy/god_map.py | 4 +-- .../{monitors => motion_graph}/__init__.py | 0 src/giskardpy/motion_graph/graph_node.py | 27 ++++++++++++++++ .../monitors}/__init__.py | 0 .../monitors/cartesian_monitors.py | 4 +-- .../monitors/joint_monitors.py | 4 +-- .../monitors/monitor_manager.py | 4 +-- .../{ => motion_graph}/monitors/monitors.py | 0 .../monitors/payload_monitors.py | 9 +++--- src/giskardpy/motion_graph/tasks/__init__.py | 0 .../{ => motion_graph}/tasks/task.py | 31 +++---------------- .../python_interface/old_python_interface.py | 2 +- .../python_interface/python_interface.py | 13 ++++---- .../tree/behaviors/check_monitor_state.py | 3 +- src/giskardpy/tree/behaviors/cleanup.py | 2 +- .../tree/behaviors/execute_payload_monitor.py | 2 +- .../tree/behaviors/plot_goal_gantt_chart.py | 4 +-- .../tree/behaviors/plot_task_graph.py | 3 +- .../tree/behaviors/ros_msg_to_goal.py | 3 +- src/giskardpy/tree/branches/check_monitors.py | 4 +-- .../tree/branches/payload_monitor_sequence.py | 2 +- .../utils/expression_definition_utils.py | 5 ++- test/test_integration_pr2.py | 4 +-- ...test_integration_pr2_mujoco_closed_loop.py | 10 +++--- test/test_integration_tiago_stand_alone.py | 2 +- test/utils_for_tests.py | 4 +-- 47 files changed, 112 insertions(+), 128 deletions(-) rename src/giskardpy/{monitors => motion_graph}/__init__.py (100%) create mode 100644 src/giskardpy/motion_graph/graph_node.py rename src/giskardpy/{tasks => motion_graph/monitors}/__init__.py (100%) rename src/giskardpy/{ => motion_graph}/monitors/cartesian_monitors.py (98%) rename src/giskardpy/{ => motion_graph}/monitors/joint_monitors.py (91%) rename src/giskardpy/{ => motion_graph}/monitors/monitor_manager.py (98%) rename src/giskardpy/{ => motion_graph}/monitors/monitors.py (100%) rename src/giskardpy/{ => motion_graph}/monitors/payload_monitors.py (94%) create mode 100644 src/giskardpy/motion_graph/tasks/__init__.py rename src/giskardpy/{ => motion_graph}/tasks/task.py (97%) diff --git a/scripts/examples/python_interface_example.py b/scripts/examples/python_interface_example.py index 89e89c2050..9b2644d143 100644 --- a/scripts/examples/python_interface_example.py +++ b/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 diff --git a/src/giskardpy/configs/giskard.py b/src/giskardpy/configs/giskard.py index 93fc5e4f1c..9fdd354a39 100644 --- a/src/giskardpy/configs/giskard.py +++ b/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, diff --git a/src/giskardpy/goals/align_planes.py b/src/giskardpy/goals/align_planes.py index c96278d2a8..ee2ee47b7f 100644 --- a/src/giskardpy/goals/align_planes.py +++ b/src/giskardpy/goals/align_planes.py @@ -1,4 +1,4 @@ -from typing import Optional, List +from typing import Optional from geometry_msgs.msg import Vector3Stamped from std_msgs.msg import ColorRGBA @@ -6,8 +6,7 @@ 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 diff --git a/src/giskardpy/goals/align_to_push_door.py b/src/giskardpy/goals/align_to_push_door.py index 9071cbd915..5e6ffc342e 100644 --- a/src/giskardpy/goals/align_to_push_door.py +++ b/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 diff --git a/src/giskardpy/goals/base_traj_follower.py b/src/giskardpy/goals/base_traj_follower.py index a8b2a8f208..112e9e0a4b 100644 --- a/src/giskardpy/goals/base_traj_follower.py +++ b/src/giskardpy/goals/base_traj_follower.py @@ -1,6 +1,6 @@ from __future__ import division -from typing import Optional, List, Dict, Tuple +from typing import Optional, List, Dict import numpy as np # import matplotlib.pyplot as plt @@ -10,10 +10,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 diff --git a/src/giskardpy/goals/cartesian_goals.py b/src/giskardpy/goals/cartesian_goals.py index 64fbf6810b..566de17b65 100644 --- a/src/giskardpy/goals/cartesian_goals.py +++ b/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 diff --git a/src/giskardpy/goals/caster.py b/src/giskardpy/goals/caster.py index b0d23ff462..44b26ec05d 100644 --- a/src/giskardpy/goals/caster.py +++ b/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 diff --git a/src/giskardpy/goals/collision_avoidance.py b/src/giskardpy/goals/collision_avoidance.py index e60abbf127..06f340d6bb 100644 --- a/src/giskardpy/goals/collision_avoidance.py +++ b/src/giskardpy/goals/collision_avoidance.py @@ -5,9 +5,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 diff --git a/src/giskardpy/goals/diff_drive_goals.py b/src/giskardpy/goals/diff_drive_goals.py index 64c20a4ef5..13e891f582 100644 --- a/src/giskardpy/goals/diff_drive_goals.py +++ b/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 diff --git a/src/giskardpy/goals/goal.py b/src/giskardpy/goals/goal.py index aa75fa9035..f2afcc053f 100644 --- a/src/giskardpy/goals/goal.py +++ b/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 diff --git a/src/giskardpy/goals/goals_tests.py b/src/giskardpy/goals/goals_tests.py index a83d7b63a5..2651057742 100644 --- a/src/giskardpy/goals/goals_tests.py +++ b/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 NonMotionGoal, Goal -from giskardpy.monitors.monitors import ExpressionMonitor from giskardpy.god_map import god_map from giskardpy.symbol_manager import symbol_manager diff --git a/src/giskardpy/goals/grasp_bar.py b/src/giskardpy/goals/grasp_bar.py index 4718a7eb98..fbe0daaf46 100644 --- a/src/giskardpy/goals/grasp_bar.py +++ b/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 diff --git a/src/giskardpy/goals/joint_goals.py b/src/giskardpy/goals/joint_goals.py index 75067d0b72..9ee4e315ff 100644 --- a/src/giskardpy/goals/joint_goals.py +++ b/src/giskardpy/goals/joint_goals.py @@ -5,12 +5,11 @@ from geometry_msgs.msg import PoseStamped from giskardpy import casadi_wrapper as cas -from giskardpy.monitors.monitors import ExpressionMonitor from giskardpy.god_map import god_map from giskardpy.symbol_manager import symbol_manager -from giskardpy.exceptions import MotionBuildingException, GoalInitalizationException +from giskardpy.exceptions import GoalInitalizationException from giskardpy.goals.goal import Goal, NonMotionGoal -from giskardpy.tasks.task import WEIGHT_BELOW_CA, Task +from giskardpy.motion_graph.tasks.task import WEIGHT_BELOW_CA from giskardpy.model.joints import OmniDrive, DiffDrive, OmniDrivePR22, OneDofJoint from giskardpy.data_types import PrefixName, Derivatives from giskardpy.utils.expression_definition_utils import transform_msg diff --git a/src/giskardpy/goals/motion_goal_manager.py b/src/giskardpy/goals/motion_goal_manager.py index 0bfb86e21e..6a96c27250 100644 --- a/src/giskardpy/goals/motion_goal_manager.py +++ b/src/giskardpy/goals/motion_goal_manager.py @@ -13,8 +13,7 @@ import giskard_msgs.msg as giskard_msgs from giskardpy.god_map import god_map 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 diff --git a/src/giskardpy/goals/open_close.py b/src/giskardpy/goals/open_close.py index bc64dd07ff..c905c41a1d 100644 --- a/src/giskardpy/goals/open_close.py +++ b/src/giskardpy/goals/open_close.py @@ -1,11 +1,10 @@ from __future__ import division -from typing import Optional, List +from typing import Optional from giskardpy.goals.cartesian_goals import CartesianPosition, CartesianOrientation from giskardpy.goals.goal import Goal -from giskardpy.monitors.monitors import ExpressionMonitor -from giskardpy.tasks.task import WEIGHT_BELOW_CA, WEIGHT_ABOVE_CA, Task +from giskardpy.motion_graph.tasks.task import WEIGHT_BELOW_CA, WEIGHT_ABOVE_CA from giskardpy.goals.joint_goals import JointPositionList from giskardpy.god_map import god_map import giskardpy.casadi_wrapper as cas diff --git a/src/giskardpy/goals/pointing.py b/src/giskardpy/goals/pointing.py index 824c91374a..ca9fff4b1a 100644 --- a/src/giskardpy/goals/pointing.py +++ b/src/giskardpy/goals/pointing.py @@ -1,6 +1,6 @@ from __future__ import division -from typing import Optional, List +from typing import Optional from geometry_msgs.msg import Vector3Stamped, PointStamped from std_msgs.msg import ColorRGBA @@ -8,9 +8,8 @@ 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.symbol_manager import symbol_manager -from giskardpy.tasks.task import WEIGHT_BELOW_CA, Task +from giskardpy.motion_graph.tasks.task import WEIGHT_BELOW_CA from giskardpy.god_map import god_map from giskardpy.utils.expression_definition_utils import transform_msg from giskardpy.utils.logging import logwarn diff --git a/src/giskardpy/goals/pre_push_door.py b/src/giskardpy/goals/pre_push_door.py index 32e6941d19..03ae827645 100644 --- a/src/giskardpy/goals/pre_push_door.py +++ b/src/giskardpy/goals/pre_push_door.py @@ -3,7 +3,7 @@ from giskardpy import casadi_wrapper as cas from giskardpy.goals.goal import Goal from giskardpy.god_map import god_map -from giskardpy.tasks.task import WEIGHT_BELOW_CA +from giskardpy.motion_graph.tasks.task import WEIGHT_BELOW_CA import numpy as np diff --git a/src/giskardpy/goals/realtime_goals.py b/src/giskardpy/goals/realtime_goals.py index dade5d1519..d0ad1127df 100644 --- a/src/giskardpy/goals/realtime_goals.py +++ b/src/giskardpy/goals/realtime_goals.py @@ -1,12 +1,11 @@ from __future__ import division -from typing import Optional, List +from typing import Optional import rospy from geometry_msgs.msg import Vector3Stamped, PointStamped -from giskardpy.monitors.monitors import ExpressionMonitor -from giskardpy.tasks.task import WEIGHT_BELOW_CA +from giskardpy.motion_graph.tasks.task import WEIGHT_BELOW_CA from giskardpy.goals.pointing import Pointing import giskardpy.casadi_wrapper as cas from giskardpy.utils.expression_definition_utils import transform_msg diff --git a/src/giskardpy/goals/set_prediction_horizon.py b/src/giskardpy/goals/set_prediction_horizon.py index dff17c5f7a..952ea04c4f 100644 --- a/src/giskardpy/goals/set_prediction_horizon.py +++ b/src/giskardpy/goals/set_prediction_horizon.py @@ -1,8 +1,7 @@ -from typing import Union, Optional, List +from typing import Union, Optional from giskardpy.configs.qp_controller_config import SupportedQPSolver from giskardpy.goals.goal import NonMotionGoal -from giskardpy.monitors.monitors import ExpressionMonitor from giskardpy.god_map import god_map from giskardpy.utils import logging import giskardpy.casadi_wrapper as cas diff --git a/src/giskardpy/goals/tracebot.py b/src/giskardpy/goals/tracebot.py index 8d139cbd32..575feddef9 100644 --- a/src/giskardpy/goals/tracebot.py +++ b/src/giskardpy/goals/tracebot.py @@ -7,8 +7,8 @@ from giskardpy import casadi_wrapper as cas from giskardpy.goals.goal import Goal -from giskardpy.tasks.task import WEIGHT_ABOVE_CA -from giskardpy.monitors.monitors import ExpressionMonitor +from giskardpy.motion_graph.tasks.task import WEIGHT_ABOVE_CA +from giskardpy.motion_graph.monitors.monitors import ExpressionMonitor from giskardpy.god_map import god_map from giskardpy.utils.expression_definition_utils import transform_msg diff --git a/src/giskardpy/goals/weight_scaling_goals.py b/src/giskardpy/goals/weight_scaling_goals.py index 1556693ba3..a9dab24671 100644 --- a/src/giskardpy/goals/weight_scaling_goals.py +++ b/src/giskardpy/goals/weight_scaling_goals.py @@ -6,7 +6,6 @@ from geometry_msgs.msg import PointStamped from giskardpy.utils.expression_definition_utils import transform_msg_and_turn_to_expr from giskardpy.data_types import Derivatives -from giskardpy.tasks.task import WEIGHT_ABOVE_CA from collections import defaultdict diff --git a/src/giskardpy/god_map.py b/src/giskardpy/god_map.py index c110623cbc..9600a3397d 100644 --- a/src/giskardpy/god_map.py +++ b/src/giskardpy/god_map.py @@ -5,7 +5,7 @@ import rospy -from giskard_msgs.msg import MoveGoal, MoveResult +from giskard_msgs.msg import MoveGoal if TYPE_CHECKING: from giskardpy.tree.behaviors.action_server import ActionServerHandler @@ -23,7 +23,7 @@ from giskardpy.configs.giskard import Giskard from giskardpy.goals.motion_goal_manager import MotionGoalManager from giskardpy.debug_expression_manager import DebugExpressionManager - from giskardpy.monitors.monitor_manager import MonitorManager + from giskardpy.motion_graph.monitors import MonitorManager from giskardpy.configs.collision_avoidance_config import CollisionAvoidanceConfig from giskardpy.configs.world_config import WorldConfig from giskardpy.model.collision_world_syncer import CollisionWorldSynchronizer, CollisionCheckerLib, \ diff --git a/src/giskardpy/monitors/__init__.py b/src/giskardpy/motion_graph/__init__.py similarity index 100% rename from src/giskardpy/monitors/__init__.py rename to src/giskardpy/motion_graph/__init__.py diff --git a/src/giskardpy/motion_graph/graph_node.py b/src/giskardpy/motion_graph/graph_node.py new file mode 100644 index 0000000000..55fd8dd3fc --- /dev/null +++ b/src/giskardpy/motion_graph/graph_node.py @@ -0,0 +1,27 @@ +import giskardpy.casadi_wrapper as cas +from giskardpy.data_types import PrefixName + + +class MotionGraphNode: + _start_condition: cas.Expression + _hold_condition: cas.Expression + _end_condition: cas.Expression + _name: str + _id: int + plot: bool + + def __init__(self, name: str, start_condition: cas.Expression, hold_condition: cas.Expression, end_condition: cas.Expression,): + self._start_condition = start_condition + self._hold_condition = hold_condition + self._end_condition = end_condition + self.plot = True + self._id = -1 + self._name = name + + @property + def id(self) -> int: + assert self._id >= 0, f'id of {self._name} is not set.' + return self._id + + def set_id(self, new_id: int) -> None: + self._id = new_id diff --git a/src/giskardpy/tasks/__init__.py b/src/giskardpy/motion_graph/monitors/__init__.py similarity index 100% rename from src/giskardpy/tasks/__init__.py rename to src/giskardpy/motion_graph/monitors/__init__.py diff --git a/src/giskardpy/monitors/cartesian_monitors.py b/src/giskardpy/motion_graph/monitors/cartesian_monitors.py similarity index 98% rename from src/giskardpy/monitors/cartesian_monitors.py rename to src/giskardpy/motion_graph/monitors/cartesian_monitors.py index fb2799cdeb..0e78bf3efb 100644 --- a/src/giskardpy/monitors/cartesian_monitors.py +++ b/src/giskardpy/motion_graph/monitors/cartesian_monitors.py @@ -1,9 +1,9 @@ -from typing import List, Optional +from typing import Optional from geometry_msgs.msg import PointStamped, QuaternionStamped, PoseStamped, Vector3Stamped import giskardpy.casadi_wrapper as cas -from giskardpy.monitors.monitors import ExpressionMonitor, Monitor +from giskardpy.motion_graph.monitors.monitors import ExpressionMonitor from giskardpy.god_map import god_map import giskardpy.utils.tfwrapper as tf from giskardpy.utils.expression_definition_utils import transform_msg, transform_msg_and_turn_to_expr diff --git a/src/giskardpy/monitors/joint_monitors.py b/src/giskardpy/motion_graph/monitors/joint_monitors.py similarity index 91% rename from src/giskardpy/monitors/joint_monitors.py rename to src/giskardpy/motion_graph/monitors/joint_monitors.py index 8f39dd7762..afbdb121d6 100644 --- a/src/giskardpy/monitors/joint_monitors.py +++ b/src/giskardpy/motion_graph/monitors/joint_monitors.py @@ -1,7 +1,7 @@ -from typing import List, Dict, Optional +from typing import Dict, Optional import giskardpy.casadi_wrapper as cas -from giskardpy.monitors.monitors import ExpressionMonitor, Monitor +from giskardpy.motion_graph.monitors.monitors import ExpressionMonitor from giskardpy.god_map import god_map from giskardpy.data_types import Derivatives diff --git a/src/giskardpy/monitors/monitor_manager.py b/src/giskardpy/motion_graph/monitors/monitor_manager.py similarity index 98% rename from src/giskardpy/monitors/monitor_manager.py rename to src/giskardpy/motion_graph/monitors/monitor_manager.py index fe340502e6..e2a7a12964 100644 --- a/src/giskardpy/monitors/monitor_manager.py +++ b/src/giskardpy/motion_graph/monitors/monitor_manager.py @@ -12,8 +12,8 @@ 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, EndMotion -from giskardpy.monitors.payload_monitors import PayloadMonitor, CancelMotion +from giskardpy.motion_graph.monitors.monitors import ExpressionMonitor, Monitor, EndMotion +from giskardpy.motion_graph.monitors.payload_monitors import PayloadMonitor, CancelMotion from giskardpy.symbol_manager import symbol_manager from giskardpy.utils import logging from giskardpy.utils.utils import get_all_classes_in_package, json_str_to_kwargs diff --git a/src/giskardpy/monitors/monitors.py b/src/giskardpy/motion_graph/monitors/monitors.py similarity index 100% rename from src/giskardpy/monitors/monitors.py rename to src/giskardpy/motion_graph/monitors/monitors.py diff --git a/src/giskardpy/monitors/payload_monitors.py b/src/giskardpy/motion_graph/monitors/payload_monitors.py similarity index 94% rename from src/giskardpy/monitors/payload_monitors.py rename to src/giskardpy/motion_graph/monitors/payload_monitors.py index 3821bef208..ac03ea3ae2 100644 --- a/src/giskardpy/monitors/payload_monitors.py +++ b/src/giskardpy/motion_graph/monitors/payload_monitors.py @@ -1,14 +1,13 @@ import abc -from abc import ABC from threading import Lock -from typing import List, Optional, Dict, Tuple +from typing import Optional, Dict, Tuple import numpy as np import rospy -from giskard_msgs.msg import MoveResult, GiskardError -from giskardpy.exceptions import GiskardException, MonitorInitalizationException -from giskardpy.monitors.monitors import Monitor, PayloadMonitor, CancelMotion +from giskard_msgs.msg import GiskardError +from giskardpy.exceptions import MonitorInitalizationException +from giskardpy.motion_graph.monitors.monitors import PayloadMonitor, CancelMotion from giskardpy.god_map import god_map from giskardpy.utils import logging import giskardpy.casadi_wrapper as cas diff --git a/src/giskardpy/motion_graph/tasks/__init__.py b/src/giskardpy/motion_graph/tasks/__init__.py new file mode 100644 index 0000000000..e69de29bb2 diff --git a/src/giskardpy/tasks/task.py b/src/giskardpy/motion_graph/tasks/task.py similarity index 97% rename from src/giskardpy/tasks/task.py rename to src/giskardpy/motion_graph/tasks/task.py index b3e88ebba3..184a68d7ea 100644 --- a/src/giskardpy/tasks/task.py +++ b/src/giskardpy/motion_graph/tasks/task.py @@ -1,17 +1,13 @@ -from enum import IntEnum from typing import Optional, List, Union, Dict, Callable, Iterable, overload, DefaultDict -import numpy as np - import giskard_msgs.msg as giskard_msgs import giskardpy.casadi_wrapper as cas from giskardpy.exceptions import GiskardException, GoalInitalizationException, DuplicateNameException from giskardpy.god_map import god_map -from giskardpy.monitors.monitors import ExpressionMonitor, Monitor from giskardpy.data_types import Derivatives, PrefixName, TaskState -from giskardpy.qp.constraint import EqualityConstraint, InequalityConstraint, DerivativeInequalityConstraint, Constraint +from giskardpy.qp.constraint import EqualityConstraint, InequalityConstraint, DerivativeInequalityConstraint from giskardpy.symbol_manager import symbol_manager -from giskardpy.utils.decorators import memoize +from giskardpy.motion_graph.graph_node import MotionGraphNode from giskardpy.utils.utils import string_shortener from giskardpy.qp.weight_gain import QuadraticWeightGain, LinearWeightGain from giskardpy.qp.free_variable import FreeVariable @@ -23,38 +19,29 @@ WEIGHT_MIN = giskard_msgs.Weights.WEIGHT_MIN -class Task: +class Task(MotionGraphNode): """ Tasks are a set of constraints with the same predicates. """ eq_constraints: Dict[PrefixName, EqualityConstraint] neq_constraints: Dict[PrefixName, InequalityConstraint] derivative_constraints: Dict[PrefixName, DerivativeInequalityConstraint] - _start_condition: cas.Expression - _hold_condition: cas.Expression - _end_condition: cas.Expression - _name: str _parent_goal_name: str - _id: int - plot: bool def __init__(self, parent_goal_name: str, name: Optional[str] = None): - self.plot = True if name is None: self._name = str(self.__class__.__name__) else: self._name = name + super().__init__(name=name, + start_condition=cas.TrueSymbol, hold_condition=cas.FalseSymbol, end_condition=cas.FalseSymbol) self._parent_goal_name = parent_goal_name self.eq_constraints = {} self.neq_constraints = {} self.derivative_constraints = {} - self._start_condition = cas.TrueSymbol - self._hold_condition = cas.FalseSymbol - self._end_condition = cas.FalseSymbol self.manip_constraints = {} self.quadratic_gains = [] self.linear_weight_gains = [] - self._id = -1 def to_ros_msg(self) -> giskard_msgs.MotionGoal: msg = giskard_msgs.MotionGoal() @@ -98,14 +85,6 @@ def end_condition(self, value: cas.Expression) -> None: raise GiskardException(f'No monitor found for this state expr: "{monitor_state_expr}".') self._end_condition = value - @property - def id(self) -> int: - assert self._id >= 0, f'id of {self.name} is not set.' - return self._id - - def set_id(self, new_id: int) -> None: - self._id = new_id - @property def name(self) -> PrefixName: return PrefixName(self._name, self._parent_goal_name) diff --git a/src/giskardpy/python_interface/old_python_interface.py b/src/giskardpy/python_interface/old_python_interface.py index 78e564bd60..c2ecf622f8 100644 --- a/src/giskardpy/python_interface/old_python_interface.py +++ b/src/giskardpy/python_interface/old_python_interface.py @@ -6,7 +6,7 @@ from giskard_msgs.srv import DyeGroupResponse, GetGroupInfoResponse from giskardpy.data_types import goal_parameter from giskardpy.python_interface.python_interface import GiskardWrapper -from giskardpy.tasks.task import WEIGHT_ABOVE_CA, WEIGHT_BELOW_CA +from giskardpy.motion_graph.tasks.task import WEIGHT_ABOVE_CA, WEIGHT_BELOW_CA class OldGiskardWrapper(GiskardWrapper): diff --git a/src/giskardpy/python_interface/python_interface.py b/src/giskardpy/python_interface/python_interface.py index 1d9d89941c..c57bd87533 100644 --- a/src/giskardpy/python_interface/python_interface.py +++ b/src/giskardpy/python_interface/python_interface.py @@ -27,12 +27,12 @@ from giskardpy.goals.pre_push_door import PrePushDoor from giskardpy.goals.set_prediction_horizon import SetPredictionHorizon from giskardpy.model.utils import make_world_body_box -from giskardpy.monitors.cartesian_monitors import PoseReached, PositionReached, OrientationReached, PointingAt, \ +from giskardpy.motion_graph.monitors.cartesian_monitors import PoseReached, PositionReached, OrientationReached, PointingAt, \ VectorsAligned, DistanceToLine -from giskardpy.monitors.joint_monitors import JointGoalReached -from giskardpy.monitors.monitors import LocalMinimumReached, TimeAbove, Alternator, CancelMotion, EndMotion -from giskardpy.monitors.payload_monitors import Print, Sleep, SetMaxTrajectoryLength, \ - UpdateParentLinkOfGroup, PayloadAlternator +from giskardpy.motion_graph.monitors.joint_monitors import JointGoalReached +from giskardpy.motion_graph.monitors.monitors import LocalMinimumReached, TimeAbove, Alternator, CancelMotion, EndMotion +from giskardpy.motion_graph.monitors.payload_monitors import Print, Sleep, SetMaxTrajectoryLength, \ + PayloadAlternator from giskardpy.utils.utils import kwargs_to_json, get_all_classes_in_package @@ -1126,7 +1126,8 @@ def get_monitors(self) -> List[Monitor]: def get_anded_monitor_names(self) -> str: non_cancel_monitors = [] for monitor in self._monitors: - if monitor.monitor_class not in get_all_classes_in_package('giskardpy.monitors', CancelMotion): + if monitor.monitor_class not in get_all_classes_in_package('giskardpy.motion_graph.monitors', + CancelMotion): non_cancel_monitors.append(f'\'{monitor.name}\'') return ' and '.join(non_cancel_monitors) diff --git a/src/giskardpy/tree/behaviors/check_monitor_state.py b/src/giskardpy/tree/behaviors/check_monitor_state.py index 1964ec2841..0590026513 100644 --- a/src/giskardpy/tree/behaviors/check_monitor_state.py +++ b/src/giskardpy/tree/behaviors/check_monitor_state.py @@ -1,9 +1,8 @@ -import numpy as np from py_trees import Status from giskardpy.data_types import TaskState from giskardpy.god_map import god_map -from giskardpy.monitors.monitors import Monitor +from giskardpy.motion_graph.monitors.monitors import Monitor from giskardpy.tree.behaviors.plugin import GiskardBehavior from giskardpy.utils.decorators import record_time, catch_and_raise_to_blackboard diff --git a/src/giskardpy/tree/behaviors/cleanup.py b/src/giskardpy/tree/behaviors/cleanup.py index 8a8f070050..df573d02ad 100644 --- a/src/giskardpy/tree/behaviors/cleanup.py +++ b/src/giskardpy/tree/behaviors/cleanup.py @@ -3,7 +3,7 @@ from visualization_msgs.msg import MarkerArray, Marker from giskardpy.debug_expression_manager import DebugExpressionManager -from giskardpy.monitors.monitor_manager import MonitorManager +from giskardpy.motion_graph.monitors.monitor_manager import MonitorManager from giskardpy.goals.motion_goal_manager import MotionGoalManager from giskardpy.god_map import god_map from giskardpy.model.collision_world_syncer import Collisions diff --git a/src/giskardpy/tree/behaviors/execute_payload_monitor.py b/src/giskardpy/tree/behaviors/execute_payload_monitor.py index 53b784d351..660810de69 100644 --- a/src/giskardpy/tree/behaviors/execute_payload_monitor.py +++ b/src/giskardpy/tree/behaviors/execute_payload_monitor.py @@ -2,7 +2,7 @@ from py_trees import Status -from giskardpy.monitors.payload_monitors import PayloadMonitor +from giskardpy.motion_graph.monitors.payload_monitors import PayloadMonitor from giskardpy.tree.behaviors.plugin import GiskardBehavior from giskardpy.utils.decorators import record_time, catch_and_raise_to_blackboard diff --git a/src/giskardpy/tree/behaviors/plot_goal_gantt_chart.py b/src/giskardpy/tree/behaviors/plot_goal_gantt_chart.py index 52b5fa0ee3..7b60240d19 100644 --- a/src/giskardpy/tree/behaviors/plot_goal_gantt_chart.py +++ b/src/giskardpy/tree/behaviors/plot_goal_gantt_chart.py @@ -6,9 +6,9 @@ from py_trees import Status from giskardpy.goals.collision_avoidance import CollisionAvoidance from giskardpy.goals.goal import Goal -from giskardpy.monitors.monitors import Monitor, EndMotion, CancelMotion +from giskardpy.motion_graph.monitors.monitors import Monitor, EndMotion, CancelMotion from giskardpy.god_map import god_map -from giskardpy.tasks.task import TaskState +from giskardpy.motion_graph.tasks.task import TaskState from giskardpy.tree.behaviors.plugin import GiskardBehavior from giskardpy.utils import logging from giskardpy.utils.decorators import record_time diff --git a/src/giskardpy/tree/behaviors/plot_task_graph.py b/src/giskardpy/tree/behaviors/plot_task_graph.py index 6aa798755e..d94c942234 100644 --- a/src/giskardpy/tree/behaviors/plot_task_graph.py +++ b/src/giskardpy/tree/behaviors/plot_task_graph.py @@ -8,7 +8,8 @@ from giskard_msgs.msg import ExecutionState from giskardpy.data_types import TaskState from giskardpy.god_map import god_map -from giskardpy.monitors.monitors import EndMotion, CancelMotion +from giskardpy.motion_graph.monitors.payload_monitors import CancelMotion +from giskardpy.motion_graph.monitors.monitor_manager import EndMotion from giskardpy.tree.behaviors.plugin import GiskardBehavior from giskardpy.tree.behaviors.publish_feedback import giskard_state_to_execution_state from giskardpy.utils import logging diff --git a/src/giskardpy/tree/behaviors/ros_msg_to_goal.py b/src/giskardpy/tree/behaviors/ros_msg_to_goal.py index 2fa1c651dc..8fbfbd2b18 100644 --- a/src/giskardpy/tree/behaviors/ros_msg_to_goal.py +++ b/src/giskardpy/tree/behaviors/ros_msg_to_goal.py @@ -1,10 +1,9 @@ import traceback from py_trees import Status -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, CancelMotion +from giskardpy.motion_graph.monitors.monitors import TimeAbove, LocalMinimumReached, EndMotion, CancelMotion from giskardpy.god_map import god_map from giskardpy.model.joints import OmniDrive, DiffDrive from giskardpy.tree.behaviors.plugin import GiskardBehavior diff --git a/src/giskardpy/tree/branches/check_monitors.py b/src/giskardpy/tree/branches/check_monitors.py index ccb5873eb0..39031474e8 100644 --- a/src/giskardpy/tree/branches/check_monitors.py +++ b/src/giskardpy/tree/branches/check_monitors.py @@ -1,5 +1,5 @@ -from giskardpy.monitors.monitors import EndMotion -from giskardpy.monitors.payload_monitors import PayloadMonitor +from giskardpy.motion_graph.monitors.monitors import EndMotion +from giskardpy.motion_graph.monitors.payload_monitors import PayloadMonitor from giskardpy.tree.branches.payload_monitor_sequence import PayloadMonitorSequence from giskardpy.tree.composites.running_selector import RunningSelector from giskardpy.tree.decorators import success_is_running diff --git a/src/giskardpy/tree/branches/payload_monitor_sequence.py b/src/giskardpy/tree/branches/payload_monitor_sequence.py index 51db3f3e8a..de00f0cede 100644 --- a/src/giskardpy/tree/branches/payload_monitor_sequence.py +++ b/src/giskardpy/tree/branches/payload_monitor_sequence.py @@ -1,5 +1,5 @@ from py_trees import Sequence -from giskardpy.monitors.payload_monitors import PayloadMonitor +from giskardpy.motion_graph.monitors.monitors import PayloadMonitor from giskardpy.tree.behaviors.check_monitor_state import CheckMonitorState from giskardpy.tree.behaviors.delete_monitors_behaviors import DeleteMonitor from giskardpy.tree.behaviors.execute_payload_monitor import ExecutePayloadMonitor diff --git a/src/giskardpy/utils/expression_definition_utils.py b/src/giskardpy/utils/expression_definition_utils.py index 50473ade2b..c0c706ffac 100644 --- a/src/giskardpy/utils/expression_definition_utils.py +++ b/src/giskardpy/utils/expression_definition_utils.py @@ -1,7 +1,6 @@ from __future__ import annotations -from typing import overload, Union, Optional, List, TYPE_CHECKING +from typing import overload, TYPE_CHECKING -import rospy from geometry_msgs.msg import PoseStamped, PointStamped, Vector3Stamped, QuaternionStamped from giskardpy.exceptions import UnknownGroupException @@ -11,7 +10,7 @@ import giskardpy.casadi_wrapper as cas if TYPE_CHECKING: - from giskardpy.monitors.monitors import ExpressionMonitor + pass @overload diff --git a/test/test_integration_pr2.py b/test/test_integration_pr2.py index dc7d6bc505..c490484745 100644 --- a/test/test_integration_pr2.py +++ b/test/test_integration_pr2.py @@ -12,7 +12,7 @@ from geometry_msgs.msg import PoseStamped, Point, Quaternion, Vector3Stamped, PointStamped, QuaternionStamped, Pose from numpy import pi from shape_msgs.msg import SolidPrimitive -from tf.transformations import quaternion_from_matrix, quaternion_about_axis, quaternion_matrix, rotation_matrix +from tf.transformations import quaternion_from_matrix, quaternion_about_axis import giskardpy.utils.tfwrapper as tf from giskard_msgs.msg import WorldBody, CollisionEntry, WorldGoal, GiskardError @@ -33,7 +33,7 @@ from giskardpy.model.utils import make_world_body_box, hacky_urdf_parser_fix from giskardpy.model.world import WorldTree from giskardpy.data_types import PrefixName -from giskardpy.tasks.task import WEIGHT_BELOW_CA, WEIGHT_ABOVE_CA, WEIGHT_COLLISION_AVOIDANCE +from giskardpy.motion_graph.tasks.task import WEIGHT_BELOW_CA, WEIGHT_ABOVE_CA, WEIGHT_COLLISION_AVOIDANCE from giskardpy.python_interface.old_python_interface import OldGiskardWrapper from giskardpy.utils.utils import launch_launchfile, suppress_stderr, resolve_ros_iris from giskardpy.utils.math import compare_points diff --git a/test/test_integration_pr2_mujoco_closed_loop.py b/test/test_integration_pr2_mujoco_closed_loop.py index c7aedb3c60..1a41c4bbb0 100644 --- a/test/test_integration_pr2_mujoco_closed_loop.py +++ b/test/test_integration_pr2_mujoco_closed_loop.py @@ -1,19 +1,17 @@ -import math from copy import deepcopy from typing import Optional import numpy as np from numpy import pi import pytest -import math import rospy from geometry_msgs.msg import PoseStamped, Point, Quaternion, Vector3Stamped, PointStamped from sensor_msgs.msg import JointState from std_srvs.srv import Trigger -from tf.transformations import quaternion_from_matrix, quaternion_about_axis, quaternion_multiply +from tf.transformations import quaternion_from_matrix, quaternion_about_axis import giskardpy.utils.tfwrapper as tf -from giskard_msgs.msg import MoveResult, MoveGoal, GiskardError +from giskard_msgs.msg import MoveGoal, GiskardError from giskardpy.configs.behavior_tree_config import ClosedLoopBTConfig from giskardpy.configs.giskard import Giskard from giskardpy.configs.iai_robots.pr2 import PR2CollisionAvoidance, PR2VelocityMujocoInterface, WorldWithPR2Config @@ -21,8 +19,8 @@ from giskardpy.data_types import JointStates from giskardpy.goals.base_traj_follower import CarryMyBullshit from giskardpy.god_map import god_map -from giskardpy.tasks.task import WEIGHT_BELOW_CA -from test_integration_pr2 import PR2TestWrapper, TestJointGoals, pocky_pose +from giskardpy.motion_graph.tasks.task import WEIGHT_BELOW_CA +from test_integration_pr2 import PR2TestWrapper, TestJointGoals from giskardpy.goals.weight_scaling_goals import MaxManipulabilityLinWeight diff --git a/test/test_integration_tiago_stand_alone.py b/test/test_integration_tiago_stand_alone.py index 37839837ff..098428a5d9 100644 --- a/test/test_integration_tiago_stand_alone.py +++ b/test/test_integration_tiago_stand_alone.py @@ -12,7 +12,7 @@ from giskardpy.configs.qp_controller_config import QPControllerConfig from giskardpy.configs.iai_robots.tiago import TiagoStandaloneInterface, TiagoCollisionAvoidanceConfig from giskardpy.configs.world_config import WorldWithDiffDriveRobot -from giskardpy.tasks.task import WEIGHT_ABOVE_CA, WEIGHT_BELOW_CA +from giskardpy.motion_graph.tasks.task import WEIGHT_ABOVE_CA, WEIGHT_BELOW_CA from giskardpy.god_map import god_map from giskardpy.data_types import PrefixName from giskardpy.utils.utils import launch_launchfile diff --git a/test/utils_for_tests.py b/test/utils_for_tests.py index 11bb4f65c4..43f8463e82 100644 --- a/test/utils_for_tests.py +++ b/test/utils_for_tests.py @@ -31,11 +31,11 @@ from giskardpy.model.collision_world_syncer import Collisions, Collision from giskardpy.model.joints import OneDofJoint, OmniDrive, DiffDrive from giskardpy.data_types import PrefixName, Derivatives -from giskardpy.monitors.payload_monitors import UpdateParentLinkOfGroup +from giskardpy.motion_graph.monitors.payload_monitors import UpdateParentLinkOfGroup from giskardpy.python_interface.old_python_interface import OldGiskardWrapper from giskardpy.qp.free_variable import FreeVariable from giskardpy.qp.qp_controller import available_solvers -from giskardpy.tasks.task import WEIGHT_ABOVE_CA +from giskardpy.motion_graph.tasks.task import WEIGHT_ABOVE_CA from giskardpy.utils import logging, utils from giskardpy.utils.math import compare_poses from giskardpy.utils.ros_timer import Timer From cffc80c86ccd71e1d5d61bbd89c74aa51c05da57 Mon Sep 17 00:00:00 2001 From: Simon Date: Thu, 6 Jun 2024 17:36:56 +0200 Subject: [PATCH 02/22] joint goal reached works again --- src/giskardpy/casadi_wrapper.py | 11 +- src/giskardpy/casadi_wrapper.pyi | 3 +- src/giskardpy/data_types.py | 2 +- src/giskardpy/goals/motion_goal_manager.py | 44 +------ src/giskardpy/motion_graph/graph_node.py | 66 ++++++++++- src/giskardpy/motion_graph/helpers.py | 48 ++++++++ .../monitors/cartesian_monitors.py | 60 +++++++--- .../motion_graph/monitors/joint_monitors.py | 10 +- .../motion_graph/monitors/monitor_manager.py | 105 ++++++----------- .../motion_graph/monitors/monitors.py | 111 ++++++++++-------- .../motion_graph/monitors/payload_monitors.py | 65 ++++++++-- src/giskardpy/motion_graph/tasks/task.py | 54 +-------- .../python_interface/python_interface.py | 32 +++-- .../tree/behaviors/plot_goal_gantt_chart.py | 4 +- .../tree/behaviors/publish_feedback.py | 4 +- .../tree/branches/payload_monitor_sequence.py | 5 +- 16 files changed, 356 insertions(+), 268 deletions(-) create mode 100644 src/giskardpy/motion_graph/helpers.py diff --git a/src/giskardpy/casadi_wrapper.py b/src/giskardpy/casadi_wrapper.py index 3490c520b1..eb3e794722 100644 --- a/src/giskardpy/casadi_wrapper.py +++ b/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), \ diff --git a/src/giskardpy/casadi_wrapper.pyi b/src/giskardpy/casadi_wrapper.pyi index 7ddc207531..987e5ecd39 100644 --- a/src/giskardpy/casadi_wrapper.pyi +++ b/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) diff --git a/src/giskardpy/data_types.py b/src/giskardpy/data_types.py index 1c3bc05676..c0c397ed4d 100644 --- a/src/giskardpy/data_types.py +++ b/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 diff --git a/src/giskardpy/goals/motion_goal_manager.py b/src/giskardpy/goals/motion_goal_manager.py index 6a96c27250..378f40e87a 100644 --- a/src/giskardpy/goals/motion_goal_manager.py +++ b/src/giskardpy/goals/motion_goal_manager.py @@ -12,6 +12,7 @@ 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.motion_graph.tasks.task import Task from giskardpy.utils import logging @@ -87,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: diff --git a/src/giskardpy/motion_graph/graph_node.py b/src/giskardpy/motion_graph/graph_node.py index 55fd8dd3fc..2ede856ac5 100644 --- a/src/giskardpy/motion_graph/graph_node.py +++ b/src/giskardpy/motion_graph/graph_node.py @@ -1,5 +1,9 @@ import giskardpy.casadi_wrapper as cas from giskardpy.data_types import PrefixName +from giskardpy.exceptions import GiskardException +from giskardpy.god_map import god_map +from giskardpy.symbol_manager import symbol_manager +from giskardpy.utils.utils import string_shortener class MotionGraphNode: @@ -10,18 +14,74 @@ class MotionGraphNode: _id: int plot: bool - def __init__(self, name: str, start_condition: cas.Expression, hold_condition: cas.Expression, end_condition: cas.Expression,): + def __init__(self, name: str, + start_condition: cas.Expression, + hold_condition: cas.Expression, + end_condition: cas.Expression, + plot: bool = True): self._start_condition = start_condition self._hold_condition = hold_condition self._end_condition = end_condition - self.plot = True + self.plot = plot self._id = -1 self._name = name + def formatted_name(self, quoted: bool = False) -> str: + formatted_name = string_shortener(original_str=str(self.name), + max_lines=4, + max_line_length=25) + result = (f'{formatted_name}\n' + f'----start_condition----\n' + f'{god_map.monitor_manager.format_condition(self.start_condition)}\n' + f'----hold_condition----\n' + f'{god_map.monitor_manager.format_condition(self.hold_condition)}\n' + f'----end_condition----\n' + f'{god_map.monitor_manager.format_condition(self.end_condition)}') + if quoted: + return '"' + result + '"' + return result + @property def id(self) -> int: assert self._id >= 0, f'id of {self._name} is not set.' return self._id - def set_id(self, new_id: int) -> None: + @id.setter + def id(self, new_id: int) -> None: self._id = new_id + + def get_life_cycle_state_expression(self) -> cas.Symbol: + raise NotImplementedError('get_state_expression is not implemented') + + @property + def start_condition(self) -> cas.Expression: + return self._start_condition + + @start_condition.setter + def start_condition(self, value: cas.Expression) -> None: + for monitor_state_expr in value.free_symbols(): + if not god_map.monitor_manager.is_monitor_registered(monitor_state_expr): + raise GiskardException(f'No monitor found for this state expr: "{monitor_state_expr}".') + self._start_condition = value + + @property + def hold_condition(self) -> cas.Expression: + return self._hold_condition + + @hold_condition.setter + def hold_condition(self, value: cas.Expression) -> None: + for monitor_state_expr in value.free_symbols(): + if not god_map.monitor_manager.is_monitor_registered(monitor_state_expr): + raise GiskardException(f'No monitor found for this state expr: "{monitor_state_expr}".') + self._hold_condition = value + + @property + def end_condition(self) -> cas.Expression: + return self._end_condition + + @end_condition.setter + def end_condition(self, value: cas.Expression) -> None: + for monitor_state_expr in value.free_symbols(): + if not god_map.monitor_manager.is_monitor_registered(monitor_state_expr): + raise GiskardException(f'No monitor found for this state expr: "{monitor_state_expr}".') + self._end_condition = value diff --git a/src/giskardpy/motion_graph/helpers.py b/src/giskardpy/motion_graph/helpers.py new file mode 100644 index 0000000000..02f1197b27 --- /dev/null +++ b/src/giskardpy/motion_graph/helpers.py @@ -0,0 +1,48 @@ +from typing import Dict + +from giskardpy.casadi_wrapper import CompiledFunction +from giskardpy.data_types import PrefixName, TaskState +from giskardpy.god_map import god_map +from giskardpy.motion_graph.graph_node import MotionGraphNode +import giskardpy.casadi_wrapper as cas + + +@profile +def compile_graph_node_state_updater(graph_nodes: Dict[PrefixName, MotionGraphNode]) -> CompiledFunction: + symbols = [] + for node in sorted(graph_nodes.values(), key=lambda x: x.id): + symbols.append(node.get_life_cycle_state_expression()) + node_state = cas.Expression(symbols) + monitor_state = god_map.monitor_manager.get_monitor_state_expr() + + state_updater = [] + for node in sorted(graph_nodes.values(), key=lambda x: x.id): + state_symbol = node_state[node.id] + + if cas.is_true(node.start_condition): + start_if = TaskState.running # start right away + else: + start_if = cas.if_else(node.start_condition, + if_result=TaskState.running, + else_result=TaskState.not_started) + if cas.is_false(node.hold_condition): + hold_if = TaskState.running # never hold + else: + hold_if = cas.if_else(node.hold_condition, + if_result=TaskState.on_hold, + else_result=TaskState.running) + if cas.is_false(node.end_condition): + else_result = hold_if # never end + else: + else_result = cas.if_else(node.end_condition, + if_result=TaskState.succeeded, + else_result=hold_if) + + state_f = cas.if_eq_cases(a=state_symbol, + b_result_cases=[(TaskState.not_started, start_if), + (TaskState.succeeded, TaskState.succeeded)], + else_result=else_result) # running or on_hold + state_updater.append(state_f) + state_updater = cas.Expression(state_updater) + symbols = node_state.free_symbols() + monitor_state.free_symbols() + return state_updater.compile(symbols) \ No newline at end of file diff --git a/src/giskardpy/motion_graph/monitors/cartesian_monitors.py b/src/giskardpy/motion_graph/monitors/cartesian_monitors.py index 0e78bf3efb..7bd872a993 100644 --- a/src/giskardpy/motion_graph/monitors/cartesian_monitors.py +++ b/src/giskardpy/motion_graph/monitors/cartesian_monitors.py @@ -19,9 +19,13 @@ def __init__(self, orientation_threshold: float = 0.01, absolute: bool = False, name: Optional[str] = None, - stay_true: bool = True, - start_condition: cas.Expression = cas.TrueSymbol): - super().__init__(name=name, stay_true=stay_true, start_condition=start_condition) + start_condition: cas.Expression = cas.TrueSymbol, + hold_condition: cas.Expression = cas.FalseSymbol, + end_condition: cas.Expression = cas.FalseSymbol): + super().__init__(name=name, + start_condition=start_condition, + hold_condition=hold_condition, + end_condition=end_condition) root_link = god_map.world.search_for_link_name(root_link, root_group) tip_link = god_map.world.search_for_link_name(tip_link, tip_group) if absolute: @@ -54,9 +58,13 @@ def __init__(self, threshold: float = 0.01, absolute: bool = False, name: Optional[str] = None, - stay_true: bool = True, - start_condition: cas.Expression = cas.TrueSymbol): - super().__init__(name=name, stay_true=stay_true, start_condition=start_condition) + start_condition: cas.Expression = cas.TrueSymbol, + hold_condition: cas.Expression = cas.FalseSymbol, + end_condition: cas.Expression = cas.FalseSymbol): + super().__init__(name=name, + start_condition=start_condition, + hold_condition=hold_condition, + end_condition=end_condition) root_link = god_map.world.search_for_link_name(root_link, root_group) tip_link = god_map.world.search_for_link_name(tip_link, tip_group) @@ -80,9 +88,13 @@ def __init__(self, threshold: float = 0.01, absolute: bool = False, name: Optional[str] = None, - stay_true: bool = True, - start_condition: cas.Expression = cas.TrueSymbol): - super().__init__(name=name, stay_true=stay_true, start_condition=start_condition) + start_condition: cas.Expression = cas.TrueSymbol, + hold_condition: cas.Expression = cas.FalseSymbol, + end_condition: cas.Expression = cas.FalseSymbol): + super().__init__(name=name, + start_condition=start_condition, + hold_condition=hold_condition, + end_condition=end_condition) root_link = god_map.world.search_for_link_name(root_link, root_group) tip_link = god_map.world.search_for_link_name(tip_link, tip_group) @@ -106,9 +118,13 @@ def __init__(self, pointing_axis: Vector3Stamped = None, threshold: float = 0.01, name: Optional[str] = None, - stay_true: bool = True, - start_condition: cas.Expression = cas.TrueSymbol): - super().__init__(name=name, stay_true=stay_true, start_condition=start_condition) + start_condition: cas.Expression = cas.TrueSymbol, + hold_condition: cas.Expression = cas.FalseSymbol, + end_condition: cas.Expression = cas.FalseSymbol): + super().__init__(name=name, + start_condition=start_condition, + hold_condition=hold_condition, + end_condition=end_condition) self.root = god_map.world.search_for_link_name(root_link, root_group) self.tip = god_map.world.search_for_link_name(tip_link, tip_group) self.root_P_goal_point = transform_msg(self.root, goal_point) @@ -139,9 +155,13 @@ def __init__(self, tip_group: Optional[str] = None, threshold: float = 0.01, name: Optional[str] = None, - stay_true: bool = True, - start_condition: cas.Expression = cas.TrueSymbol): - super().__init__(name=name, stay_true=stay_true, start_condition=start_condition) + start_condition: cas.Expression = cas.TrueSymbol, + hold_condition: cas.Expression = cas.FalseSymbol, + end_condition: cas.Expression = cas.FalseSymbol): + super().__init__(name=name, + start_condition=start_condition, + hold_condition=hold_condition, + end_condition=end_condition) self.root = god_map.world.search_for_link_name(root_link, root_group) self.tip = god_map.world.search_for_link_name(tip_link, tip_group) @@ -171,9 +191,13 @@ def __init__(self, tip_group: Optional[str] = None, threshold: float = 0.01, name: Optional[str] = None, - stay_true: bool = True, - start_condition: cas.Expression = cas.TrueSymbol): - super().__init__(name=name, stay_true=stay_true, start_condition=start_condition) + start_condition: cas.Expression = cas.TrueSymbol, + hold_condition: cas.Expression = cas.FalseSymbol, + end_condition: cas.Expression = cas.FalseSymbol): + super().__init__(name=name, + start_condition=start_condition, + hold_condition=hold_condition, + end_condition=end_condition) self.root = god_map.world.search_for_link_name(root_link, root_group) self.tip = god_map.world.search_for_link_name(tip_link, tip_group) diff --git a/src/giskardpy/motion_graph/monitors/joint_monitors.py b/src/giskardpy/motion_graph/monitors/joint_monitors.py index afbdb121d6..021169c2b4 100644 --- a/src/giskardpy/motion_graph/monitors/joint_monitors.py +++ b/src/giskardpy/motion_graph/monitors/joint_monitors.py @@ -11,8 +11,9 @@ def __init__(self, goal_state: Dict[str, float], threshold: float, name: Optional[str] = None, - stay_true: bool = True, - start_condition: cas.Expression = cas.TrueSymbol): + start_condition: cas.Expression = cas.TrueSymbol, + hold_condition: cas.Expression = cas.FalseSymbol, + end_condition: cas.Expression = cas.FalseSymbol): comparison_list = [] for joint_name, goal in goal_state.items(): joint_name = god_map.world.search_for_joint_name(joint_name) @@ -23,5 +24,8 @@ def __init__(self, error = goal - current comparison_list.append(cas.less(cas.abs(error), threshold)) expression = cas.logic_all(cas.Expression(comparison_list)) - super().__init__(name=name, stay_true=stay_true, start_condition=start_condition) + super().__init__(name=name, + start_condition=start_condition, + hold_condition=hold_condition, + end_condition=end_condition) self.expression = expression diff --git a/src/giskardpy/motion_graph/monitors/monitor_manager.py b/src/giskardpy/motion_graph/monitors/monitor_manager.py index e2a7a12964..8a16ffafa1 100644 --- a/src/giskardpy/motion_graph/monitors/monitor_manager.py +++ b/src/giskardpy/motion_graph/monitors/monitor_manager.py @@ -9,9 +9,10 @@ import giskard_msgs.msg as giskard_msgs import giskardpy.casadi_wrapper as cas from giskardpy.casadi_wrapper import CompiledFunction -from giskardpy.data_types import TaskState +from giskardpy.data_types import TaskState, PrefixName from giskardpy.exceptions import GiskardException, MonitorInitalizationException, UnknownMonitorException from giskardpy.god_map import god_map +from giskardpy.motion_graph.helpers import compile_graph_node_state_updater from giskardpy.motion_graph.monitors.monitors import ExpressionMonitor, Monitor, EndMotion from giskardpy.motion_graph.monitors.payload_monitors import PayloadMonitor, CancelMotion from giskardpy.symbol_manager import symbol_manager @@ -19,16 +20,6 @@ from giskardpy.utils.utils import get_all_classes_in_package, json_str_to_kwargs -def flipped_to_one(a: np.ndarray, b: np.ndarray) -> np.ndarray: - """ - 0, 0 -> 0 - 0, 1 -> 1 - 1, 0 -> 0 - 1, 1 -> 0 - """ - return np.logical_and(np.logical_not(a), np.logical_or(a, b)) - - def monitor_list_to_monitor_name_tuple(monitors: Iterable[Union[str, ExpressionMonitor]]) -> Tuple[str, ...]: return tuple(sorted(monitor.name if isinstance(monitor, Monitor) else monitor for monitor in monitors)) @@ -40,7 +31,7 @@ class MonitorManager: compiled_life_cycle_state_updater: CompiledFunction state_history: List[Tuple[float, Tuple[np.ndarray, np.ndarray]]] # time -> (state, life_cycle_state) - monitors: List[Monitor] + monitors: Dict[PrefixName, Monitor] substitution_values: Dict[int, Dict[str, float]] # id -> (old_symbol, value) triggers: Dict[int, Callable] # id -> updater callback @@ -48,7 +39,7 @@ class MonitorManager: compiled_trigger_conditions: cas.CompiledFunction # stacked compiled function which returns array of evaluated conditions def __init__(self): - self.monitors = [] + self.monitors = OrderedDict() self.allowed_monitor_types = {} for path in god_map.giskard.monitor_package_paths: self.allowed_monitor_types.update(get_all_classes_in_package(path, Monitor)) @@ -103,20 +94,26 @@ def compile_monitors(self, traj_tracking: bool = False) -> None: @profile def get_monitor(self, name: str) -> Monitor: - for monitor in self.monitors: + for monitor in self.monitors.values(): if monitor.name == name: return monitor raise KeyError(f'No monitor of name \'{name}\' found.') + def get_monitor_state_expr(self) -> cas.Expression: + symbols = [] + for monitor in self.monitors.values(): + symbols.append(monitor.get_state_expression()) + return cas.Expression(symbols) + def set_initial_life_cycle_state(self): - for monitor in self.monitors: + for monitor in self.monitors.values(): if cas.is_true(monitor.start_condition): self.life_cycle_state[monitor.id] = TaskState.running else: self.life_cycle_state[monitor.id] = TaskState.not_started def get_monitor_from_state_expr(self, expr: cas.Expression) -> Monitor: - for monitor in self.monitors: + for monitor in self.monitors.values(): if cas.is_true(monitor.get_state_expression() == expr): return monitor raise GiskardException('No monitor found.') @@ -145,51 +142,24 @@ def format_condition(self, condition: cas.Expression, new_line: str = '\n') -> s return condition @profile - def compile_monitor_state_updater(self): - monitor_state = cas.Expression(self.get_state_expression_symbols()) - - symbols = [] - for i in range(len(god_map.monitor_manager.monitors)): - symbols.append(self.monitors[i].get_life_cycle_state_expression()) - monitor_life_cycle_state = cas.Expression(symbols) - + def compile_monitor_state_updater(self) -> None: state_updater = [] - life_cycle_state_updater = [] - for i, monitor in enumerate(self.monitors): - state_symbol = monitor_state[monitor.id] - life_cycle_state_symbol = monitor_life_cycle_state[monitor.id] - - if not cas.is_true(monitor.start_condition): - start_if = cas.if_else(monitor.start_condition, - if_result=int(TaskState.running), - else_result=life_cycle_state_symbol) - else: - start_if = life_cycle_state_symbol - - done_if = cas.if_else(condition=cas.logic_and(cas.Expression(monitor.stay_true), state_symbol), - if_result=int(TaskState.succeeded), - else_result=life_cycle_state_symbol) - - life_cycle_state_f = cas.if_eq_cases(a=state_symbol, - b_result_cases=[(int(TaskState.not_started), start_if), - (int(TaskState.running), done_if)], - else_result=life_cycle_state_symbol) - life_cycle_state_updater.append(life_cycle_state_f) + for monitor in self.monitors.values(): + state_symbol = monitor.get_state_expression() if isinstance(monitor, ExpressionMonitor): monitor.compile() - state_f = cas.if_eq(life_cycle_state_symbol, int(TaskState.running), + state_f = cas.if_eq(monitor.get_life_cycle_state_expression(), int(TaskState.running), if_result=monitor.expression, else_result=state_symbol) else: state_f = state_symbol # if payload monitor, copy last state state_updater.append(state_f) - symbols = monitor_state.free_symbols() + monitor_life_cycle_state.free_symbols() - life_cycle_state_updater = cas.Expression(life_cycle_state_updater) state_updater = cas.Expression(state_updater) self.compiled_state_updater = state_updater.compile(state_updater.free_symbols()) - self.compiled_life_cycle_state_updater = life_cycle_state_updater.compile(symbols) + + self.compiled_life_cycle_state_updater = compile_graph_node_state_updater(self.monitors) @property def expression_monitors(self) -> List[ExpressionMonitor]: @@ -197,25 +167,25 @@ def expression_monitors(self) -> List[ExpressionMonitor]: @property def payload_monitors(self) -> List[PayloadMonitor]: - return [x for x in self.monitors if isinstance(x, PayloadMonitor)] + return [x for x in self.monitors.values() if isinstance(x, PayloadMonitor)] @profile def add_expression_monitor(self, monitor: ExpressionMonitor): - if [x for x in self.monitors if x.name == monitor.name]: + if [x for x in self.monitors.values() if x.name == monitor.name]: raise MonitorInitalizationException(f'Monitor named {monitor.name} already exists.') - self.monitors.append(monitor) - monitor.set_id(len(self.monitors) - 1) + self.monitors[monitor.name] = monitor + monitor.id = len(self.monitors) - 1 @profile def add_payload_monitor(self, monitor: PayloadMonitor): - if [x for x in self.monitors if x.name == monitor.name]: + if [x for x in self.monitors.values() if x.name == monitor.name]: raise MonitorInitalizationException(f'Monitor named {monitor.name} already exists.') - self.monitors.append(monitor) - monitor.set_id(len(self.monitors) - 1) + self.monitors[monitor.name] = monitor + monitor.id = len(self.monitors) - 1 def get_state_dict(self) -> Dict[str, Tuple[str, bool]]: return OrderedDict((monitor.name, (str(TaskState(self.life_cycle_state[i])), bool(self.state[i]))) - for i, monitor in enumerate(self.monitors)) + for i, monitor in enumerate(self.monitors.values())) @profile def register_expression_updater(self, expression: cas.PreservedCasType, @@ -240,10 +210,10 @@ def register_expression_updater(self, expression: cas.PreservedCasType, @profile def to_state_filter(self, monitor_names: List[Union[str, Monitor]]) -> np.ndarray: monitor_names = monitor_list_to_monitor_name_tuple(monitor_names) - return np.array([monitor.id for monitor in self.monitors if monitor.name in monitor_names]) + return np.array([monitor.id for monitor in self.monitors.values() if monitor.name in monitor_names]) def get_state_expression_symbols(self) -> List[cas.Symbol]: - return [m.get_state_expression() for m in self.monitors] + return [m.get_state_expression() for m in self.monitors.values()] @profile def _register_expression_update_triggers(self): @@ -280,12 +250,9 @@ def trigger_update_triggers(self, state: np.ndarray): self.triggers[updater_id]() del self.triggers[updater_id] - def evaluate_trigger_conditions(self, state: np.ndarray) -> None: - pass - @cached_property def payload_monitor_filter(self): - return np.array([i for i, m in enumerate(self.monitors) if isinstance(m, PayloadMonitor)]) + return np.array([i for i, m in enumerate(self.monitors.values()) if isinstance(m, PayloadMonitor)]) @profile def evaluate_monitors(self): @@ -294,7 +261,7 @@ def evaluate_monitors(self): self.state = self.compiled_state_updater.fast_call(args) # %% update life cycle state - args = np.concatenate((self.state, self.life_cycle_state)) + args = np.concatenate((self.life_cycle_state, self.state)) self.life_cycle_state = self.compiled_life_cycle_state_updater.fast_call(args) if len(self.payload_monitor_filter) > 0: @@ -323,14 +290,20 @@ def parse_monitors(self, monitor_msgs: List[giskard_msgs.Monitor]): raise UnknownMonitorException(f'unknown monitor type: \'{monitor_msg.monitor_class}\'.') try: kwargs = json_str_to_kwargs(monitor_msg.kwargs) - start_condition = self.logic_str_to_expr(monitor_msg.start_condition, default=cas.TrueSymbol) + hold_condition = kwargs.pop('hold_condition') + end_condition = kwargs.pop('end_condition') monitor = C(name=monitor_msg.name, - start_condition=start_condition, **kwargs) if isinstance(monitor, ExpressionMonitor): self.add_expression_monitor(monitor) elif isinstance(monitor, PayloadMonitor): self.add_payload_monitor(monitor) + start_condition = self.logic_str_to_expr(monitor_msg.start_condition, default=cas.TrueSymbol) + hold_condition = self.logic_str_to_expr(hold_condition, default=cas.FalseSymbol) + end_condition = self.logic_str_to_expr(end_condition, default=cas.FalseSymbol) + monitor.start_condition = start_condition + monitor.hold_condition = hold_condition + monitor.end_condition = end_condition except Exception as e: traceback.print_exc() error_msg = f'Initialization of \'{C.__name__}\' monitor failed: \n {e} \n' diff --git a/src/giskardpy/motion_graph/monitors/monitors.py b/src/giskardpy/motion_graph/monitors/monitors.py index 0b4dcea9ae..5ba5f8f9f8 100644 --- a/src/giskardpy/motion_graph/monitors/monitors.py +++ b/src/giskardpy/motion_graph/monitors/monitors.py @@ -10,41 +10,34 @@ import giskard_msgs.msg as giskard_msgs import giskardpy.casadi_wrapper as cas from giskard_msgs.msg import GiskardError -from giskardpy.data_types import Derivatives +from giskardpy.data_types import Derivatives, PrefixName from giskardpy.exceptions import GiskardException, MonitorInitalizationException from giskardpy.god_map import god_map +from giskardpy.motion_graph.graph_node import MotionGraphNode from giskardpy.symbol_manager import symbol_manager from giskardpy.utils.utils import string_shortener -class Monitor: - id: int - name: str - start_condition: cas.Expression - plot: bool - stay_true: bool +class Monitor(MotionGraphNode): def __init__(self, *, name: Optional[str] = None, start_condition: cas.Expression = cas.TrueSymbol, - plot: bool = True, - stay_true: bool = False): + hold_condition: cas.Expression = cas.FalseSymbol, + end_condition: cas.Expression = cas.FalseSymbol, + plot: bool = True): """ Every class inheriting from this can be called via the ROS interface. :param name: name of the monitor :param start_condition: A logical casadi expression using monitor variables, "not", "and" and "or". This monitor will only get executed once this condition becomes True. :param plot: If true, this monitor will not be plotted in the gantt chart and task graph. - :param stay_true: If True, this monitor will stay True once it gets into that state. """ - self.name = name or self.__class__.__name__ + self.name = PrefixName(name or self.__class__.__name__) + super().__init__(name=name, + start_condition=start_condition, hold_condition=hold_condition, end_condition=end_condition, + plot=plot) self.start_condition = start_condition - self._id = -1 - self.plot = plot - self.stay_true = stay_true - - def set_id(self, new_id: int): - self._id = new_id def to_ros_msg(self) -> giskard_msgs.Monitor: msg = giskard_msgs.Monitor() @@ -58,12 +51,6 @@ def to_ros_msg(self) -> giskard_msgs.Monitor: msg.start_condition = god_map.monitor_manager.format_condition(self.start_condition, new_line=' ') return msg - @property - def id(self) -> int: - if self._id == -1: - raise MonitorInitalizationException(f'Id of {self.name} is not set.') - return self._id - @cached_property def state_filter(self) -> np.ndarray: return god_map.monitor_manager.to_state_filter(self.start_condition) @@ -74,19 +61,8 @@ def get_state_expression(self): def get_life_cycle_state_expression(self): return symbol_manager.get_symbol(f'god_map.monitor_manager.life_cycle_state[{self.id}]') - def formatted_name(self, quoted: bool = False) -> str: - formatted_name = string_shortener(original_str=self.name, - max_lines=4, - max_line_length=25) - result = (f'{formatted_name}\n' - f'----start_condition----\n' - f'{god_map.monitor_manager.format_condition(self.start_condition)}') - if quoted: - return '"' + result + '"' - return result - def __repr__(self) -> str: - return self.name + return str(self.name) class PayloadMonitor(Monitor, ABC): @@ -96,8 +72,9 @@ class PayloadMonitor(Monitor, ABC): def __init__(self, *, run_call_in_thread: bool, name: Optional[str] = None, - stay_true: bool = True, - start_condition: cas.Expression = cas.TrueSymbol): + start_condition: cas.Expression = cas.TrueSymbol, + hold_condition: cas.Expression = cas.FalseSymbol, + end_condition: cas.Expression = cas.FalseSymbol): """ A monitor which executes its __call__ function when start_condition becomes True. Subclass this and implement __init__ and __call__. The __call__ method should change self.state to True when @@ -106,7 +83,10 @@ def __init__(self, *, """ self.state = False self.run_call_in_thread = run_call_in_thread - super().__init__(name=name, start_condition=start_condition, stay_true=stay_true) + super().__init__(name=name, + start_condition=start_condition, + hold_condition=hold_condition, + end_condition=end_condition) def get_state(self) -> bool: return self.state @@ -119,8 +99,14 @@ def __call__(self): class EndMotion(PayloadMonitor): def __init__(self, name: Optional[str] = None, - start_condition: cas.Expression = cas.TrueSymbol, ): - super().__init__(name=name, start_condition=start_condition, run_call_in_thread=False) + start_condition: cas.Expression = cas.TrueSymbol, + hold_condition: cas.Expression = cas.FalseSymbol, + end_condition: cas.Expression = cas.FalseSymbol): + super().__init__(name=name, + start_condition=start_condition, + hold_condition=hold_condition, + end_condition=end_condition, + run_call_in_thread=False) def __call__(self): self.state = True @@ -134,8 +120,14 @@ def __init__(self, error_message: str, error_code: int = GiskardError.ERROR, name: Optional[str] = None, - start_condition: cas.Expression = cas.TrueSymbol, ): - super().__init__(name=name, start_condition=start_condition, run_call_in_thread=False) + start_condition: cas.Expression = cas.TrueSymbol, + hold_condition: cas.Expression = cas.FalseSymbol, + end_condition: cas.Expression = cas.FalseSymbol): + super().__init__(name=name, + start_condition=start_condition, + hold_condition=hold_condition, + end_condition=end_condition, + run_call_in_thread=False) self.error_message = error_message self.error_code = error_code @@ -153,8 +145,9 @@ class ExpressionMonitor(Monitor): def __init__(self, *, name: Optional[str] = None, - stay_true: bool = False, start_condition: cas.Expression = cas.TrueSymbol, + hold_condition: cas.Expression = cas.TrueSymbol, + end_condition: cas.Expression = cas.TrueSymbol, plot: bool = True): """ A Monitor whose state is determined by its expression. @@ -163,7 +156,11 @@ def __init__(self, *, self.substitution_values = [] self.substitution_keys = [] self._expression = None - super().__init__(name=name, start_condition=start_condition, plot=plot, stay_true=stay_true) + super().__init__(name=name, + start_condition=start_condition, + hold_condition=hold_condition, + end_condition=end_condition, + plot=plot) @property def expression(self) -> cas.Expression: @@ -189,8 +186,12 @@ def __init__(self, joint_convergence_threshold: float = 0.01, windows_size: int = 1, start_condition: cas.Expression = cas.TrueSymbol, - stay_true: bool = True): - super().__init__(name=name, stay_true=stay_true, start_condition=start_condition) + hold_condition: cas.Expression = cas.FalseSymbol, + end_condition: cas.Expression = cas.FalseSymbol): + super().__init__(name=name, + start_condition=start_condition, + hold_condition=hold_condition, + end_condition=end_condition) self.joint_convergence_threshold = joint_convergence_threshold self.min_cut_off = min_cut_off self.max_cut_off = max_cut_off @@ -220,10 +221,13 @@ class TimeAbove(ExpressionMonitor): def __init__(self, threshold: Optional[float] = None, name: Optional[str] = None, - start_condition: cas.Expression = cas.TrueSymbol): + start_condition: cas.Expression = cas.TrueSymbol, + hold_condition: cas.Expression = cas.FalseSymbol, + end_condition: cas.Expression = cas.FalseSymbol): super().__init__(name=name, - stay_true=False, - start_condition=start_condition) + start_condition=start_condition, + hold_condition=hold_condition, + end_condition=end_condition) if threshold is None: threshold = god_map.qp_controller_config.max_trajectory_length traj_length_in_sec = symbol_manager.time @@ -235,11 +239,16 @@ class Alternator(ExpressionMonitor): def __init__(self, name: Optional[str] = None, - stay_true: bool = False, start_condition: cas.Expression = cas.TrueSymbol, + hold_condition: cas.Expression = cas.FalseSymbol, + end_condition: cas.Expression = cas.FalseSymbol, mod: int = 2, plot: bool = True): - super().__init__(name=name, stay_true=stay_true, start_condition=start_condition, plot=plot) + super().__init__(name=name, + start_condition=start_condition, + hold_condition=hold_condition, + end_condition=end_condition, + plot=plot) time = symbol_manager.time expr = cas.equal(cas.fmod(cas.floor(time), mod), 0) self.expression = expr diff --git a/src/giskardpy/motion_graph/monitors/payload_monitors.py b/src/giskardpy/motion_graph/monitors/payload_monitors.py index ac03ea3ae2..cc418351c1 100644 --- a/src/giskardpy/motion_graph/monitors/payload_monitors.py +++ b/src/giskardpy/motion_graph/monitors/payload_monitors.py @@ -18,8 +18,14 @@ class WorldUpdatePayloadMonitor(PayloadMonitor): def __init__(self, *, name: Optional[str] = None, - start_condition: cas.Expression = cas.TrueSymbol): - super().__init__(name=name, start_condition=start_condition, run_call_in_thread=True) + start_condition: cas.Expression = cas.TrueSymbol, + hold_condition: cas.Expression = cas.FalseSymbol, + end_condition: cas.Expression = cas.FalseSymbol): + super().__init__(name=name, + start_condition=start_condition, + hold_condition=hold_condition, + end_condition=end_condition, + run_call_in_thread=True) @abc.abstractmethod def apply_world_update(self): @@ -37,7 +43,9 @@ class SetMaxTrajectoryLength(CancelMotion): def __init__(self, new_length: Optional[float] = None, name: Optional[str] = None, - start_condition: cas.Expression = cas.TrueSymbol, ): + start_condition: cas.Expression = cas.TrueSymbol, + hold_condition: cas.Expression = cas.FalseSymbol, + end_condition: cas.Expression = cas.FalseSymbol): if not (start_condition == cas.TrueSymbol).evaluate(): raise MonitorInitalizationException(f'Cannot set start_condition for {SetMaxTrajectoryLength.__name__}') if new_length is None: @@ -47,6 +55,8 @@ def __init__(self, error_message = f'Trajectory longer than {self.new_length}' super().__init__(name=name, start_condition=start_condition, + hold_condition=hold_condition, + end_condition=end_condition, error_message=error_message, error_code=GiskardError.MAX_TRAJECTORY_LENGTH) @@ -60,9 +70,15 @@ class Print(PayloadMonitor): def __init__(self, message: str, name: Optional[str] = None, - start_condition: cas.Expression = cas.TrueSymbol): + start_condition: cas.Expression = cas.TrueSymbol, + hold_condition: cas.Expression = cas.FalseSymbol, + end_condition: cas.Expression = cas.FalseSymbol): self.message = message - super().__init__(name=name, start_condition=start_condition, run_call_in_thread=False) + super().__init__(name=name, + start_condition=start_condition, + hold_condition=hold_condition, + end_condition=end_condition, + run_call_in_thread=False) def __call__(self): logging.loginfo(self.message) @@ -73,9 +89,15 @@ class Sleep(PayloadMonitor): def __init__(self, seconds: float, name: Optional[str] = None, - start_condition: cas.Expression = cas.TrueSymbol): + start_condition: cas.Expression = cas.TrueSymbol, + hold_condition: cas.Expression = cas.FalseSymbol, + end_condition: cas.Expression = cas.FalseSymbol): self.seconds = seconds - super().__init__(name=name, start_condition=start_condition, run_call_in_thread=True) + super().__init__(name=name, + start_condition=start_condition, + hold_condition=hold_condition, + end_condition=end_condition, + run_call_in_thread=True) def __call__(self): rospy.sleep(self.seconds) @@ -88,12 +110,17 @@ def __init__(self, parent_link: str, parent_link_group: Optional[str] = '', name: Optional[str] = None, - start_condition: cas.Expression = cas.TrueSymbol): + start_condition: cas.Expression = cas.TrueSymbol, + hold_condition: cas.Expression = cas.FalseSymbol, + end_condition: cas.Expression = cas.FalseSymbol): if not god_map.is_standalone(): raise MonitorInitalizationException(f'This monitor can only be used in standalone mode.') self.group_name = group_name self.new_parent_link = god_map.world.search_for_link_name(parent_link, parent_link_group) - super().__init__(name=name, start_condition=start_condition) + super().__init__(name=name, + start_condition=start_condition, + hold_condition=hold_condition, + end_condition=end_condition) def apply_world_update(self): god_map.world.move_group(group_name=self.group_name, @@ -107,8 +134,14 @@ class CollisionMatrixUpdater(PayloadMonitor): def __init__(self, new_collision_matrix: Dict[Tuple[str, str], float], name: Optional[str] = None, - start_condition: cas.Expression = cas.TrueSymbol): - super().__init__(name=name, start_condition=start_condition, run_call_in_thread=False) + start_condition: cas.Expression = cas.TrueSymbol, + hold_condition: cas.Expression = cas.FalseSymbol, + end_condition: cas.Expression = cas.FalseSymbol): + super().__init__(name=name, + start_condition=start_condition, + hold_condition=hold_condition, + end_condition=end_condition, + run_call_in_thread=False) self.collision_matrix = new_collision_matrix @profile @@ -123,8 +156,14 @@ class PayloadAlternator(PayloadMonitor): def __init__(self, mod: int = 2, name: Optional[str] = None, - start_condition: cas.Expression = cas.TrueSymbol): - super().__init__(name=name, stay_true=False, start_condition=start_condition, run_call_in_thread=False) + start_condition: cas.Expression = cas.TrueSymbol, + hold_condition: cas.Expression = cas.FalseSymbol, + end_condition: cas.Expression = cas.FalseSymbol): + super().__init__(name=name, + start_condition=start_condition, + hold_condition=hold_condition, + end_condition=end_condition, + run_call_in_thread=False) self.mod = mod def __call__(self): diff --git a/src/giskardpy/motion_graph/tasks/task.py b/src/giskardpy/motion_graph/tasks/task.py index 184a68d7ea..8abcf397f8 100644 --- a/src/giskardpy/motion_graph/tasks/task.py +++ b/src/giskardpy/motion_graph/tasks/task.py @@ -52,39 +52,6 @@ def to_ros_msg(self) -> giskard_msgs.MotionGoal: msg.end_condition = god_map.monitor_manager.format_condition(self.end_condition, new_line=' ') return msg - @property - def start_condition(self) -> cas.Expression: - return self._start_condition - - @start_condition.setter - def start_condition(self, value: cas.Expression) -> None: - for monitor_state_expr in value.free_symbols(): - if not god_map.monitor_manager.is_monitor_registered(monitor_state_expr): - raise GiskardException(f'No monitor found for this state expr: "{monitor_state_expr}".') - self._start_condition = value - - @property - def hold_condition(self) -> cas.Expression: - return self._hold_condition - - @hold_condition.setter - def hold_condition(self, value: cas.Expression) -> None: - for monitor_state_expr in value.free_symbols(): - if not god_map.monitor_manager.is_monitor_registered(monitor_state_expr): - raise GiskardException(f'No monitor found for this state expr: "{monitor_state_expr}".') - self._hold_condition = value - - @property - def end_condition(self) -> cas.Expression: - return self._end_condition - - @end_condition.setter - def end_condition(self, value: cas.Expression) -> None: - for monitor_state_expr in value.free_symbols(): - if not god_map.monitor_manager.is_monitor_registered(monitor_state_expr): - raise GiskardException(f'No monitor found for this state expr: "{monitor_state_expr}".') - self._end_condition = value - @property def name(self) -> PrefixName: return PrefixName(self._name, self._parent_goal_name) @@ -92,20 +59,8 @@ def name(self) -> PrefixName: def __str__(self): return self.name - def formatted_name(self, quoted: bool = False) -> str: - formatted_name = string_shortener(original_str=str(self.name), - max_lines=4, - max_line_length=25) - result = (f'{formatted_name}\n' - f'----start_condition----\n' - f'{god_map.monitor_manager.format_condition(self.start_condition)}\n' - f'----hold_condition----\n' - f'{god_map.monitor_manager.format_condition(self.hold_condition)}\n' - f'----end_condition----\n' - f'{god_map.monitor_manager.format_condition(self.end_condition)}') - if quoted: - return '"' + result + '"' - return result + def get_life_cycle_state_expression(self) -> cas.Symbol: + return symbol_manager.get_symbol(f'god_map.motion_goal_manager.task_state[{self.id}]') def get_eq_constraints(self) -> List[EqualityConstraint]: return self._apply_monitors_to_constraints(self.eq_constraints.values()) @@ -122,9 +77,6 @@ def get_quadratic_gains(self) -> List[QuadraticWeightGain]: def get_linear_gains(self) -> List[LinearWeightGain]: return self.linear_weight_gains - def get_state_expression(self) -> cas.Symbol: - return symbol_manager.get_symbol(f'god_map.motion_goal_manager.task_state[{self.id}]') - @overload def _apply_monitors_to_constraints(self, constraints: Iterable[EqualityConstraint]) \ -> List[Union[EqualityConstraint]]: @@ -143,7 +95,7 @@ def _apply_monitors_to_constraints(self, constraints: Iterable[DerivativeInequal def _apply_monitors_to_constraints(self, constraints): output_constraints = [] for constraint in constraints: - is_running = cas.if_eq(self.get_state_expression(), int(TaskState.running), + is_running = cas.if_eq(self.get_life_cycle_state_expression(), int(TaskState.running), if_result=1, else_result=0) constraint.quadratic_weight *= is_running diff --git a/src/giskardpy/python_interface/python_interface.py b/src/giskardpy/python_interface/python_interface.py index c57bd87533..d26f7c7a06 100644 --- a/src/giskardpy/python_interface/python_interface.py +++ b/src/giskardpy/python_interface/python_interface.py @@ -1138,6 +1138,8 @@ def add_monitor(self, *, monitor_class: str, name: Optional[str] = None, start_condition: str = '', + hold_condition: str = '', + end_condition: Optional[str] = None, **kwargs) -> str: """ Generic function to add a monitor. @@ -1153,27 +1155,35 @@ def add_monitor(self, *, name = f'M{str(len(self._monitors))} {name}' if [x for x in self._monitors if x.name == name]: raise KeyError(f'monitor named {name} already exists.') + monitor = giskard_msgs.Monitor() monitor.name = name + if not name.startswith('\'') and not name.startswith('"'): + name = f'\'{name}\'' # put all monitor names in quotes so that the user doesn't have to + + if end_condition is None: + end_condition = name monitor.monitor_class = monitor_class monitor.start_condition = start_condition + kwargs['hold_condition'] = hold_condition + kwargs['end_condition'] = end_condition monitor.kwargs = kwargs_to_json(kwargs) self._monitors.append(monitor) - if not name.startswith('\'') and not name.startswith('"'): - name = f'\'{name}\'' # put all monitor names in quotes so that the user doesn't have to return name def add_local_minimum_reached(self, name: Optional[str] = None, - stay_true: bool = True, - start_condition: str = ''): + start_condition: str = '', + hold_condition: str = '', + end_condition: Optional[str] = None): """ True if the world is currently in a local minimum. """ return self.add_monitor(monitor_class=LocalMinimumReached.__name__, name=name, start_condition=start_condition, - stay_true=stay_true) + hold_condition=hold_condition, + end_condition=end_condition) def add_time_above(self, threshold: float, @@ -1192,7 +1202,8 @@ def add_joint_position(self, threshold: float = 0.01, name: Optional[str] = None, start_condition: str = '', - stay_true: bool = True) -> str: + hold_condition: str = '', + end_condition: Optional[str] = None) -> str: """ True if all joints in goal_state are closer than threshold to their respective value. """ @@ -1201,7 +1212,8 @@ def add_joint_position(self, goal_state=goal_state, threshold=threshold, start_condition=start_condition, - stay_true=stay_true) + hold_condition=hold_condition, + end_condition=end_condition) def add_cartesian_pose(self, root_link: str, @@ -1368,7 +1380,9 @@ def add_end_motion(self, """ return self.add_monitor(monitor_class=EndMotion.__name__, name=name, - start_condition=start_condition) + start_condition=start_condition, + hold_condition='', + end_condition='') def add_cancel_motion(self, start_condition: str, @@ -1382,6 +1396,8 @@ def add_cancel_motion(self, return self.add_monitor(monitor_class=CancelMotion.__name__, name=name, start_condition=start_condition, + hold_condition='', + end_condition='', error_message=error_message, error_code=error_code) diff --git a/src/giskardpy/tree/behaviors/plot_goal_gantt_chart.py b/src/giskardpy/tree/behaviors/plot_goal_gantt_chart.py index 7b60240d19..80bd66a9a4 100644 --- a/src/giskardpy/tree/behaviors/plot_goal_gantt_chart.py +++ b/src/giskardpy/tree/behaviors/plot_goal_gantt_chart.py @@ -22,7 +22,7 @@ def __init__(self, name: str = 'plot gantt chart'): super().__init__(name) def plot_gantt_chart(self, goals: List[Goal], monitors: List[Monitor], file_name: str): - monitor_plot_filter = np.array([monitor.plot for monitor in god_map.monitor_manager.monitors]) + monitor_plot_filter = np.array([monitor.plot for monitor in god_map.monitor_manager.monitors.values()]) tasks = [task for g in goals for task in g.tasks] task_plot_filter = np.array([not isinstance(g, CollisionAvoidance) for g in goals for _ in g.tasks]) @@ -103,7 +103,7 @@ def update(self): return Status.SUCCESS try: goals = list(god_map.motion_goal_manager.motion_goals.values()) - monitors = god_map.monitor_manager.monitors + monitors = list(god_map.monitor_manager.monitors.values()) file_name = god_map.giskard.tmp_folder + f'gantt_charts/goal_{god_map.move_action_server.goal_id}.pdf' self.plot_gantt_chart(goals, monitors, file_name) except Exception as e: diff --git a/src/giskardpy/tree/behaviors/publish_feedback.py b/src/giskardpy/tree/behaviors/publish_feedback.py index c56a315b6d..a1aafc96c5 100644 --- a/src/giskardpy/tree/behaviors/publish_feedback.py +++ b/src/giskardpy/tree/behaviors/publish_feedback.py @@ -14,12 +14,12 @@ def giskard_state_to_execution_state() -> ExecutionState: - monitor_filter = np.array([monitor.plot for monitor in god_map.monitor_manager.monitors]) + monitor_filter = np.array([monitor.plot for monitor in god_map.monitor_manager.monitors.values()]) task_filter = np.array([task.plot for task in god_map.motion_goal_manager.tasks.values()]) msg = ExecutionState() msg.header.stamp = rospy.Time.now() msg.goal_id = god_map.move_action_server.goal_id - msg.monitors = [m.to_ros_msg() for m in god_map.monitor_manager.monitors if m.plot] + msg.monitors = [m.to_ros_msg() for m in god_map.monitor_manager.monitors.values() if m.plot] msg.tasks = [t.to_ros_msg() for t in god_map.motion_goal_manager.tasks.values() if t.plot] try: msg.monitor_state = god_map.monitor_manager.state_history[-1][1][0][monitor_filter].tolist() diff --git a/src/giskardpy/tree/branches/payload_monitor_sequence.py b/src/giskardpy/tree/branches/payload_monitor_sequence.py index de00f0cede..56bb60d997 100644 --- a/src/giskardpy/tree/branches/payload_monitor_sequence.py +++ b/src/giskardpy/tree/branches/payload_monitor_sequence.py @@ -3,15 +3,16 @@ from giskardpy.tree.behaviors.check_monitor_state import CheckMonitorState from giskardpy.tree.behaviors.delete_monitors_behaviors import DeleteMonitor from giskardpy.tree.behaviors.execute_payload_monitor import ExecutePayloadMonitor +import giskardpy.casadi_wrapper as cas class PayloadMonitorSequence(Sequence): monitor: PayloadMonitor def __init__(self, monitor: PayloadMonitor): - super().__init__(monitor.name) + super().__init__(str(monitor.name)) self.monitor = monitor - if self.monitor.stay_true: + if not cas.is_false(self.monitor.end_condition): self.add_child(DeleteMonitor(name=f'delete\nparent?', parent=self)) if self.monitor.start_condition: self.add_child(CheckMonitorState(monitor=self.monitor)) From 30d996ac32951779db4ae63b5bf5d221c64f647b Mon Sep 17 00:00:00 2001 From: Simon Date: Thu, 6 Jun 2024 17:51:06 +0200 Subject: [PATCH 03/22] replaced stay true parameter with hold and end conditions for monitors in python interface --- scripts/examples/python_interface_example.py | 3 +- src/giskardpy/goals/cartesian_goals.py | 8 +-- src/giskardpy/goals/tracebot.py | 6 +- .../python_interface/python_interface.py | 61 ++++++++++++++----- test/test_integration_pr2.py | 4 +- 5 files changed, 58 insertions(+), 24 deletions(-) diff --git a/scripts/examples/python_interface_example.py b/scripts/examples/python_interface_example.py index 9b2644d143..0200ce941e 100644 --- a/scripts/examples/python_interface_example.py +++ b/scripts/examples/python_interface_example.py @@ -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 diff --git a/src/giskardpy/goals/cartesian_goals.py b/src/giskardpy/goals/cartesian_goals.py index 566de17b65..84ce9618ff 100644 --- a/src/giskardpy/goals/cartesian_goals.py +++ b/src/giskardpy/goals/cartesian_goals.py @@ -591,13 +591,13 @@ 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') + error1_monitor.end_condition = error1_monitor.get_state_expression() self.add_monitor(error1_monitor) error1_monitor.expression = cas.less(cas.abs(error1), 0.01) - error2_monitor = ExpressionMonitor(name='p2', - stay_true=True) + error2_monitor = ExpressionMonitor(name='p2') + error2_monitor.end_condition = error2_monitor.get_state_expression() self.add_monitor(error2_monitor) root_P_goal2_cached = transform_msg_and_turn_to_expr(self.root_link, diff --git a/src/giskardpy/goals/tracebot.py b/src/giskardpy/goals/tracebot.py index 575feddef9..dad4634a42 100644 --- a/src/giskardpy/goals/tracebot.py +++ b/src/giskardpy/goals/tracebot.py @@ -61,15 +61,17 @@ def __init__(self, root_P_top = root_P_hole + root_V_up * self.pre_grasp_height distance_to_top = cas.euclidean_distance(root_P_tip, root_P_top) top_reached = cas.less(distance_to_top, 0.01) - top_reached_monitor = ExpressionMonitor(name='top reached', stay_true=True, start_condition=start_condition) + top_reached_monitor = ExpressionMonitor(name='top reached', start_condition=start_condition) + top_reached_monitor.end_condition = top_reached_monitor.get_state_expression() self.add_monitor(top_reached_monitor) top_reached_monitor.expression = top_reached distance_to_line, root_P_on_line = cas.distance_point_to_line_segment(root_P_tip, root_P_hole, root_P_top) distance_to_hole = cas.norm(root_P_hole - root_P_tip) bottom_reached = cas.less(distance_to_hole, 0.01) - bottom_reached_monitor = ExpressionMonitor(name='bottom reached', stay_true=True, + bottom_reached_monitor = ExpressionMonitor(name='bottom reached', start_condition=start_condition) + bottom_reached_monitor.end_condition = bottom_reached_monitor.get_state_expression() bottom_reached_monitor.expression = bottom_reached self.add_monitor(bottom_reached_monitor) diff --git a/src/giskardpy/python_interface/python_interface.py b/src/giskardpy/python_interface/python_interface.py index d26f7c7a06..582bfd501f 100644 --- a/src/giskardpy/python_interface/python_interface.py +++ b/src/giskardpy/python_interface/python_interface.py @@ -27,7 +27,8 @@ from giskardpy.goals.pre_push_door import PrePushDoor from giskardpy.goals.set_prediction_horizon import SetPredictionHorizon from giskardpy.model.utils import make_world_body_box -from giskardpy.motion_graph.monitors.cartesian_monitors import PoseReached, PositionReached, OrientationReached, PointingAt, \ +from giskardpy.motion_graph.monitors.cartesian_monitors import PoseReached, PositionReached, OrientationReached, \ + PointingAt, \ VectorsAligned, DistanceToLine from giskardpy.motion_graph.monitors.joint_monitors import JointGoalReached from giskardpy.motion_graph.monitors.monitors import LocalMinimumReached, TimeAbove, Alternator, CancelMotion, EndMotion @@ -1147,6 +1148,8 @@ def add_monitor(self, *, :param name: a unique name for the goal, will use class name by default :param start_condition: a logical expression to define the start condition for this monitor. e.g. not 'monitor1' and ('monitor2' or 'monitor3') + :param hold_condition: a logical expression to define the hold condition for this monitor. + :param end_condition: a logical expression to define the end condition for this monitor. :param kwargs: kwargs for __init__ function of motion_goal_class :return: the name of the monitor with added quotes to be used in logical expressions for conditions. """ @@ -1188,13 +1191,17 @@ def add_local_minimum_reached(self, def add_time_above(self, threshold: float, name: Optional[str] = None, - start_condition: str = ''): + start_condition: str = '', + hold_condition: str = '', + end_condition: Optional[str] = None): """ True if the length of the trajectory is above threshold """ return self.add_monitor(monitor_class=TimeAbove.__name__, name=name, start_condition=start_condition, + hold_condition=hold_condition, + end_condition=end_condition, threshold=threshold) def add_joint_position(self, @@ -1226,7 +1233,8 @@ def add_cartesian_pose(self, absolute: bool = False, name: Optional[str] = None, start_condition: str = '', - stay_true: bool = True): + hold_condition: str = '', + end_condition: Optional[str] = None): """ True if tip_link is closer than the thresholds to goal_pose. """ @@ -1239,9 +1247,10 @@ def add_cartesian_pose(self, tip_group=tip_group, absolute=absolute, start_condition=start_condition, + hold_condition=hold_condition, + end_condition=end_condition, position_threshold=position_threshold, - orientation_threshold=orientation_threshold, - stay_true=stay_true) + orientation_threshold=orientation_threshold) def add_cartesian_position(self, root_link: str, @@ -1253,7 +1262,8 @@ def add_cartesian_position(self, absolute: bool = False, name: Optional[str] = None, start_condition: str = '', - stay_true: bool = True) -> str: + hold_condition: str = '', + end_condition: Optional[str] = None) -> str: """ True if tip_link is closer than threshold to goal_point. """ @@ -1264,10 +1274,11 @@ def add_cartesian_position(self, goal_point=goal_point, root_group=root_group, start_condition=start_condition, + hold_condition=hold_condition, + end_condition=end_condition, absolute=absolute, tip_group=tip_group, - threshold=threshold, - stay_true=stay_true) + threshold=threshold) def add_distance_to_line(self, root_link: str, @@ -1278,8 +1289,9 @@ def add_distance_to_line(self, name: Optional[str] = None, root_group: Optional[str] = None, tip_group: Optional[str] = None, - stay_true: bool = True, start_condition: str = '', + hold_condition: str = '', + end_condition: Optional[str] = None, threshold: float = 0.01): """ True if tip_link is closer than threshold to the line defined by center_point, line_axis and line_length. @@ -1292,8 +1304,9 @@ def add_distance_to_line(self, root_link=root_link, tip_link=tip_link, start_condition=start_condition, + hold_condition=hold_condition, + end_condition=end_condition, root_group=root_group, - stay_true=stay_true, tip_group=tip_group, threshold=threshold) @@ -1307,7 +1320,8 @@ def add_cartesian_orientation(self, absolute: bool = False, name: Optional[str] = None, start_condition: str = '', - stay_true: bool = True): + hold_condition: str = '', + end_condition: Optional[str] = None): """ True if tip_link is closer than threshold to goal_orientation """ @@ -1320,8 +1334,9 @@ def add_cartesian_orientation(self, tip_group=tip_group, absolute=absolute, start_condition=start_condition, - threshold=threshold, - stay_true=stay_true) + hold_condition=hold_condition, + end_condition=end_condition, + threshold=threshold) def add_pointing_at(self, goal_point: PointStamped, @@ -1331,6 +1346,8 @@ def add_pointing_at(self, name: Optional[str] = None, tip_group: Optional[str] = None, start_condition: str = '', + hold_condition: str = '', + end_condition: Optional[str] = None, root_group: Optional[str] = None, threshold: float = 0.01) -> str: """ @@ -1343,6 +1360,8 @@ def add_pointing_at(self, root_link=root_link, tip_group=tip_group, start_condition=start_condition, + hold_condition=hold_condition, + end_condition=end_condition, root_group=root_group, pointing_axis=pointing_axis, threshold=threshold) @@ -1354,6 +1373,8 @@ def add_vectors_aligned(self, tip_normal: Vector3Stamped, name: Optional[str] = None, start_condition: str = '', + hold_condition: str = '', + end_condition: Optional[str] = None, root_group: Optional[str] = None, tip_group: Optional[str] = None, threshold: float = 0.01) -> str: @@ -1367,6 +1388,8 @@ def add_vectors_aligned(self, goal_normal=goal_normal, tip_normal=tip_normal, start_condition=start_condition, + hold_condition=hold_condition, + end_condition=end_condition, root_group=root_group, tip_group=tip_group, threshold=threshold) @@ -1410,7 +1433,9 @@ def add_max_trajectory_length(self, return self.add_monitor(name=None, monitor_class=SetMaxTrajectoryLength.__name__, new_length=max_trajectory_length, - start_condition='') + start_condition='', + hold_condition='', + end_condition='') def add_print(self, message: str, @@ -1439,6 +1464,8 @@ def add_sleep(self, def add_alternator(self, start_condition: str = '', + hold_condition: str = '', + end_condition: Optional[str] = None, name: Optional[str] = None, mod: int = 2) -> str: """ @@ -1450,10 +1477,14 @@ def add_alternator(self, return self.add_monitor(monitor_class=Alternator.__name__, name=name, start_condition=start_condition, + hold_condition=hold_condition, + end_condition=end_condition, mod=mod) def add_payload_alternator(self, start_condition: str = '', + hold_condition: str = '', + end_condition: Optional[str] = None, name: Optional[str] = None, mod: int = 2) -> str: """ @@ -1465,6 +1496,8 @@ def add_payload_alternator(self, return self.add_monitor(monitor_class=Alternator.__name__, name=name, start_condition=start_condition, + hold_condition=hold_condition, + end_condition=end_condition, mod=mod) diff --git a/test/test_integration_pr2.py b/test/test_integration_pr2.py index c490484745..daabb2abd5 100644 --- a/test/test_integration_pr2.py +++ b/test/test_integration_pr2.py @@ -920,7 +920,7 @@ def test_sleep(self, zero_pose: PR2TestWrapper): hold_condition=f'not {alternator}', end_condition=base_monitor) - local_min = zero_pose.monitors.add_local_minimum_reached(stay_true=False) + local_min = zero_pose.monitors.add_local_minimum_reached(end_condition='') end = zero_pose.monitors.add_end_motion(start_condition=' and '.join([local_min, sleep2, right_monitor, @@ -973,7 +973,7 @@ def test_hold_monitors2(self, zero_pose: PR2TestWrapper): stayed_put = zero_pose.monitors.add_cartesian_pose(goal_pose=current_base, tip_link='base_footprint', root_link='map', - stay_true=False, + end_condition='', name='goal reached') zero_pose.motion_goals.add_cartesian_pose(goal_pose=base_goal, From b1a15d655ded80d51ac974538a27880d2165dfe2 Mon Sep 17 00:00:00 2001 From: Simon Date: Thu, 6 Jun 2024 18:00:46 +0200 Subject: [PATCH 04/22] executionstate includes hold and end condition into kwargs for monitors --- src/giskardpy/motion_graph/monitors/monitors.py | 6 +++++- .../{plot_task_graph.py => plot_motion_graph.py} | 13 ++++++++++--- src/giskardpy/tree/branches/prepare_control_loop.py | 2 +- 3 files changed, 16 insertions(+), 5 deletions(-) rename src/giskardpy/tree/behaviors/{plot_task_graph.py => plot_motion_graph.py} (93%) diff --git a/src/giskardpy/motion_graph/monitors/monitors.py b/src/giskardpy/motion_graph/monitors/monitors.py index 5ba5f8f9f8..17c7b74f55 100644 --- a/src/giskardpy/motion_graph/monitors/monitors.py +++ b/src/giskardpy/motion_graph/monitors/monitors.py @@ -15,7 +15,7 @@ from giskardpy.god_map import god_map from giskardpy.motion_graph.graph_node import MotionGraphNode from giskardpy.symbol_manager import symbol_manager -from giskardpy.utils.utils import string_shortener +from giskardpy.utils.utils import string_shortener, kwargs_to_json class Monitor(MotionGraphNode): @@ -49,6 +49,10 @@ def to_ros_msg(self) -> giskard_msgs.Monitor: else: msg.monitor_class = self.__class__.__name__ msg.start_condition = god_map.monitor_manager.format_condition(self.start_condition, new_line=' ') + msg.kwargs = kwargs_to_json({'hold_condition': god_map.monitor_manager.format_condition(self.hold_condition, + new_line=' '), + 'end_condition': god_map.monitor_manager.format_condition(self.end_condition, + new_line=' ')}) return msg @cached_property diff --git a/src/giskardpy/tree/behaviors/plot_task_graph.py b/src/giskardpy/tree/behaviors/plot_motion_graph.py similarity index 93% rename from src/giskardpy/tree/behaviors/plot_task_graph.py rename to src/giskardpy/tree/behaviors/plot_motion_graph.py index d94c942234..b30f3a048e 100644 --- a/src/giskardpy/tree/behaviors/plot_task_graph.py +++ b/src/giskardpy/tree/behaviors/plot_motion_graph.py @@ -14,7 +14,7 @@ from giskardpy.tree.behaviors.publish_feedback import giskard_state_to_execution_state from giskardpy.utils import logging from giskardpy.utils.decorators import record_time, catch_and_raise_to_blackboard -from giskardpy.utils.utils import create_path +from giskardpy.utils.utils import create_path, json_str_to_kwargs def extract_monitor_names_from_condition(condition: str) -> List[str]: @@ -55,12 +55,19 @@ def format_condition(condition: str) -> str: condition = condition.replace('0.0', 'False') return condition + def format_monitor_msg(msg: giskard_msgs.Monitor) -> str: start_condition = format_condition(msg.start_condition) + kwargs = json_str_to_kwargs(msg.kwargs) + hold_condition = format_condition(kwargs['hold_condition']) + end_condition = format_condition(kwargs['end_condition']) return (f'"\'{msg.name}\'\n' - f'class: {msg.monitor_class}\n' f'----------start_condition:----------\n' - f'{start_condition}"') + f'{start_condition}\n' + f'----------hold_condition:-----------\n' + f'{hold_condition}\n' + f'-----------end_condition:-----------\n' + f'{end_condition}"') def format_task_msg(msg: giskard_msgs.MotionGoal) -> str: diff --git a/src/giskardpy/tree/branches/prepare_control_loop.py b/src/giskardpy/tree/branches/prepare_control_loop.py index df4c56900c..8efb68515e 100644 --- a/src/giskardpy/tree/branches/prepare_control_loop.py +++ b/src/giskardpy/tree/branches/prepare_control_loop.py @@ -5,7 +5,7 @@ from giskardpy.tree.behaviors.compile_monitors import CompileMonitors from giskardpy.tree.behaviors.init_qp_controller import InitQPController from giskardpy.tree.behaviors.new_trajectory import NewTrajectory -from giskardpy.tree.behaviors.plot_task_graph import PlotTaskMonitorGraph +from giskardpy.tree.behaviors.plot_motion_graph import PlotTaskMonitorGraph from giskardpy.tree.behaviors.ros_msg_to_goal import ParseActionGoal, AddBaseTrajFollowerGoal, SetExecutionMode from giskardpy.tree.behaviors.set_tracking_start_time import SetTrackingStartTime From 4fedbc140a5ef83bc0665b48262d4e9cdc60d7b5 Mon Sep 17 00:00:00 2001 From: Simon Date: Wed, 19 Jun 2024 11:05:07 +0200 Subject: [PATCH 05/22] has_payload_monitors_which_are_not_end_nor_cancel uses .values() --- src/giskardpy/motion_graph/monitors/monitor_manager.py | 6 +++--- .../motion_graph/monitors/overwrite_state_monitors.py | 6 +++--- 2 files changed, 6 insertions(+), 6 deletions(-) diff --git a/src/giskardpy/motion_graph/monitors/monitor_manager.py b/src/giskardpy/motion_graph/monitors/monitor_manager.py index 6f68c0a587..eb38c7a11c 100644 --- a/src/giskardpy/motion_graph/monitors/monitor_manager.py +++ b/src/giskardpy/motion_graph/monitors/monitor_manager.py @@ -312,19 +312,19 @@ def parse_monitors(self, monitor_msgs: List[giskard_msgs.Monitor]): raise e def has_end_motion_monitor(self) -> bool: - for m in self.monitors: + for m in self.monitors.values(): if isinstance(m, EndMotion): return True return False def has_cancel_motion_monitor(self) -> bool: - for m in self.monitors: + for m in self.monitors.values(): if isinstance(m, CancelMotion): return True return False def has_payload_monitors_which_are_not_end_nor_cancel(self) -> bool: - for m in self.monitors: + for m in self.monitors.values(): if not isinstance(m, (CancelMotion, EndMotion)) and isinstance(m, PayloadMonitor): return True return False diff --git a/src/giskardpy/motion_graph/monitors/overwrite_state_monitors.py b/src/giskardpy/motion_graph/monitors/overwrite_state_monitors.py index 1b0cc3c6c3..d8ad23a976 100644 --- a/src/giskardpy/motion_graph/monitors/overwrite_state_monitors.py +++ b/src/giskardpy/motion_graph/monitors/overwrite_state_monitors.py @@ -5,7 +5,7 @@ from geometry_msgs.msg import PoseStamped from giskardpy import casadi_wrapper as cas -from giskardpy.monitors.monitors import PayloadMonitor +from giskardpy.motion_graph.monitors.monitors import PayloadMonitor from giskardpy.god_map import god_map from giskardpy.exceptions import GoalInitalizationException from giskardpy.model.joints import OmniDrive, DiffDrive, OmniDrivePR22 @@ -29,7 +29,7 @@ def __init__(self, self.seed_configuration = seed_configuration if name is None: name = f'{str(self.__class__.__name__)}/{list(self.seed_configuration.keys())}' - super().__init__(run_call_in_thread=False, name=name, stay_true=True, start_condition=start_condition) + super().__init__(run_call_in_thread=False, name=name, start_condition=start_condition) self.group_name = group_name if group_name is not None: self.seed_configuration = {PrefixName(joint_name, group_name): v for joint_name, v in @@ -56,7 +56,7 @@ def __init__(self, self.group_name = group_name if name is None: name = f'{self.__class__.__name__}/{self.group_name}' - super().__init__(run_call_in_thread=False, name=name, stay_true=True, start_condition=start_condition) + super().__init__(run_call_in_thread=False, name=name, start_condition=start_condition) self.base_pose = base_pose if self.group_name is None: drive_joints = god_map.world.search_for_joint_of_type(self.odom_joints) From c6a4bc5f96e4f5580a137d2183a8321417b04cf8 Mon Sep 17 00:00:00 2001 From: Simon Date: Wed, 19 Jun 2024 16:35:57 +0200 Subject: [PATCH 06/22] fixed default hold and end condition for expression monitors --- src/giskardpy/motion_graph/monitors/monitors.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/giskardpy/motion_graph/monitors/monitors.py b/src/giskardpy/motion_graph/monitors/monitors.py index 17c7b74f55..e7a37caa17 100644 --- a/src/giskardpy/motion_graph/monitors/monitors.py +++ b/src/giskardpy/motion_graph/monitors/monitors.py @@ -150,8 +150,8 @@ class ExpressionMonitor(Monitor): def __init__(self, *, name: Optional[str] = None, start_condition: cas.Expression = cas.TrueSymbol, - hold_condition: cas.Expression = cas.TrueSymbol, - end_condition: cas.Expression = cas.TrueSymbol, + hold_condition: cas.Expression = cas.FalseSymbol, + end_condition: cas.Expression = cas.FalseSymbol, plot: bool = True): """ A Monitor whose state is determined by its expression. From 5f1e85ea2b51dd1057824a4cade5af2a04b35fcb Mon Sep 17 00:00:00 2001 From: Simon Date: Wed, 19 Jun 2024 17:02:49 +0200 Subject: [PATCH 07/22] fixed motion graph plotter --- src/giskardpy/tree/behaviors/plot_motion_graph.py | 14 ++++++++++++++ 1 file changed, 14 insertions(+) diff --git a/src/giskardpy/tree/behaviors/plot_motion_graph.py b/src/giskardpy/tree/behaviors/plot_motion_graph.py index b30f3a048e..7029607381 100644 --- a/src/giskardpy/tree/behaviors/plot_motion_graph.py +++ b/src/giskardpy/tree/behaviors/plot_motion_graph.py @@ -115,6 +115,9 @@ def add_or_get_node(thing: Union[giskard_msgs.Monitor, giskard_msgs.MotionGoal]) # Process monitors and their start_condition for i, monitor in enumerate(execution_state.monitors): + kwargs = json_str_to_kwargs(monitor.kwargs) + hold_condition = format_condition(kwargs['hold_condition']) + end_condition = format_condition(kwargs['end_condition']) monitor_node = add_or_get_node(monitor) monitor_node.obj_dict['attributes']['color'] = monitor_state_to_color[ (execution_state.monitor_life_cycle_state[i], @@ -124,6 +127,17 @@ def add_or_get_node(thing: Union[giskard_msgs.Monitor, giskard_msgs.MotionGoal]) sub_monitor = search_for_monitor(sub_monitor_name, execution_state) sub_monitor_node = add_or_get_node(sub_monitor) graph.add_edge(pydot.Edge(sub_monitor_node, monitor_node, color='green')) + free_symbols = extract_monitor_names_from_condition(hold_condition) + for sub_monitor_name in free_symbols: + sub_monitor = search_for_monitor(sub_monitor_name, execution_state) + sub_monitor_node = add_or_get_node(sub_monitor) + graph.add_edge(pydot.Edge(sub_monitor_node, monitor_node, color='orange')) + free_symbols = extract_monitor_names_from_condition(end_condition) + for sub_monitor_name in free_symbols: + sub_monitor = search_for_monitor(sub_monitor_name, execution_state) + sub_monitor_node = add_or_get_node(sub_monitor) + graph.add_edge(pydot.Edge(sub_monitor_node, monitor_node, color='red', arrowhead='none', arrowtail='normal', + dir='both')) # Process goals and their connections for i, task in enumerate(execution_state.tasks): From 0d2fdd7e4f77212948392b949dea9cd3d533571a Mon Sep 17 00:00:00 2001 From: Simon Date: Wed, 19 Jun 2024 17:02:57 +0200 Subject: [PATCH 08/22] fixed RelativePositionSequence --- src/giskardpy/goals/cartesian_goals.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/giskardpy/goals/cartesian_goals.py b/src/giskardpy/goals/cartesian_goals.py index 84ce9618ff..270395b7bf 100644 --- a/src/giskardpy/goals/cartesian_goals.py +++ b/src/giskardpy/goals/cartesian_goals.py @@ -592,13 +592,13 @@ def __init__(self, error1 = cas.euclidean_distance(root_P_goal1, root_P_current) error1_monitor = ExpressionMonitor(name='p1') - error1_monitor.end_condition = error1_monitor.get_state_expression() 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') - error2_monitor.end_condition = error2_monitor.get_state_expression() 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, From 570345a0ddcf5f75786ff3034b70caed4306e02d Mon Sep 17 00:00:00 2001 From: Simon Date: Wed, 19 Jun 2024 17:36:24 +0200 Subject: [PATCH 09/22] fixed bug in debug traj plotter --- src/giskardpy/tree/behaviors/plot_debug_expressions.py | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/giskardpy/tree/behaviors/plot_debug_expressions.py b/src/giskardpy/tree/behaviors/plot_debug_expressions.py index c5b55b092b..bf8f0f921a 100644 --- a/src/giskardpy/tree/behaviors/plot_debug_expressions.py +++ b/src/giskardpy/tree/behaviors/plot_debug_expressions.py @@ -2,7 +2,7 @@ from threading import Lock import numpy as np -from giskardpy.data_types import JointStates +from giskardpy.data_types import JointStates, PrefixName from giskardpy.god_map import god_map from giskardpy.model.trajectory import Trajectory from giskardpy.tree.behaviors.plot_trajectory import PlotTrajectory @@ -29,13 +29,13 @@ def split_traj(self, traj) -> Trajectory: if isinstance(js_.position, np.ndarray): if len(js_.position.shape) == 1: for x in range(js_.position.shape[0]): - tmp_name = f'{name}|{x}' + tmp_name = PrefixName(f'{name}|{x}') new_js[tmp_name].position = js_.position[x] new_js[tmp_name].velocity = js_.velocity[x] else: for x in range(js_.position.shape[0]): for y in range(js_.position.shape[1]): - tmp_name = f'{name}|{x}_{y}' + tmp_name = PrefixName(f'{name}|{x}_{y}') new_js[tmp_name].position = js_.position[x, y] new_js[tmp_name].velocity = js_.velocity[x, y] else: From 64b211dfdd5f0313958083759aac32de233af3dc Mon Sep 17 00:00:00 2001 From: Simon Date: Wed, 19 Jun 2024 18:03:47 +0200 Subject: [PATCH 10/22] sleep monitor can be put on hold --- .../motion_graph/monitors/payload_monitors.py | 10 +++++++--- 1 file changed, 7 insertions(+), 3 deletions(-) diff --git a/src/giskardpy/motion_graph/monitors/payload_monitors.py b/src/giskardpy/motion_graph/monitors/payload_monitors.py index cc418351c1..920e133366 100644 --- a/src/giskardpy/motion_graph/monitors/payload_monitors.py +++ b/src/giskardpy/motion_graph/monitors/payload_monitors.py @@ -86,6 +86,8 @@ def __call__(self): class Sleep(PayloadMonitor): + start_time: float + def __init__(self, seconds: float, name: Optional[str] = None, @@ -97,11 +99,13 @@ def __init__(self, start_condition=start_condition, hold_condition=hold_condition, end_condition=end_condition, - run_call_in_thread=True) + run_call_in_thread=False) + self.start_time = None def __call__(self): - rospy.sleep(self.seconds) - self.state = True + if self.start_time is None: + self.start_time = god_map.time + self.state = god_map.time - self.start_time >= self.seconds class UpdateParentLinkOfGroup(WorldUpdatePayloadMonitor): From e3a24cdffc8f7be4557606fcec487ca146ee3d83 Mon Sep 17 00:00:00 2001 From: Simon Date: Wed, 19 Jun 2024 18:04:05 +0200 Subject: [PATCH 11/22] updated colors in motion graph plotter --- .../tree/behaviors/plot_motion_graph.py | 22 +++++++++---------- 1 file changed, 11 insertions(+), 11 deletions(-) diff --git a/src/giskardpy/tree/behaviors/plot_motion_graph.py b/src/giskardpy/tree/behaviors/plot_motion_graph.py index 7029607381..f4d4c17513 100644 --- a/src/giskardpy/tree/behaviors/plot_motion_graph.py +++ b/src/giskardpy/tree/behaviors/plot_motion_graph.py @@ -26,25 +26,25 @@ def search_for_monitor(monitor_name: str, execution_state: ExecutionState) -> gi task_state_to_color: Dict[TaskState, str] = { - TaskState.not_started: 'black', + TaskState.not_started: 'gray', TaskState.running: 'green', - TaskState.on_hold: 'gray', + TaskState.on_hold: 'orange', TaskState.succeeded: 'palegreen', - TaskState.failed: 'tomato' + TaskState.failed: 'red' } monitor_state_to_color: Dict[Tuple[TaskState, int], str] = { - (TaskState.not_started, 1): 'black', # doesn't exist + (TaskState.not_started, 1): 'darkgreen', (TaskState.running, 1): 'green', - (TaskState.on_hold, 1): 'gray', + (TaskState.on_hold, 1): 'turquoise', (TaskState.succeeded, 1): 'palegreen', - (TaskState.failed, 1): 'red', + (TaskState.failed, 1): 'gray', - (TaskState.not_started, 0): 'black', - (TaskState.running, 0): 'orange', - (TaskState.on_hold, 0): 'gray', - (TaskState.succeeded, 0): 'palegreen', - (TaskState.failed, 0): 'red' + (TaskState.not_started, 0): 'darkred', + (TaskState.running, 0): 'red2', + (TaskState.on_hold, 0): 'orange', + (TaskState.succeeded, 0): 'red', + (TaskState.failed, 0): 'black' } From c94d1d5dd1001b47470f6290530fc738f6234d10 Mon Sep 17 00:00:00 2001 From: Simon Date: Wed, 19 Jun 2024 18:04:22 +0200 Subject: [PATCH 12/22] added tests for hold conditions of expression monitors --- test/test_integration_pr2.py | 29 +++++++++++++++++++++++++++++ 1 file changed, 29 insertions(+) diff --git a/test/test_integration_pr2.py b/test/test_integration_pr2.py index 9f9ee34fe2..eea8ef253d 100644 --- a/test/test_integration_pr2.py +++ b/test/test_integration_pr2.py @@ -942,6 +942,35 @@ def test_hold_monitors2(self, zero_pose: PR2TestWrapper): zero_pose.set_max_traj_length(30) zero_pose.execute(add_local_minimum_reached=False) + def test_hold_condition_of_monitor(self, zero_pose: PR2TestWrapper): + sleep = zero_pose.monitors.add_sleep(2, name='sleep') + joint_goal = zero_pose.monitors.add_joint_position(name='joint reached', + goal_state=zero_pose.better_pose, + hold_condition=f'not {sleep}') + + zero_pose.motion_goals.add_joint_position(goal_state=zero_pose.better_pose) + zero_pose.monitors.add_end_motion(start_condition=joint_goal) + zero_pose.execute(add_local_minimum_reached=False) + + def test_hold_condition_of_monitor2(self, zero_pose: PR2TestWrapper): + sleep = zero_pose.monitors.add_sleep(1, name='sleep') + sleep2 = zero_pose.monitors.add_sleep(1, name='sleep2', start_condition=sleep) + joint_goal = zero_pose.monitors.add_joint_position(name='joint reached', + goal_state=zero_pose.better_pose) + teleport = zero_pose.monitors.add_set_seed_configuration(seed_configuration=zero_pose.default_pose) + joint_goal2 = zero_pose.monitors.add_joint_position(name='joint reached2', + goal_state=zero_pose.default_pose, + threshold=0.03, + start_condition=teleport, + hold_condition=f'{sleep}', + end_condition='') + + zero_pose.motion_goals.add_joint_position(goal_state=zero_pose.better_pose, + start_condition=sleep2) + zero_pose.monitors.add_end_motion(start_condition=joint_goal) + zero_pose.monitors.add_cancel_motion(start_condition=f'not {joint_goal2} and {sleep2}', error_message='fail') + zero_pose.execute(add_local_minimum_reached=False) + def test_only_payload_monitors(self, zero_pose: PR2TestWrapper): sleep = zero_pose.monitors.add_sleep(5) zero_pose.monitors.add_cancel_motion(start_condition=sleep, error_message='time up', From a5b934167d4fe5bd5e1fa8fb6763ed9ac8f51694 Mon Sep 17 00:00:00 2001 From: Simon Date: Thu, 20 Jun 2024 11:38:57 +0200 Subject: [PATCH 13/22] alternator has no end condition by default --- src/giskardpy/python_interface/python_interface.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/giskardpy/python_interface/python_interface.py b/src/giskardpy/python_interface/python_interface.py index 41a4681b1b..56d4c135b1 100644 --- a/src/giskardpy/python_interface/python_interface.py +++ b/src/giskardpy/python_interface/python_interface.py @@ -1627,7 +1627,7 @@ def add_set_prediction_horizon(self, prediction_horizon: int, **kwargs: goal_par def add_alternator(self, start_condition: str = '', hold_condition: str = '', - end_condition: Optional[str] = None, + end_condition: str = '', name: Optional[str] = None, mod: int = 2) -> str: """ From 771dfb5615b126ecea1280d84f43ad7ba00faca1 Mon Sep 17 00:00:00 2001 From: Simon Date: Thu, 20 Jun 2024 11:39:11 +0200 Subject: [PATCH 14/22] updated colors in motion graph plotter --- src/giskardpy/tree/behaviors/plot_motion_graph.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/giskardpy/tree/behaviors/plot_motion_graph.py b/src/giskardpy/tree/behaviors/plot_motion_graph.py index f4d4c17513..3b56bf7cbe 100644 --- a/src/giskardpy/tree/behaviors/plot_motion_graph.py +++ b/src/giskardpy/tree/behaviors/plot_motion_graph.py @@ -41,9 +41,9 @@ def search_for_monitor(monitor_name: str, execution_state: ExecutionState) -> gi (TaskState.failed, 1): 'gray', (TaskState.not_started, 0): 'darkred', - (TaskState.running, 0): 'red2', + (TaskState.running, 0): 'red', (TaskState.on_hold, 0): 'orange', - (TaskState.succeeded, 0): 'red', + (TaskState.succeeded, 0): 'lightpink', (TaskState.failed, 0): 'black' } From 4f47494bcb1d71b61e1c73af97f9cf7bbe0dff11 Mon Sep 17 00:00:00 2001 From: Simon Date: Thu, 20 Jun 2024 11:39:24 +0200 Subject: [PATCH 15/22] added test for monitor end condition --- test/test_integration_pr2.py | 14 ++++++++++++++ 1 file changed, 14 insertions(+) diff --git a/test/test_integration_pr2.py b/test/test_integration_pr2.py index eea8ef253d..b9b134743c 100644 --- a/test/test_integration_pr2.py +++ b/test/test_integration_pr2.py @@ -971,6 +971,20 @@ def test_hold_condition_of_monitor2(self, zero_pose: PR2TestWrapper): zero_pose.monitors.add_cancel_motion(start_condition=f'not {joint_goal2} and {sleep2}', error_message='fail') zero_pose.execute(add_local_minimum_reached=False) + def test_end_plus_false_monitor(self, zero_pose: PR2TestWrapper): + sleep = zero_pose.monitors.add_sleep(0.5, name='sleep') + joint_goal = zero_pose.monitors.add_joint_position(name='joint reached', + goal_state=zero_pose.better_pose) + joint_goal2 = zero_pose.monitors.add_joint_position(name='joint reached2', + goal_state=zero_pose.better_pose, + end_condition=sleep) + + zero_pose.motion_goals.add_joint_position(goal_state=zero_pose.better_pose, + start_condition=sleep) + zero_pose.monitors.add_end_motion(start_condition=f'{joint_goal} and not {joint_goal2}') + zero_pose.monitors.add_cancel_motion(start_condition=f'{joint_goal} and {joint_goal2}', error_message='fail') + zero_pose.execute(add_local_minimum_reached=False) + def test_only_payload_monitors(self, zero_pose: PR2TestWrapper): sleep = zero_pose.monitors.add_sleep(5) zero_pose.monitors.add_cancel_motion(start_condition=sleep, error_message='time up', From 7f000fc832b52e59f531701ad1a025a060b24202 Mon Sep 17 00:00:00 2001 From: Simon Date: Thu, 20 Jun 2024 12:25:33 +0200 Subject: [PATCH 16/22] fixed bug there plot gantt chart overwrite lifecycle history --- src/giskardpy/tree/behaviors/plot_goal_gantt_chart.py | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/giskardpy/tree/behaviors/plot_goal_gantt_chart.py b/src/giskardpy/tree/behaviors/plot_goal_gantt_chart.py index 80bd66a9a4..fd5ccbdf61 100644 --- a/src/giskardpy/tree/behaviors/plot_goal_gantt_chart.py +++ b/src/giskardpy/tree/behaviors/plot_goal_gantt_chart.py @@ -1,4 +1,5 @@ import traceback +from copy import copy from typing import List, Dict, Tuple, Optional import matplotlib.pyplot as plt @@ -83,6 +84,7 @@ def get_new_history(self) \ god_map.monitor_manager.evaluate_monitors() monitor_history: List[Tuple[float, List[Optional[TaskState]]]] = [] for time_id, (time, (state, life_cycle_state)) in enumerate(god_map.monitor_manager.state_history): + life_cycle_state = copy(life_cycle_state) for monitor_id in range(len(state)): if not state[monitor_id] and life_cycle_state[monitor_id] == TaskState.running: life_cycle_state[monitor_id] = TaskState.on_hold From b2f3b998f8e46c06730dc72938d43da39b14174c Mon Sep 17 00:00:00 2001 From: Simon Date: Thu, 20 Jun 2024 14:07:46 +0200 Subject: [PATCH 17/22] fixed insert cylinder goal --- src/giskardpy/goals/tracebot.py | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/giskardpy/goals/tracebot.py b/src/giskardpy/goals/tracebot.py index dad4634a42..d2f19604bb 100644 --- a/src/giskardpy/goals/tracebot.py +++ b/src/giskardpy/goals/tracebot.py @@ -62,8 +62,8 @@ def __init__(self, distance_to_top = cas.euclidean_distance(root_P_tip, root_P_top) top_reached = cas.less(distance_to_top, 0.01) top_reached_monitor = ExpressionMonitor(name='top reached', start_condition=start_condition) - top_reached_monitor.end_condition = top_reached_monitor.get_state_expression() self.add_monitor(top_reached_monitor) + top_reached_monitor.end_condition = top_reached_monitor.get_state_expression() top_reached_monitor.expression = top_reached distance_to_line, root_P_on_line = cas.distance_point_to_line_segment(root_P_tip, root_P_hole, root_P_top) @@ -71,9 +71,9 @@ def __init__(self, bottom_reached = cas.less(distance_to_hole, 0.01) bottom_reached_monitor = ExpressionMonitor(name='bottom reached', start_condition=start_condition) + self.add_monitor(bottom_reached_monitor) bottom_reached_monitor.end_condition = bottom_reached_monitor.get_state_expression() bottom_reached_monitor.expression = bottom_reached - self.add_monitor(bottom_reached_monitor) reach_top = self.create_and_add_task('reach top') reach_top.add_point_goal_constraints(frame_P_current=root_P_tip, @@ -115,8 +115,8 @@ def __init__(self, # # tilt straight tilt_error = cas.angle_between_vector(root_V_cylinder_z, root_V_up) tilt_monitor = ExpressionMonitor(name='straight', start_condition=start_condition) - tilt_monitor.expression = cas.less(tilt_error, 0.01) self.add_monitor(tilt_monitor) + tilt_monitor.expression = cas.less(tilt_error, 0.01) tilt_straight_task = self.create_and_add_task('tilt straight') tilt_straight_task.add_vector_goal_constraints(frame_V_current=root_V_cylinder_z, From b571244a78f60b9b71f2e37d4e81ae1fa494c39b Mon Sep 17 00:00:00 2001 From: Simon Date: Thu, 20 Jun 2024 14:08:22 +0200 Subject: [PATCH 18/22] monitor manager passes start hold and end condition as init parameters to monitors --- .../motion_graph/monitors/monitor_manager.py | 35 ++++++++++++------- 1 file changed, 22 insertions(+), 13 deletions(-) diff --git a/src/giskardpy/motion_graph/monitors/monitor_manager.py b/src/giskardpy/motion_graph/monitors/monitor_manager.py index eb38c7a11c..111ac556b3 100644 --- a/src/giskardpy/motion_graph/monitors/monitor_manager.py +++ b/src/giskardpy/motion_graph/monitors/monitor_manager.py @@ -48,26 +48,30 @@ def __init__(self): self.triggers = {} self.trigger_conditions = [] - def evaluate_expression(self, node): + def evaluate_expression(self, node, + monitor_name_to_state_expr: Dict[str, cas.Expression]) -> cas.Expression: if isinstance(node, ast.BoolOp): if isinstance(node.op, ast.And): - return cas.logic_and(*[self.evaluate_expression(x) for x in node.values]) + return cas.logic_and(*[self.evaluate_expression(x, monitor_name_to_state_expr) for x in node.values]) elif isinstance(node.op, ast.Or): - return cas.logic_or(*[self.evaluate_expression(x) for x in node.values]) + return cas.logic_or(*[self.evaluate_expression(x, monitor_name_to_state_expr) for x in node.values]) elif isinstance(node, ast.UnaryOp): if isinstance(node.op, ast.Not): - return cas.logic_not(self.evaluate_expression(node.operand)) + return cas.logic_not(self.evaluate_expression(node.operand, monitor_name_to_state_expr)) elif isinstance(node, ast.Str): # replace monitor name with its state expression - return self.get_monitor(node.value).get_state_expression() + return monitor_name_to_state_expr[node.value] raise Exception(f'failed to parse {node}') - def logic_str_to_expr(self, logic_str: str, default: cas.Expression) -> cas.Expression: + def logic_str_to_expr(self, logic_str: str, default: cas.Expression, + monitor_name_to_state_expr: Optional[Dict[str, cas.Expression]] = None) -> cas.Expression: + if monitor_name_to_state_expr is None: + monitor_name_to_state_expr = {key: value.get_state_expression() for key, value in self.monitors.items()} if logic_str == '': return default tree = ast.parse(logic_str, mode='eval') try: - return self.evaluate_expression(tree.body) + return self.evaluate_expression(tree.body, monitor_name_to_state_expr) except KeyError as e: raise GiskardException(f'Unknown symbol {e}') except Exception as e: @@ -292,18 +296,23 @@ def parse_monitors(self, monitor_msgs: List[giskard_msgs.Monitor]): kwargs = json_str_to_kwargs(monitor_msg.kwargs) hold_condition = kwargs.pop('hold_condition') end_condition = kwargs.pop('end_condition') + monitor_name_to_state_expr = {str(key): value.get_state_expression() for key, value in self.monitors.items()} + monitor_name_to_state_expr[monitor_msg.name] = symbol_manager.get_symbol(f'god_map.monitor_manager.state[{len(self.monitors)}]') + start_condition = self.logic_str_to_expr(monitor_msg.start_condition, default=cas.TrueSymbol, + monitor_name_to_state_expr=monitor_name_to_state_expr) + hold_condition = self.logic_str_to_expr(hold_condition, default=cas.FalseSymbol, + monitor_name_to_state_expr=monitor_name_to_state_expr) + end_condition = self.logic_str_to_expr(end_condition, default=cas.FalseSymbol, + monitor_name_to_state_expr=monitor_name_to_state_expr) monitor = C(name=monitor_msg.name, + start_condition=start_condition, + hold_condition=hold_condition, + end_condition=end_condition, **kwargs) if isinstance(monitor, ExpressionMonitor): self.add_expression_monitor(monitor) elif isinstance(monitor, PayloadMonitor): self.add_payload_monitor(monitor) - start_condition = self.logic_str_to_expr(monitor_msg.start_condition, default=cas.TrueSymbol) - hold_condition = self.logic_str_to_expr(hold_condition, default=cas.FalseSymbol) - end_condition = self.logic_str_to_expr(end_condition, default=cas.FalseSymbol) - monitor.start_condition = start_condition - monitor.hold_condition = hold_condition - monitor.end_condition = end_condition except Exception as e: traceback.print_exc() error_msg = f'Initialization of \'{C.__name__}\' monitor failed: \n {e} \n' From fc18c2b952830f1892efdb513ae065847825ed09 Mon Sep 17 00:00:00 2001 From: Simon Date: Thu, 20 Jun 2024 14:08:33 +0200 Subject: [PATCH 19/22] fixed monitor tests --- .../monitors/overwrite_state_monitors.py | 18 ++++++++++++++---- test/test_integration_pr2.py | 1 + 2 files changed, 15 insertions(+), 4 deletions(-) diff --git a/src/giskardpy/motion_graph/monitors/overwrite_state_monitors.py b/src/giskardpy/motion_graph/monitors/overwrite_state_monitors.py index d8ad23a976..3a45bb64cd 100644 --- a/src/giskardpy/motion_graph/monitors/overwrite_state_monitors.py +++ b/src/giskardpy/motion_graph/monitors/overwrite_state_monitors.py @@ -19,7 +19,9 @@ def __init__(self, seed_configuration: Dict[str, float], group_name: Optional[str] = None, name: Optional[str] = None, - start_condition: cas.Expression = cas.TrueSymbol): + start_condition: cas.Expression = cas.TrueSymbol, + hold_condition: cas.Expression = cas.FalseSymbol, + end_condition: cas.Expression = cas.FalseSymbol): """ Overwrite the configuration of the world to allow starting the planning from a different state. CAUTION! don't use this to overwrite the robot's state outside standalone mode! @@ -29,7 +31,10 @@ def __init__(self, self.seed_configuration = seed_configuration if name is None: name = f'{str(self.__class__.__name__)}/{list(self.seed_configuration.keys())}' - super().__init__(run_call_in_thread=False, name=name, start_condition=start_condition) + super().__init__(run_call_in_thread=False, name=name, + start_condition=start_condition, + hold_condition=hold_condition, + end_condition=end_condition) self.group_name = group_name if group_name is not None: self.seed_configuration = {PrefixName(joint_name, group_name): v for joint_name, v in @@ -52,11 +57,16 @@ def __init__(self, base_pose: PoseStamped, group_name: Optional[str] = None, name: Optional[str] = None, - start_condition: cas.Expression = cas.TrueSymbol): + start_condition: cas.Expression = cas.TrueSymbol, + hold_condition: cas.Expression = cas.FalseSymbol, + end_condition: cas.Expression = cas.FalseSymbol): self.group_name = group_name if name is None: name = f'{self.__class__.__name__}/{self.group_name}' - super().__init__(run_call_in_thread=False, name=name, start_condition=start_condition) + super().__init__(run_call_in_thread=False, name=name, + start_condition=start_condition, + hold_condition=hold_condition, + end_condition=end_condition) self.base_pose = base_pose if self.group_name is None: drive_joints = god_map.world.search_for_joint_of_type(self.odom_joints) diff --git a/test/test_integration_pr2.py b/test/test_integration_pr2.py index b9b134743c..9d770bb080 100644 --- a/test/test_integration_pr2.py +++ b/test/test_integration_pr2.py @@ -969,6 +969,7 @@ def test_hold_condition_of_monitor2(self, zero_pose: PR2TestWrapper): start_condition=sleep2) zero_pose.monitors.add_end_motion(start_condition=joint_goal) zero_pose.monitors.add_cancel_motion(start_condition=f'not {joint_goal2} and {sleep2}', error_message='fail') + zero_pose.monitors.add_max_trajectory_length(30) zero_pose.execute(add_local_minimum_reached=False) def test_end_plus_false_monitor(self, zero_pose: PR2TestWrapper): From e4eaf60750c834c1621795fa6686325663caf23d Mon Sep 17 00:00:00 2001 From: Simon Date: Thu, 20 Jun 2024 14:56:57 +0200 Subject: [PATCH 20/22] fixed setprediction horizon --- .../monitors/set_prediction_horizon.py | 27 ++++++++++++++----- 1 file changed, 21 insertions(+), 6 deletions(-) diff --git a/src/giskardpy/motion_graph/monitors/set_prediction_horizon.py b/src/giskardpy/motion_graph/monitors/set_prediction_horizon.py index 99cd27f0f9..2c0b39ad89 100644 --- a/src/giskardpy/motion_graph/monitors/set_prediction_horizon.py +++ b/src/giskardpy/motion_graph/monitors/set_prediction_horizon.py @@ -10,7 +10,9 @@ class SetPredictionHorizon(PayloadMonitor): def __init__(self, prediction_horizon: int, name: Optional[str] = None, - start_condition: cas.Expression = cas.TrueSymbol): + start_condition: cas.Expression = cas.TrueSymbol, + hold_condition: cas.Expression = cas.FalseSymbol, + end_condition: cas.Expression = cas.FalseSymbol): """ Will overwrite the prediction horizon for a single goal. Setting it to 1 will turn of acceleration and jerk limits. @@ -20,7 +22,10 @@ def __init__(self, prediction_horizon: int, name: Optional[str] = None, raise MonitorInitalizationException(f'{self.__class__.__name__}: start_condition must be True.') if name is None: name = self.__class__.__name__ - super().__init__(run_call_in_thread=False, name=name) + super().__init__(run_call_in_thread=False, name=name, + start_condition=start_condition, + hold_condition=hold_condition, + end_condition=end_condition) self.new_prediction_horizon = prediction_horizon if self.new_prediction_horizon < 7: @@ -34,12 +39,17 @@ def __call__(self): class SetQPSolver(PayloadMonitor): def __init__(self, qp_solver_id: Union[SupportedQPSolver, int], name: Optional[str] = None, - start_condition: cas.Expression = cas.TrueSymbol): + start_condition: cas.Expression = cas.TrueSymbol, + hold_condition: cas.Expression = cas.FalseSymbol, + end_condition: cas.Expression = cas.FalseSymbol): if not cas.is_true(start_condition): raise MonitorInitalizationException(f'{self.__class__.__name__}: start_condition must be True.') if name is None: name = self.__class__.__name__ - super().__init__(run_call_in_thread=False, name=name) + super().__init__(run_call_in_thread=False, name=name, + start_condition=start_condition, + hold_condition=hold_condition, + end_condition=end_condition) qp_solver_id = SupportedQPSolver(qp_solver_id) god_map.qp_controller_config.set_qp_solver(qp_solver_id) @@ -49,7 +59,9 @@ def __call__(self): class EnableVelocityTrajectoryTracking(PayloadMonitor): def __init__(self, enabled: bool = True, name: Optional[str] = None, - start_condition: cas.Expression = cas.TrueSymbol): + start_condition: cas.Expression = cas.TrueSymbol, + hold_condition: cas.Expression = cas.FalseSymbol, + end_condition: cas.Expression = cas.FalseSymbol): """ A hack for the PR2. This goal decides whether the velocity part of the trajectory message is filled, when they are send to the robot. @@ -59,7 +71,10 @@ def __init__(self, enabled: bool = True, name: Optional[str] = None, raise MonitorInitalizationException(f'{self.__class__.__name__}: start_condition must be True.') if name is None: name = self.__class__.__name__ - super().__init__(run_call_in_thread=False, name=name) + super().__init__(run_call_in_thread=False, name=name, + start_condition=start_condition, + hold_condition=hold_condition, + end_condition=end_condition) god_map.fill_trajectory_velocity_values = enabled def __call__(self): From 5f9249ec0cf4283ffaa6476bf3434e8437f0f542 Mon Sep 17 00:00:00 2001 From: Simon Date: Mon, 24 Jun 2024 10:23:57 +0200 Subject: [PATCH 21/22] fixed bug where open goal couldn't be used twice for the same joint --- src/giskardpy/goals/open_close.py | 9 ++++----- 1 file changed, 4 insertions(+), 5 deletions(-) diff --git a/src/giskardpy/goals/open_close.py b/src/giskardpy/goals/open_close.py index c905c41a1d..53a90032dc 100644 --- a/src/giskardpy/goals/open_close.py +++ b/src/giskardpy/goals/open_close.py @@ -22,8 +22,7 @@ def __init__(self, name: Optional[str] = None, start_condition: cas.Expression = cas.TrueSymbol, hold_condition: cas.Expression = cas.FalseSymbol, - end_condition: cas.Expression = cas.FalseSymbol - ): + end_condition: cas.Expression = cas.FalseSymbol): """ Open a container in an environment. Only works with the environment was added as urdf. @@ -87,7 +86,8 @@ def __init__(self, weight=WEIGHT_BELOW_CA, start_condition=start_condition, hold_condition=hold_condition, - end_condition=end_condition)) + end_condition=end_condition, + name=f'{self.name}/{self.joint_name.short_name}')) class Close(Goal): @@ -101,8 +101,7 @@ def __init__(self, name: Optional[str] = None, start_condition: cas.Expression = cas.TrueSymbol, hold_condition: cas.Expression = cas.FalseSymbol, - end_condition: cas.Expression = cas.FalseSymbol - ): + end_condition: cas.Expression = cas.FalseSymbol): """ Same as Open, but will use minimum value as default for goal_joint_state """ From 5934892d7329a22b031e9e3d4fa6a23cf8d423d3 Mon Sep 17 00:00:00 2001 From: Simon Date: Mon, 24 Jun 2024 10:24:25 +0200 Subject: [PATCH 22/22] added more monitor tests --- test/test_integration_hsr.py | 68 ++++++++++++++++++++++++++++++++++-- test/test_integration_pr2.py | 28 +++++++++++++++ 2 files changed, 94 insertions(+), 2 deletions(-) diff --git a/test/test_integration_hsr.py b/test/test_integration_hsr.py index 76b954893f..346499727e 100644 --- a/test/test_integration_hsr.py +++ b/test/test_integration_hsr.py @@ -36,7 +36,7 @@ def __init__(self, giskard=None): giskard = Giskard(world_config=WorldWithHSRConfig(), collision_avoidance_config=HSRCollisionAvoidanceConfig(), robot_interface_config=HSRStandaloneInterface(), - behavior_tree_config=StandAloneBTConfig(debug_mode=True, publish_js=True), + behavior_tree_config=StandAloneBTConfig(debug_mode=False, publish_js=True, simulation_max_hz=10), qp_controller_config=QPControllerConfig()) super().__init__(giskard) self.gripper_group = 'gripper' @@ -62,7 +62,6 @@ def reset(self): root_link_name='hand_palm_link') - @pytest.fixture(scope='module') def giskard(request, ros): launch_launchfile('package://hsr_description/launch/upload_hsrb.launch') @@ -333,6 +332,71 @@ def test_open_fridge(self, kitchen_setup: HSRTestWrapper): kitchen_setup.allow_self_collision() kitchen_setup.plan_and_execute() + def test_open_fridge_sequence(self, kitchen_setup: HSRTestWrapper): + handle_frame_id = 'iai_kitchen/iai_fridge_door_handle' + handle_name = 'iai_fridge_door_handle' + kitchen_setup.open_gripper() + base_goal = PoseStamped() + base_goal.header.frame_id = 'map' + base_goal.pose.position = Point(0.3, -0.5, 0) + base_goal.pose.orientation.w = 1 + kitchen_setup.allow_all_collisions() + kitchen_setup.move_base(base_goal) + + bar_axis = Vector3Stamped() + bar_axis.header.frame_id = handle_frame_id + bar_axis.vector.z = 1 + + bar_center = PointStamped() + bar_center.header.frame_id = handle_frame_id + + tip_grasp_axis = Vector3Stamped() + tip_grasp_axis.header.frame_id = kitchen_setup.tip + tip_grasp_axis.vector.x = 1 + + # %% phase 1 + bar_grasped = kitchen_setup.monitors.add_distance_to_line(name='bar grasped', + root_link=kitchen_setup.default_root, + tip_link=kitchen_setup.tip, + center_point=bar_center, + line_axis=bar_axis, + line_length=.4) + kitchen_setup.motion_goals.add_grasp_bar(root_link=kitchen_setup.default_root, + tip_link=kitchen_setup.tip, + tip_grasp_axis=tip_grasp_axis, + bar_center=bar_center, + bar_axis=bar_axis, + bar_length=.4, + name='grasp bar', + end_condition=bar_grasped) + x_gripper = Vector3Stamped() + x_gripper.header.frame_id = kitchen_setup.tip + x_gripper.vector.z = 1 + + x_goal = Vector3Stamped() + x_goal.header.frame_id = handle_frame_id + x_goal.vector.x = -1 + kitchen_setup.motion_goals.add_align_planes(tip_link=kitchen_setup.tip, + tip_normal=x_gripper, + goal_normal=x_goal, + root_link='map', + name='orient to door', + end_condition=bar_grasped) + + # %% phase 2 open door + door_open = kitchen_setup.monitors.add_local_minimum_reached(name='door open', + start_condition=bar_grasped) + kitchen_setup.motion_goals.add_open_container(tip_link=kitchen_setup.tip, + environment_link=handle_name, + goal_joint_state=1.5, + name='open door', + start_condition=bar_grasped, + end_condition=door_open) + + kitchen_setup.allow_all_collisions() + kitchen_setup.monitors.add_end_motion(start_condition=door_open) + kitchen_setup.execute(add_local_minimum_reached=False) + class TestCollisionAvoidanceGoals: diff --git a/test/test_integration_pr2.py b/test/test_integration_pr2.py index 9d770bb080..3a589f9013 100644 --- a/test/test_integration_pr2.py +++ b/test/test_integration_pr2.py @@ -399,6 +399,34 @@ def test_hard_joint_limits(self, zero_pose: PR2TestWrapper): class TestMonitors: + def test_cart_goal_sequence_interrupt(self, zero_pose: PR2TestWrapper): + pose1 = PoseStamped() + pose1.header.frame_id = 'map' + pose1.pose.position.x = 10 + pose1.pose.orientation.w = 1 + + pose2 = PoseStamped() + pose2.header.frame_id = 'base_footprint' + pose2.pose.position.y = 1 + pose2.pose.orientation.w = 1 + + sleep = zero_pose.monitors.add_sleep(3) + zero_pose.motion_goals.add_cartesian_pose(goal_pose=pose1, + tip_link='base_footprint', + root_link='map', + end_condition=sleep, + name='pose1') + zero_pose.motion_goals.add_cartesian_pose(goal_pose=pose2, + tip_link='base_footprint', + root_link='map', + start_condition=sleep, + absolute=True, + name='pose2') + local_min = zero_pose.monitors.add_local_minimum_reached() + zero_pose.monitors.add_end_motion(start_condition=local_min) + zero_pose.allow_all_collisions() + zero_pose.execute(add_local_minimum_reached=False) + def test_start_of_expression_monitor(self, zero_pose: PR2TestWrapper): time_above = zero_pose.monitors.add_time_above(threshold=5) local_min = zero_pose.monitors.add_local_minimum_reached(start_condition=time_above)