| from __future__ import annotations |
|
|
| from typing import Any |
|
|
| from dovla_cil.data.schema import ActionChunk, FailureInfo, RewardInfo, StructuredEffect |
| from dovla_cil.tasks.schema import TaskSpec |
|
|
|
|
| def classify_failure( |
| task: TaskSpec, |
| action: ActionChunk, |
| effect: StructuredEffect, |
| reward: RewardInfo, |
| ) -> FailureInfo: |
| if reward.terminal_success: |
| return FailureInfo( |
| type="success", |
| symbolic_reason="all success predicates are satisfied", |
| language_explanation="The task was completed successfully.", |
| ) |
| if _has_collision_or_instability(effect): |
| return FailureInfo( |
| type="collision_or_unstable", |
| symbolic_reason="rollout reported collision or instability", |
| language_explanation="The intervention caused an unstable or colliding rollout.", |
| ) |
| if _no_motion(effect): |
| return FailureInfo( |
| type="no_motion", |
| symbolic_reason="no object pose, grasp, or articulation change was observed", |
| language_explanation="The action did not produce meaningful motion.", |
| ) |
| if _is_wrong_target(task, action, effect): |
| return FailureInfo( |
| type="wrong_target", |
| symbolic_reason="action or motion targeted a non-target object", |
| language_explanation="The intervention affected the wrong object.", |
| ) |
| if _missed_grasp(task, action, effect): |
| return FailureInfo( |
| type="missed_grasp", |
| symbolic_reason="grasp was attempted but no target grasp succeeded", |
| language_explanation="The robot failed to grasp the intended target.", |
| ) |
| if _dropped_object(task, effect): |
| return FailureInfo( |
| type="dropped_object", |
| symbolic_reason="target changed from grasped/lifted to unheld", |
| language_explanation="The robot dropped the object before completing the task.", |
| ) |
| if _wrong_relation(task, action, effect): |
| return FailureInfo( |
| type="wrong_relation", |
| symbolic_reason=( |
| "action pursued or produced a relation different from the task relation" |
| ), |
| language_explanation=( |
| "The intervention produced the wrong spatial or symbolic relation." |
| ), |
| ) |
| if 0.0 < reward.progress < 0.95: |
| return FailureInfo( |
| type="partial_success", |
| symbolic_reason="some progress components improved but terminal predicates failed", |
| language_explanation="The action made partial progress but did not complete the task.", |
| ) |
| if reward.progress <= 0.0: |
| return FailureInfo( |
| type="insufficient_progress", |
| symbolic_reason="reward progress is zero", |
| language_explanation="The intervention did not make task-relevant progress.", |
| ) |
| return FailureInfo( |
| type="unknown", |
| symbolic_reason="no deterministic failure heuristic matched", |
| language_explanation="The failure mode could not be determined locally.", |
| ) |
|
|
|
|
| def classify_toy_failure( |
| *, distance: float, tolerance: float, collision: bool = False |
| ) -> str | None: |
| if collision: |
| return "collision" |
| if distance <= tolerance: |
| return None |
| return "missed_target" |
|
|
|
|
| def refine_failure_explanation( |
| local_failure: FailureInfo, |
| *, |
| explanation: str, |
| avoidance_hint: str | None = None, |
| suggested_failure_type: str | None = None, |
| confidence: float | None = None, |
| source: str = "local", |
| metadata: dict[str, Any] | None = None, |
| ) -> FailureInfo: |
| """Return a `FailureInfo` with refined language while preserving local type. |
| |
| This helper is used by optional VLM annotation. The simulator-derived failure type remains |
| authoritative; any VLM-proposed type is stored only as metadata. |
| """ |
|
|
| annotation_metadata = { |
| "source": source, |
| "suggested_failure_type": suggested_failure_type, |
| "avoidance_hint": avoidance_hint, |
| "confidence": confidence, |
| } |
| if metadata: |
| annotation_metadata.update(metadata) |
| return FailureInfo( |
| type=local_failure.type, |
| symbolic_reason=local_failure.symbolic_reason, |
| language_explanation=explanation or local_failure.language_explanation, |
| metadata={ |
| **local_failure.metadata, |
| "semantic_annotation": annotation_metadata, |
| }, |
| ) |
|
|
|
|
| def _has_collision_or_instability(effect: StructuredEffect) -> bool: |
| if effect.relation_after.get("collision", False): |
| return True |
| for event in effect.contact_events: |
| event_type = str(event.get("type", "")) |
| if event_type in {"collision", "unstable", "fall", "topple"}: |
| return True |
| return bool(effect.metadata.get("unstable", False)) |
|
|
|
|
| def _no_motion(effect: StructuredEffect) -> bool: |
| moved = any(effect.moved_objects) |
| articulated = any(abs(value) > 1e-6 for value in effect.articulation_delta.values()) |
| grasped = effect.grasp_success is True |
| return not moved and not articulated and not grasped |
|
|
|
|
| def _is_wrong_target(task: TaskSpec, action: ActionChunk, effect: StructuredEffect) -> bool: |
| target_ids = set(task.target_object_ids) |
| intended_target = action.metadata.get("intended_target") |
| if action.metadata.get("candidate_type") == "wrong_target": |
| return True |
| if intended_target and intended_target not in target_ids: |
| return True |
| moved_non_targets = [obj for obj in effect.moved_objects if obj not in target_ids] |
| moved_targets = [obj for obj in effect.moved_objects if obj in target_ids] |
| return bool(moved_non_targets and not moved_targets) |
|
|
|
|
| def _missed_grasp(task: TaskSpec, action: ActionChunk, effect: StructuredEffect) -> bool: |
| relation = _task_relation(task) |
| skill = action.skill_type or "" |
| intended = str(action.metadata.get("intended_relation") or "") |
| attempted_grasp = ( |
| relation in {"grasped", "lifted"} |
| or intended in {"grasped", "lifted"} |
| or skill in {"grasp", "lift"} |
| or _contains_command(action, "grasp") |
| ) |
| return attempted_grasp and effect.grasp_success is False |
|
|
|
|
| def _dropped_object(task: TaskSpec, effect: StructuredEffect) -> bool: |
| for target in task.target_object_ids: |
| before = _object_state(effect.symbolic_before, target) |
| after = _object_state(effect.symbolic_after, target) |
| was_held = bool(before.get("grasped", False) or before.get("lifted", False)) |
| now_held = bool(after.get("grasped", False) or after.get("lifted", False)) |
| if was_held and not now_held: |
| return True |
| return False |
|
|
|
|
| def _wrong_relation(task: TaskSpec, action: ActionChunk, effect: StructuredEffect) -> bool: |
| if action.metadata.get("candidate_type") == "wrong_relation": |
| return True |
| task_relation = _task_relation(task) |
| intended_relation = action.metadata.get("intended_relation") |
| if intended_relation and task_relation and intended_relation != task_relation: |
| return True |
| task_keys = { |
| f"{predicate.name}({','.join(predicate.args)})" |
| for predicate in task.success_predicates |
| } |
| other_true_relations = [ |
| key |
| for key, value in effect.relation_after.items() |
| if value and key not in task_keys and "(" in key |
| ] |
| task_relation_satisfied = any(effect.relation_after.get(key, False) for key in task_keys) |
| return bool(other_true_relations and not task_relation_satisfied) |
|
|
|
|
| def _task_relation(task: TaskSpec) -> str | None: |
| return task.success_predicates[0].name if task.success_predicates else None |
|
|
|
|
| def _object_state(symbolic_state: dict[str, Any], object_id: str) -> dict[str, Any]: |
| objects = symbolic_state.get("objects", {}) |
| if isinstance(objects, dict) and isinstance(objects.get(object_id), dict): |
| return objects[object_id] |
| value = symbolic_state.get(object_id, {}) |
| return value if isinstance(value, dict) else {} |
|
|
|
|
| def _contains_command(action: ActionChunk, command: str) -> bool: |
| if not isinstance(action.values, list): |
| return False |
| return any(isinstance(item, dict) and item.get("command") == command for item in action.values) |
|
|
