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)