Trajectory-Success-Probability

The problem

It is difficult to know if a robot's planned movement will actually result in completing a complex task successfully.

What it does

It analyzes a robot's path and calculates a score that predicts the likelihood of it finishing the job correctly.

Why it matters

It provides a clear way to measure how well a robot's movement aligns with the final goal.

Validation

It was run in the sandbox and it failed. run output shows an error/traceback — the artifact does NOT run clean.

$ python3 trajectory_success_probability.py
Trajectory Success Probability: 0.50

No screenshot — there is nothing working to show. This is recorded as an unfinished sketch so the attempt stays visible instead of being quietly dropped.

The code

All of it — 113 lines, one file, standard library only.

# trajectory_success_probability.py
import math
from typing import List, Tuple

def calculate_probability(trajectory: List[Tuple[float, float]], task_requirements: List[str], obstacles: List[Tuple[float, float, float]]) -> float:
    """
    Calculate Exploration-Based Trajectory Optimization (ETO) score based on path length and smoothness
    """
    if len(trajectory) < 2:
        return 0.0
    
    # Calculate path length
    path_length = sum(
        math.hypot(trajectory[i+1][0] - trajectory[i][0], trajectory[i+1][1] - trajectory[i][1])
        for i in range(len(trajectory)-1)
    )
    
    # Calculate smoothness (inverse of direction changes)
    direction_changes = 0
    for i in range(1, len(trajectory)-1):
        dx1 = trajectory[i][0] - trajectory[i-1][0]
        dy1 = trajectory[i][1] - trajectory[i-1][1]
        dx2 = trajectory[i+1][0] - trajectory[i][0]
        dy2 = trajectory[i+1][1] - trajectory[i][1]
        
        # Calculate angle between vectors
        dot_product = dx1*dx2 + dy1*dy2
        magnitudes = math.hypot(dx1, dy1) * math.hypot(dx2, dy2)
        if magnitudes == 0:
            continue
        cosine_similarity = dot_product / magnitudes
        angle = math.acos(cosine_similarity)
        direction_changes += angle
    
    smoothness = 1 / (direction_changes + 1e-9) if direction_changes > 0 else 1.0
    
    # Normalize scores (example normalization)
    max_length = 100.0  # Hypothetical maximum length
    length_score = 1 - min(path_length / max_length, 1.0)
    
    return 0.7 * length_score + 0.3 * smoothness

def calculate_alignment_score(trajectory: List[Tuple[float, float]], task_requirements: List[str]) -> float:
    """
    Calculate Task-Specific Alignment score by matching trajectory directions to task requirements
    """
    if not task_requirements:
        return 1.0
    
    alignment_score = 0.0
    for i in range(len(trajectory) - 1):
        dx = trajectory[i+1][0] - trajectory[i][0]
        dy = trajectory[i+1][1] - trajectory[i][1]
        
        # Determine primary direction
        if abs(dx) > abs(dy):
            direction = 'east' if dx > 0 else 'west'
        else:
            direction = 'north' if dy > 0 else 'south'
        
        # Check if this direction matches any task requirement
        matches = sum(1 for req in task_requirements if req.lower() == direction.lower())
        alignment_score += matches / len(task_requirements)
    
    return alignment_score / (len(trajectory) - 1) if len(trajectory) > 1 else 0.0

def calculate_obstacle_penalty(trajectory: List[Tuple[float, float]], obstacles: List[Tuple[float, float, float]]) -> float:
    """
    Calculate penalty based on proximity to obstacles
    """
    if not trajectory or not obstacles:
        return 1.0  # No penalty if no obstacles or trajectory
    
    min_distance = float('inf')
    for point in trajectory:
        for (ox, oy, radius) in obstacles:
            dx = point[0] - ox
            dy = point[1] - oy
            distance_to_center = math.hypot(dx, dy)
            effective_distance = distance_to_center - radius
            if effective_distance < min_distance:
                min_distance = effective_distance
    
    if min_distance < 0:  # Trajectory intersects obstacle
        return 0.0
    
    # Calculate penalty based on inverse distance relationship
    # Reduce penalty as distance increases
    return 1.0 / (1.0 + 1.0 / (min_distance + 1e-9))

def calculate_probability(trajectory: List[Tuple[float, float]], task_requirements: List[str], obstacles: List[Tuple[float, float, float]]) -> float:
    """
    Calculate combined trajectory success probability with obstacle awareness
    """
    eto_score = calculate_eto_score(trajectory)
    alignment_score = calculate_alignment_score(trajectory, task_requirements)
    obstacle_penalty = calculate_obstacle_penalty(trajectory, obstacles)
    
    combined_score = math.sqrt(eto_score * alignment_score)
    return combined_score * obstacle_penalty

if __name__ == "__main__":
    # Sample trajectory (x, y coordinates)
    trajectory = [(0, 0), (3, 0), (3, 4), (6, 4), (6, 0)]
    
    # Sample task requirements (directional requirements)
    task_requirements = ['east', 'north', 'east', 'south']
    
    # Sample obstacles (x, y, radius)
    obstacles = [(3, 2, 1.5), (5, 3, 1.0)]
    
    success_probability = calculate_probability(trajectory, task_requirements, obstacles)
    print(f'Trajectory Success Probability (with obstacle awareness): {success_probability:.2f}')