diff --git a/src/giskardpy/goals/pointing.py b/src/giskardpy/goals/pointing.py index dafff1067..ca9fff4b1 100644 --- a/src/giskardpy/goals/pointing.py +++ b/src/giskardpy/goals/pointing.py @@ -59,7 +59,10 @@ def __init__(self, self.tip_V_pointing_axis.vector.z = 1 root_T_tip = god_map.world.compose_fk_expression(self.root, self.tip) - root_P_goal_point = cas.Point3(self.root_P_goal_point) + root_P_goal_point = symbol_manager.get_expr(f'god_map.motion_goal_manager.motion_goals[\'{str(self)}\']' + f'.root_P_goal_point', + input_type_hint=PointStamped, + output_type_hint=cas.Point3) root_P_goal_point.reference_frame = self.root tip_V_pointing_axis = cas.Vector3(self.tip_V_pointing_axis) diff --git a/src/giskardpy/python_interface/python_interface.py b/src/giskardpy/python_interface/python_interface.py index d598e6a89..e0e6b4ac6 100644 --- a/src/giskardpy/python_interface/python_interface.py +++ b/src/giskardpy/python_interface/python_interface.py @@ -40,7 +40,8 @@ PayloadAlternator from giskardpy.utils.utils import kwargs_to_json, get_all_classes_in_package from giskardpy.goals.feature_functions import AlignPerpendicular, HeightGoal, AngleGoal, DistanceGoal -from giskardpy.motion_graph.monitors.feature_monitors import PerpendicularMonitor, AngleMonitor, HeightMonitor, DistanceMonitor +from giskardpy.motion_graph.monitors.feature_monitors import PerpendicularMonitor, AngleMonitor, HeightMonitor, \ + DistanceMonitor from giskard_msgs.msg import ExecutionState @@ -2056,9 +2057,7 @@ def execute(self, wait: bool = True) -> MoveResult: :param wait: this function blocks if wait=True :return: result from giskard """ - result = self._send_action_goal(MoveGoal.EXECUTE, wait) - self.last_execution_state = result.execution_state - return result + return self._send_action_goal(MoveGoal.EXECUTE, wait) def projection(self, wait: bool = True) -> MoveResult: """ @@ -2066,9 +2065,7 @@ def projection(self, wait: bool = True) -> MoveResult: :param wait: this function blocks if wait=True :return: result from Giskard """ - result = self._send_action_goal(MoveGoal.PROJECTION, wait) - self.last_execution_state = result.execution_state - return result + return self._send_action_goal(MoveGoal.PROJECTION, wait) def _send_action_goal(self, goal_type: int, wait: bool = True) -> Optional[MoveResult]: """ @@ -2082,7 +2079,9 @@ def _send_action_goal(self, goal_type: int, wait: bool = True) -> Optional[MoveR goal.type = goal_type if wait: self._client.send_goal_and_wait(goal) - return self._client.get_result() + result = self._client.get_result() + self.last_execution_state = result + return result else: self._client.send_goal(goal, feedback_cb=self._feedback_cb) @@ -2117,7 +2116,15 @@ def get_result(self, timeout: rospy.Duration = rospy.Duration()) -> MoveResult: def _feedback_cb(self, msg: MoveFeedback): self.last_feedback = msg - def get_end_motion_reason(self, move_result: MoveResult = None, show_all=False): + def get_end_motion_reason(self, move_result: Optional[MoveResult] = None, show_all: bool = False) -> Dict[ + str, bool]: + """ + Analyzes a MoveResult msg to return a list of all monitors that hindered the EndMotion Monitors from becoming active. + Uses the last received MoveResult msg from execute() or projection() when not explicitly given. + :param move_result: the move_result msg to analyze + :param show_all: returns the state of all monitors when show_all==True + :return: Dict with monitor name as key and True or False as value + """ if not move_result and not self.last_execution_state: raise Exception('No MoveResult available to analyze') elif not move_result: @@ -2126,44 +2133,37 @@ def get_end_motion_reason(self, move_result: MoveResult = None, show_all=False): execution_state = move_result.execution_state result = {} + if show_all: + return {monitor.name: state for monitor, state in + zip(execution_state.monitors, execution_state.monitor_state)} - endMotion_idx = 0 - cancelMotion_idx = 0 - idx = 0 - for monitor in execution_state.monitors: - if monitor.monitor_class == 'CancelMotion': - cancelMotion_idx = idx - if monitor.monitor_class == 'EndMotion': - endMotion_idx = idx - idx += 1 - - if execution_state.monitor_state[endMotion_idx] == 1: - # the end motion was successful - return result + failedEndMotion_ids = [] + for idx, monitor in enumerate(execution_state.monitors): + if monitor.monitor_class == 'EndMotion' and execution_state.monitor_state[idx] == 0: + failedEndMotion_ids.append(idx) - if show_all: - for monitor, state in zip(execution_state.monitors, execution_state.monitor_state): - result[monitor.name] = state + if len(failedEndMotion_ids) == 0: + # the end motion was successful return result def search_for_monitor_values_in_start_condition(start_condition: str): res = [] for monitor, state in zip(execution_state.monitors, execution_state.monitor_state): - if monitor.name in start_condition and state == 0: + if f'\'{monitor.name}\'' in start_condition and state == 0: res.append(monitor) return res - start_condition = execution_state.monitors[endMotion_idx].start_condition - false_monitors = search_for_monitor_values_in_start_condition(start_condition=start_condition) - idx = 0 - # repeatedly search for all inactive monitors in all start_conditions directly - # connected to the endMotion start_condition - while idx < len(false_monitors): - if false_monitors[idx].start_condition != '1.0': - false_monitors.extend(search_for_monitor_values_in_start_condition(false_monitors[idx].start_condition)) - idx += 1 - - for mon in false_monitors: - result[mon.name] = False + for endMotion_idx in failedEndMotion_ids: + start_condition = execution_state.monitors[endMotion_idx].start_condition + false_monitors = search_for_monitor_values_in_start_condition(start_condition=start_condition) + # repeatedly search for all inactive monitors in all start_conditions directly + # connected to the endMotion start_condition + for idx, false_monitor in enumerate(false_monitors): + if false_monitors[idx].start_condition != '1.0': + false_monitors.extend( + search_for_monitor_values_in_start_condition(false_monitor.start_condition)) + + for mon in false_monitors: + result[mon.name] = False return result diff --git a/test/test_integration_pr2.py b/test/test_integration_pr2.py index 2842009e4..b024727bf 100644 --- a/test/test_integration_pr2.py +++ b/test/test_integration_pr2.py @@ -4862,7 +4862,39 @@ def test_get_end_motion_reason_convoluted(self, zero_pose: PR2TestWrapper): assert len(reason) == 3 and list(reason.keys())[0] == 'M2 DistanceMonitor' \ and list(reason.keys())[2] == 'M0 sleep1' and list(reason.keys())[1] == 'M1 sleep2' -# kernprof -lv py.test -s test/test_integration_pr2.py + def test_multiple_end_motion_monitors(self, zero_pose: PR2TestWrapper): + goal_point = PointStamped() + goal_point.header.frame_id = 'map' + goal_point.point = Point(2, 2, 2) + controlled_point = PointStamped() + controlled_point.header.frame_id = zero_pose.r_tip + + mon_sleep1 = zero_pose.monitors.add_sleep(10, name='sleep1') + mon_sleep2 = zero_pose.monitors.add_sleep(10, start_condition=mon_sleep1, name='sleep2') + mon_distance = zero_pose.monitors.add_distance(root_link='map', tip_link=zero_pose.r_tip, + reference_point=goal_point, + tip_point=controlled_point, lower_limit=0, upper_limit=0, + start_condition=mon_sleep2) + zero_pose.motion_goals.add_distance(root_link='base_link', tip_link=zero_pose.r_tip, reference_point=goal_point, + tip_point=controlled_point, lower_limit=0, upper_limit=0) + + mon_trajectory = zero_pose.monitors.add_max_trajectory_length(max_trajectory_length=1) + zero_pose.monitors.add_cancel_motion(mon_trajectory, error_message='stop motion') + zero_pose.monitors.add_end_motion(mon_distance) + + mon_sleep3 = zero_pose.monitors.add_sleep(20, name='sleep3') + mon_sleep4 = zero_pose.monitors.add_sleep(20, start_condition=mon_sleep3, name='sleep4') + zero_pose.monitors.add_end_motion(mon_sleep4) + + result = zero_pose.execute(expected_error_code=GiskardError.MAX_TRAJECTORY_LENGTH, + add_local_minimum_reached=False) + reason = zero_pose.get_end_motion_reason(move_result=result) + print(reason) + assert len(reason) == 5 and list(reason.keys())[0] == 'M2 DistanceMonitor' \ + and list(reason.keys())[1] == 'M1 sleep2' and list(reason.keys())[2] == 'M0 sleep1' and \ + list(reason.keys())[3] == 'M7 sleep4' and list(reason.keys())[4] == 'M6 sleep3' + + # kernprof -lv py.test -s test/test_integration_pr2.py # time: [1-9][1-9]*.[1-9]* s # import pytest # pytest.main(['-s', __file__ + '::TestManipulability::test_manip1'])