Action Execution Engine Code Examples
1. Basic Action Execution Engine
import time
import threading
import queue
from typing import Dict, List, Any, Optional, Callable
from dataclasses import dataclass
from enum import Enum
class ActionStatus(Enum):
PENDING = "pending"
EXECUTING = "executing"
SUCCESS = "success"
FAILURE = "failure"
CANCELLED = "cancelled"
@dataclass
class RobotAction:
"""Represents a single robot action"""
action_type: str
parameters: Dict[str, Any]
description: str
duration_estimate: float = 1.0
@dataclass
class ActionExecutionResult:
"""Result of action execution"""
status: ActionStatus
duration: float
details: Dict[str, Any]
error: Optional[str] = None
class BasicActionEngine:
def __init__(self):
"""Initialize the basic action execution engine"""
self.active_actions = {}
self.action_queue = queue.Queue()
self.executor_thread = None
self.running = False
self.robot_interface = MockRobotInterface() # In practice, connect to real robot
def start_engine(self):
"""Start the action execution engine"""
self.running = True
self.executor_thread = threading.Thread(target=self._execution_loop)
self.executor_thread.daemon = True
self.executor_thread.start()
def stop_engine(self):
"""Stop the action execution engine"""
self.running = False
if self.executor_thread:
self.executor_thread.join()
def submit_action(self, action: RobotAction) -> str:
"""
Submit an action for execution
Args:
action: The action to execute
Returns:
str: Action ID for tracking
"""
action_id = f"action_{int(time.time() * 1000)}_{len(self.active_actions)}"
action_item = {
"id": action_id,
"action": action,
"status": ActionStatus.PENDING,
"start_time": None,
"result": None
}
self.action_queue.put(action_item)
return action_id
def get_action_status(self, action_id: str) -> Optional[ActionStatus]:
"""Get the status of a specific action"""
if action_id in self.active_actions:
return self.active_actions[action_id]["status"]
# Note: We don't check the queue as it might be processed immediately
return None
def _execution_loop(self):
"""Main execution loop for processing actions"""
while self.running:
try:
# Get next action from queue (with timeout to allow checking self.running)
try:
action_item = self.action_queue.get(timeout=0.1)
except queue.Empty:
continue # Check if we should still be running
# Process the action
self._process_action(action_item)
except Exception as e:
print(f"Error in execution loop: {e}")
def _process_action(self, action_item: Dict[str, Any]):
"""Process a single action"""
action_id = action_item["id"]
# Move to active actions
action_item["status"] = ActionStatus.EXECUTING
action_item["start_time"] = time.time()
self.active_actions[action_id] = action_item
# Execute the action
result = self._execute_action(action_item["action"])
# Update action status
action_item["status"] = result.status
action_item["result"] = result
action_item["duration"] = time.time() - action_item["start_time"]
# Remove from active actions
del self.active_actions[action_id]
def _execute_action(self, action: RobotAction) -> ActionExecutionResult:
"""Execute a single action"""
start_time = time.time()
try:
if action.action_type == "move_to":
result = self._execute_move_to(action.parameters)
elif action.action_type == "grasp":
result = self._execute_grasp(action.parameters)
elif action.action_type == "release":
result = self._execute_release(action.parameters)
elif action.action_type == "navigate":
result = self._execute_navigate(action.parameters)
elif action.action_type == "detect_object":
result = self._execute_detect_object(action.parameters)
elif action.action_type == "speak":
result = self._execute_speak(action.parameters)
else:
return ActionExecutionResult(
status=ActionStatus.FAILURE,
duration=time.time() - start_time,
details={},
error=f"Unknown action type: {action.action_type}"
)
return result
except Exception as e:
return ActionExecutionResult(
status=ActionStatus.FAILURE,
duration=time.time() - start_time,
details={},
error=str(e)
)
def _execute_move_to(self, parameters: Dict[str, Any]) -> ActionExecutionResult:
"""Execute move to location action"""
location = parameters.get("location", [0, 0, 0])
speed = parameters.get("speed", 0.5)
# In practice, this would interface with the robot's navigation system
execution_time = 2.0 # Simulate movement time based on distance if needed
time.sleep(execution_time)
success = self.robot_interface.move_to(location, speed)
return ActionExecutionResult(
status=ActionStatus.SUCCESS if success else ActionStatus.FAILURE,
duration=execution_time,
details={"target_location": location, "actual_location": location},
error=None if success else "Failed to reach target location"
)
def _execute_grasp(self, parameters: Dict[str, Any]) -> ActionExecutionResult:
"""Execute grasp object action"""
object_name = parameters.get("object", "unknown_object")
position = parameters.get("position", [0, 0, 0])
force = parameters.get("force", 10) # Default grasp force
execution_time = 3.0 # Simulate grasp time
time.sleep(execution_time)
success = self.robot_interface.grasp_object(object_name, position, force)
return ActionExecutionResult(
status=ActionStatus.SUCCESS if success else ActionStatus.FAILURE,
duration=execution_time,
details={"object": object_name, "position": position, "force": force},
error=None if success else f"Failed to grasp {object_name}"
)
def _execute_release(self, parameters: Dict[str, Any]) -> ActionExecutionResult:
"""Execute release object action"""
object_name = parameters.get("object", "unknown_object")
position = parameters.get("position", [0, 0, 0])
execution_time = 1.5 # Simulate release time
time.sleep(execution_time)
success = self.robot_interface.release_object(object_name, position)
return ActionExecutionResult(
status=ActionStatus.SUCCESS if success else ActionStatus.FAILURE,
duration=execution_time,
details={"object": object_name, "position": position},
error=None if success else f"Failed to release {object_name}"
)
def _execute_navigate(self, parameters: Dict[str, Any]) -> ActionExecutionResult:
"""Execute navigation action"""
waypoints = parameters.get("waypoints", [])
speed = parameters.get("speed", 0.3)
execution_time = 5.0 # Simulate navigation time
time.sleep(execution_time)
success = self.robot_interface.navigate_path(waypoints, speed)
return ActionExecutionResult(
status=ActionStatus.SUCCESS if success else ActionStatus.FAILURE,
duration=execution_time,
details={"waypoints": waypoints, "speed": speed},
error=None if success else "Failed to navigate path"
)
def _execute_detect_object(self, parameters: Dict[str, Any]) -> ActionExecutionResult:
"""Execute object detection action"""
object_type = parameters.get("object_type", "any")
detection_range = parameters.get("range", 2.0)
execution_time = 1.0 # Simulate detection time
time.sleep(execution_time)
detected_objects = self.robot_interface.detect_objects(object_type, detection_range)
return ActionExecutionResult(
status=ActionStatus.SUCCESS,
duration=execution_time,
details={"detected_objects": detected_objects, "search_type": object_type, "range": detection_range},
error=None
)
def _execute_speak(self, parameters: Dict[str, Any]) -> ActionExecutionResult:
"""Execute speech action"""
text = parameters.get("text", "")
volume = parameters.get("volume", 0.7)
execution_time = len(text) * 0.1 # Simulate speech duration
time.sleep(execution_time)
success = self.robot_interface.speak_text(text, volume)
return ActionExecutionResult(
status=ActionStatus.SUCCESS if success else ActionStatus.FAILURE,
duration=execution_time,
details={"spoken_text": text, "volume": volume},
error=None if success else "Failed to speak text"
)
class MockRobotInterface:
"""Mock interface to simulate robot hardware for examples"""
def move_to(self, location: List[float], speed: float = 0.5) -> bool:
"""Simulate moving to a location"""
print(f"Moving to location: {location} at speed {speed}")
return True # Simulate success
def grasp_object(self, object_name: str, position: List[float], force: float = 10) -> bool:
"""Simulate grasping an object"""
print(f"Grasping {object_name} at {position} with force {force}")
return True # Simulate success
def release_object(self, object_name: str, position: List[float]) -> bool:
"""Simulate releasing an object"""
print(f"Releasing {object_name} at {position}")
return True # Simulate success
def navigate_path(self, waypoints: List[List[float]], speed: float = 0.3) -> bool:
"""Simulate navigating a path"""
print(f"Navigating path with {len(waypoints)} waypoints at speed {speed}")
return True # Simulate success
def detect_objects(self, object_type: str, detection_range: float) -> List[Dict[str, Any]]:
"""Simulate detecting objects"""
print(f"Detecting {object_type} objects within range {detection_range}")
return [{"name": "simulated_object", "type": object_type, "position": [0.5, 0.3, 0.8]}]
def speak_text(self, text: str, volume: float = 0.7) -> bool:
"""Simulate speaking text"""
print(f"Speaking: '{text}' at volume {volume}")
return True # Simulate success
# Example usage
engine = BasicActionEngine()
engine.start_engine()
# Submit some actions
action1 = RobotAction("move_to", {"location": [1.0, 2.0, 0.0]}, "Move to position [1, 2, 0]")
action_id1 = engine.submit_action(action1)
action2 = RobotAction("grasp", {"object": "red_cup", "position": [1.0, 2.0, 0.5]}, "Grasp the red cup")
action_id2 = engine.submit_action(action2)
print(f"Submitted actions with IDs: {action_id1}, {action_id2}")
# Stop the engine after some time
time.sleep(10)
engine.stop_engine()
2. Action Engine with Safety and Validation
from typing import Protocol
class RobotSafetyValidator(Protocol):
"""Protocol for safety validation"""
def validate_action(self, action: RobotAction, environment_context: Dict[str, Any]) -> tuple:
"""
Validate an action for safety
Returns:
tuple: (is_safe: bool, safety_level: str, reasons: List[str])
"""
...
class SafetyActionEngine(BasicActionEngine):
def __init__(self, safety_validator: RobotSafetyValidator = None):
"""
Action engine with safety validation
Args:
safety_validator: Optional safety validation system
"""
super().__init__()
self.safety_validator = safety_validator or DefaultSafetyValidator()
self.safety_log = []
def submit_action_with_validation(self, action: RobotAction,
environment_context: Dict[str, Any] = None) -> str:
"""
Submit an action with safety validation
Args:
action: The action to execute
environment_context: Current environment context for safety validation
Returns:
str: Action ID for tracking, or empty string if action is unsafe
"""
if environment_context is None:
environment_context = {}
# Validate action for safety
is_safe, safety_level, reasons = self.safety_validator.validate_action(
action, environment_context
)
if not is_safe:
self._log_safety_event("unsafe_action_blocked", {
"action": action.description,
"reasons": reasons,
"safety_level": safety_level
})
return "" # Don't submit unsafe actions
return self.submit_action(action)
def _log_safety_event(self, event_type: str, details: Dict[str, Any]):
"""Log a safety-related event"""
event = {
"timestamp": time.time(),
"type": event_type,
"details": details
}
self.safety_log.append(event)
# Keep only recent safety events
if len(self.safety_log) > 1000:
self.safety_log = self.safety_log[-1000:]
def _execute_action(self, action: RobotAction) -> ActionExecutionResult:
"""Execute action with safety monitoring"""
# In a real system, we would continuously monitor safety during execution
return super()._execute_action(action)
class DefaultSafetyValidator:
"""Default safety validator implementation"""
def validate_action(self, action: RobotAction, environment_context: Dict[str, Any]) -> tuple:
"""
Validate an action for safety using default rules
"""
reasons = []
safety_level = "safe"
# Check for potentially dangerous action parameters
if action.action_type == "grasp":
force = action.parameters.get("force", 10)
if force > 50: # Too much force
reasons.append(f"Grasp force {force} too high (max 50)")
safety_level = "danger"
elif action.action_type == "move_to" or action.action_type == "navigate":
location = action.parameters.get("location", [0, 0, 0])
# Check for obstacles in environment context
obstacles = environment_context.get("obstacles", [])
for obstacle in obstacles:
obstacle_pos = obstacle.get("position", [0, 0, 0])
distance = self._calculate_distance(location, obstacle_pos)
if distance < 0.5: # Less than 50cm
reasons.append(f"Path to {location} blocked by obstacle at {obstacle_pos}")
safety_level = "danger"
# Check if robot is in operational state
robot_status = environment_context.get("robot_status", {})
if not robot_status.get("operational", True):
reasons.append("Robot is not in operational state")
safety_level = "critical"
is_safe = safety_level not in ["danger", "critical"]
return is_safe, safety_level, reasons
def _calculate_distance(self, pos1: List[float], pos2: List[float]) -> float:
"""Calculate Euclidean distance between two 3D positions"""
return sum((a - b) ** 2 for a, b in zip(pos1, pos2)) ** 0.5
# Example usage
safety_validator = DefaultSafetyValidator()
safety_engine = SafetyActionEngine(safety_validator)
safety_engine.start_engine()
# Environment context with obstacles
env_context = {
"obstacles": [
{"type": "chair", "position": [1.0, 1.0, 0.0]},
{"type": "table", "position": [2.0, 2.0, 0.0]}
],
"robot_status": {"operational": True}
}
# Try to submit a safe action
safe_action = RobotAction(
"move_to",
{"location": [3.0, 3.0, 0.0]},
"Move to safe location"
)
action_id = safety_engine.submit_action_with_validation(safe_action, env_context)
print(f"Safe action ID: {action_id}")
# Try to submit an unsafe action
unsafe_action = RobotAction(
"move_to",
{"location": [1.0, 1.0, 0.0]}, # Location with obstacle
"Move to location with obstacle"
)
unsafe_id = safety_engine.submit_action_with_validation(unsafe_action, env_context)
print(f"Unsafe action ID: {unsafe_id}") # Should be empty
safety_engine.stop_engine()
3. Concurrent Action Execution Engine
import asyncio
import concurrent.futures
from typing import Set
class ConcurrentActionEngine:
def __init__(self, max_concurrent_actions: int = 3):
"""
Action engine that can execute multiple actions concurrently
Args:
max_concurrent_actions: Maximum number of concurrent actions
"""
self.max_concurrent_actions = max_concurrent_actions
self.executor = concurrent.futures.ThreadPoolExecutor(max_workers=max_concurrent_actions)
self.active_actions = {}
self.action_queue = queue.Queue()
self.running = False
self.robot_interface = MockRobotInterface()
# Action types that can run concurrently vs. those that need exclusivity
self.concurrent_actions = {"detect_object", "speak", "monitor"} # Actions that don't conflict
self.exclusive_actions = {"move_to", "grasp", "release", "navigate"} # Actions that conflict
def start_engine(self):
"""Start the concurrent action engine"""
self.running = True
# In a real implementation, we might use asyncio for better concurrency
# For this example, we'll use a single thread to manage the action queue
self.engine_thread = threading.Thread(target=self._management_loop)
self.engine_thread.daemon = True
self.engine_thread.start()
def stop_engine(self):
"""Stop the concurrent action engine"""
self.running = False
if hasattr(self, 'engine_thread'):
self.engine_thread.join()
def submit_action(self, action: RobotAction) -> str:
"""
Submit an action for execution
Args:
action: The action to execute
Returns:
str: Action ID for tracking
"""
action_id = f"concurrent_action_{int(time.time() * 1000)}_{len(self.active_actions)}"
action_item = {
"id": action_id,
"action": action,
"status": ActionStatus.PENDING,
"start_time": None,
"result": None,
"future": None
}
self.action_queue.put(action_item)
return action_id
def _management_loop(self):
"""Management loop for concurrent action execution"""
while self.running:
try:
# Check if we have capacity for more actions
if self._get_concurrent_action_count() < self.max_concurrent_actions:
try:
# Get next action from queue
action_item = self.action_queue.get(timeout=0.1)
self._start_action_execution(action_item)
except queue.Empty:
pass # No actions to process
# Check status of running actions
self._check_running_actions()
time.sleep(0.01) # Small delay to prevent busy waiting
except Exception as e:
print(f"Error in management loop: {e}")
def _get_concurrent_action_count(self) -> int:
"""Get the number of currently executing actions that don't conflict"""
count = 0
for action_item in self.active_actions.values():
if action_item["action"].action_type in self.concurrent_actions:
count += 1
return count
def _start_action_execution(self, action_item: Dict[str, Any]):
"""Start execution of an action in a separate thread"""
action_id = action_item["id"]
# Check if this action type conflicts with others
if action_item["action"].action_type in self.exclusive_actions:
# Check if any exclusive actions are currently running
for other_item in self.active_actions.values():
if other_item["action"].action_type in self.exclusive_actions:
# Wait for exclusive actions to complete before starting this one
print(f"Waiting for exclusive action to complete before starting {action_id}")
# For this example, we'll just skip and try later
# In a real system, you'd properly queue this action
return
# Mark as executing
action_item["status"] = ActionStatus.EXECUTING
action_item["start_time"] = time.time()
self.active_actions[action_id] = action_item
# Submit to executor
future = self.executor.submit(self._execute_action_thread, action_item)
action_item["future"] = future
def _execute_action_thread(self, action_item: Dict[str, Any]):
"""Execute action in a separate thread"""
action = action_item["action"]
start_time = action_item["start_time"]
try:
if action.action_type == "move_to":
result = self._execute_move_to(action.parameters)
elif action.action_type == "grasp":
result = self._execute_grasp(action.parameters)
elif action.action_type == "release":
result = self._execute_release(action.parameters)
elif action.action_type == "navigate":
result = self._execute_navigate(action.parameters)
elif action.action_type == "detect_object":
result = self._execute_detect_object(action.parameters)
elif action.action_type == "speak":
result = self._execute_speak(action.parameters)
else:
result = ActionExecutionResult(
status=ActionStatus.FAILURE,
duration=time.time() - start_time,
details={},
error=f"Unknown action type: {action.action_type}"
)
# Update action result
action_item["result"] = result
action_item["status"] = result.status
action_item["duration"] = time.time() - start_time
except Exception as e:
result = ActionExecutionResult(
status=ActionStatus.FAILURE,
duration=time.time() - start_time,
details={},
error=str(e)
)
action_item["result"] = result
action_item["status"] = result.status
action_item["duration"] = time.time() - start_time
def _check_running_actions(self):
"""Check if running actions have completed"""
completed_ids = []
for action_id, action_item in self.active_actions.items():
future = action_item.get("future")
if future and future.done():
completed_ids.append(action_id)
# Remove completed actions
for action_id in completed_ids:
del self.active_actions[action_id]
def _execute_move_to(self, parameters: Dict[str, Any]) -> ActionExecutionResult:
"""Execute move to location action"""
location = parameters.get("location", [0, 0, 0])
speed = parameters.get("speed", 0.5)
execution_time = 2.0 # Simulate movement time
time.sleep(execution_time)
success = self.robot_interface.move_to(location, speed)
return ActionExecutionResult(
status=ActionStatus.SUCCESS if success else ActionStatus.FAILURE,
duration=execution_time,
details={"target_location": location, "actual_location": location},
error=None if success else "Failed to reach target location"
)
def _execute_grasp(self, parameters: Dict[str, Any]) -> ActionExecutionResult:
"""Execute grasp object action"""
object_name = parameters.get("object", "unknown_object")
position = parameters.get("position", [0, 0, 0])
force = parameters.get("force", 10)
execution_time = 3.0
time.sleep(execution_time)
success = self.robot_interface.grasp_object(object_name, position, force)
return ActionExecutionResult(
status=ActionStatus.SUCCESS if success else ActionStatus.FAILURE,
duration=execution_time,
details={"object": object_name, "position": position, "force": force},
error=None if success else f"Failed to grasp {object_name}"
)
def _execute_release(self, parameters: Dict[str, Any]) -> ActionExecutionResult:
"""Execute release object action"""
object_name = parameters.get("object", "unknown_object")
position = parameters.get("position", [0, 0, 0])
execution_time = 1.5
time.sleep(execution_time)
success = self.robot_interface.release_object(object_name, position)
return ActionExecutionResult(
status=ActionStatus.SUCCESS if success else ActionStatus.FAILURE,
duration=execution_time,
details={"object": object_name, "position": position},
error=None if success else f"Failed to release {object_name}"
)
def _execute_navigate(self, parameters: Dict[str, Any]) -> ActionExecutionResult:
"""Execute navigation action"""
waypoints = parameters.get("waypoints", [])
speed = parameters.get("speed", 0.3)
execution_time = 5.0
time.sleep(execution_time)
success = self.robot_interface.navigate_path(waypoints, speed)
return ActionExecutionResult(
status=ActionStatus.SUCCESS if success else ActionStatus.FAILURE,
duration=execution_time,
details={"waypoints": waypoints, "speed": speed},
error=None if success else "Failed to navigate path"
)
def _execute_detect_object(self, parameters: Dict[str, Any]) -> ActionExecutionResult:
"""Execute object detection action"""
object_type = parameters.get("object_type", "any")
detection_range = parameters.get("range", 2.0)
execution_time = 1.0
time.sleep(execution_time)
detected_objects = self.robot_interface.detect_objects(object_type, detection_range)
return ActionExecutionResult(
status=ActionStatus.SUCCESS,
duration=execution_time,
details={"detected_objects": detected_objects, "search_type": object_type, "range": detection_range},
error=None
)
def _execute_speak(self, parameters: Dict[str, Any]) -> ActionExecutionResult:
"""Execute speech action"""
text = parameters.get("text", "")
volume = parameters.get("volume", 0.7)
execution_time = len(text) * 0.1
time.sleep(execution_time)
success = self.robot_interface.speak_text(text, volume)
return ActionExecutionResult(
status=ActionStatus.SUCCESS if success else ActionStatus.FAILURE,
duration=execution_time,
details={"spoken_text": text, "volume": volume},
error=None if success else "Failed to speak text"
)
# Example usage
concurrent_engine = ConcurrentActionEngine(max_concurrent_actions=5)
concurrent_engine.start_engine()
# Submit multiple actions
actions = [
RobotAction("detect_object", {"object_type": "cup"}, "Detect cup"),
RobotAction("speak", {"text": "Looking for objects"}, "Speak announcement"),
RobotAction("move_to", {"location": [1.0, 0.0, 0.0]}, "Move to first position"),
RobotAction("detect_object", {"object_type": "bottle"}, "Detect bottle"),
RobotAction("speak", {"text": "Found objects"}, "Speak result"),
]
action_ids = []
for action in actions:
action_id = concurrent_engine.submit_action(action)
action_ids.append(action_id)
print(f"Submitted action: {action_id} - {action.description}")
time.sleep(15) # Wait for actions to complete
concurrent_engine.stop_engine()
4. Action Engine with State Management
class RobotState:
"""Represents the current state of the robot"""
def __init__(self):
self.position = [0.0, 0.0, 0.0]
self.orientation = [0.0, 0.0, 0.0, 1.0] # quaternion [x, y, z, w]
self.gripper_status = "open" # "open", "closed", "holding_object"
self.held_object = None
self.battery_level = 100.0
self.current_task = None
self.last_action_result = None
class StatefulActionEngine(BasicActionEngine):
def __init__(self):
"""Action engine with state management"""
super().__init__()
self.robot_state = RobotState()
self.state_history = []
def _execute_action(self, action: RobotAction) -> ActionExecutionResult:
"""Execute action and update robot state"""
# Store previous state
previous_state = self._copy_state(self.robot_state)
# Execute the action
result = super()._execute_action(action)
# Update state based on action type and result
if result.status == ActionStatus.SUCCESS:
self._update_state_based_on_action(action)
# Record state change
self.state_history.append({
"action": action,
"previous_state": previous_state,
"new_state": self._copy_state(self.robot_state),
"result": result,
"timestamp": time.time()
})
# Keep only recent history
if len(self.state_history) > 1000:
self.state_history = self.state_history[-1000:]
return result
def _copy_state(self, state: RobotState) -> RobotState:
"""Create a copy of the robot state"""
new_state = RobotState()
new_state.position = state.position.copy()
new_state.orientation = state.orientation.copy()
new_state.gripper_status = state.gripper_status
new_state.held_object = state.held_object
new_state.battery_level = state.battery_level
new_state.current_task = state.current_task
new_state.last_action_result = state.last_action_result
return new_state
def _update_state_based_on_action(self, action: RobotAction):
"""Update robot state based on the action performed"""
if action.action_type == "move_to":
# Update position
if "location" in action.parameters:
self.robot_state.position = action.parameters["location"]
elif action.action_type == "grasp":
# Update gripper status and held object
self.robot_state.gripper_status = "closed"
if "object" in action.parameters:
self.robot_state.held_object = action.parameters["object"]
elif action.action_type == "release":
# Update gripper status and clear held object
self.robot_state.gripper_status = "open"
self.robot_state.held_object = None
elif action.action_type == "navigate":
# Update position to the last waypoint
waypoints = action.parameters.get("waypoints", [])
if waypoints:
self.robot_state.position = waypoints[-1]
elif action.action_type == "detect_object":
# No state change for detection, but we could update detected objects
pass
# Update battery level (simulated consumption)
self.robot_state.battery_level = max(0, self.robot_state.battery_level - 0.1)
def get_robot_state(self) -> RobotState:
"""Get the current robot state"""
return self._copy_state(self.robot_state)
def get_state_history(self, limit: int = 10) -> List[Dict[str, Any]]:
"""Get recent state history"""
return self.state_history[-limit:]
def _execute_move_to(self, parameters: Dict[str, Any]) -> ActionExecutionResult:
"""Execute move to location action with state update"""
location = parameters.get("location", [0, 0, 0])
speed = parameters.get("speed", 0.5)
execution_time = 2.0
time.sleep(execution_time)
# In a real system, we would check if the movement is possible given current state
# For example, ensure gripper is not holding a fragile object during fast movement
if self.robot_state.gripper_status == "closed" and self.robot_state.held_object:
if speed > 0.3: # Slow down if holding an object
execution_time *= 1.5 # Movement takes longer when holding object
success = self.robot_interface.move_to(location, speed)
return ActionExecutionResult(
status=ActionStatus.SUCCESS if success else ActionStatus.FAILURE,
duration=execution_time,
details={"target_location": location, "actual_location": location},
error=None if success else "Failed to reach target location"
)
def _execute_grasp(self, parameters: Dict[str, Any]) -> ActionExecutionResult:
"""Execute grasp object action with state validation"""
# Check if gripper is already holding something
if self.robot_state.held_object is not None:
return ActionExecutionResult(
status=ActionStatus.FAILURE,
duration=0.0,
details={},
error="Gripper already holding an object"
)
object_name = parameters.get("object", "unknown_object")
position = parameters.get("position", [0, 0, 0])
force = parameters.get("force", 10)
execution_time = 3.0
time.sleep(execution_time)
success = self.robot_interface.grasp_object(object_name, position, force)
return ActionExecutionResult(
status=ActionStatus.SUCCESS if success else ActionStatus.FAILURE,
duration=execution_time,
details={"object": object_name, "position": position, "force": force},
error=None if success else f"Failed to grasp {object_name}"
)
def _execute_release(self, parameters: Dict[str, Any]) -> ActionExecutionResult:
"""Execute release object action with state validation"""
# Check if gripper is actually holding something
if self.robot_state.held_object is None:
return ActionExecutionResult(
status=ActionStatus.FAILURE,
duration=0.0,
details={},
error="Gripper not holding any object"
)
object_name = parameters.get("object", self.robot_state.held_object)
position = parameters.get("position", self.robot_state.position)
execution_time = 1.5
time.sleep(execution_time)
success = self.robot_interface.release_object(object_name, position)
return ActionExecutionResult(
status=ActionStatus.SUCCESS if success else ActionStatus.FAILURE,
duration=execution_time,
details={"object": object_name, "position": position},
error=None if success else f"Failed to release {object_name}"
)
# Example usage
stateful_engine = StatefulActionEngine()
stateful_engine.start_engine()
print(f"Initial state - Position: {stateful_engine.get_robot_state().position}, "
f"Gripper: {stateful_engine.get_robot_state().gripper_status}")
# Submit actions that change state
move_action = RobotAction("move_to", {"location": [1.0, 1.0, 0.0]}, "Move to [1,1,0]")
move_id = stateful_engine.submit_action(move_action)
grasp_action = RobotAction("grasp", {"object": "red_cup"}, "Grasp red cup")
grasp_id = stateful_engine.submit_action(grasp_action)
time.sleep(10) # Wait for actions to complete
print(f"Final state - Position: {stateful_engine.get_robot_state().position}, "
f"Gripper: {stateful_engine.get_robot_state().gripper_status}, "
f"Holding: {stateful_engine.get_robot_state().held_object}")
# Show state history
history = stateful_engine.get_state_history()
for i, entry in enumerate(history):
print(f"History {i}: {entry['action'].description} -> "
f"Position: {entry['new_state'].position}, "
f"Gripper: {entry['new_state'].gripper_status}")
stateful_engine.stop_engine()
5. Action Engine with Recovery and Error Handling
class RecoveryActionEngine(StatefulActionEngine):
def __init__(self):
"""Action engine with automatic recovery from failures"""
super().__init__()
self.error_recovery_strategies = self._initialize_recovery_strategies()
def _initialize_recovery_strategies(self) -> Dict[str, Callable]:
"""Initialize error recovery strategies"""
return {
"move_failure": self._recover_from_move_failure,
"grasp_failure": self._recover_from_grasp_failure,
"navigation_failure": self._recover_from_navigation_failure,
"communication_error": self._recover_from_communication_error
}
def _execute_action(self, action: RobotAction) -> ActionExecutionResult:
"""Execute action with error handling and recovery"""
# First, try to execute the action normally
result = super()._execute_action(action)
# If it failed, attempt recovery
if result.status == ActionStatus.FAILURE:
recovery_result = self._attempt_recovery(action, result)
if recovery_result["success"]:
# Retry the action after recovery
result = super()._execute_action(action)
result.details["recovered"] = True
return result
def _attempt_recovery(self, failed_action: RobotAction,
failure_result: ActionExecutionResult) -> Dict[str, Any]:
"""Attempt to recover from a failed action"""
error_type = self._classify_error(failed_action, failure_result.error)
if error_type in self.error_recovery_strategies:
try:
recovery_func = self.error_recovery_strategies[error_type]
return recovery_func(failed_action, failure_result)
except Exception as e:
return {
"success": False,
"error": f"Recovery failed: {str(e)}"
}
return {
"success": False,
"error": f"No recovery strategy for error type: {error_type}"
}
def _classify_error(self, action: RobotAction, error_message: str) -> str:
"""Classify an error to determine appropriate recovery strategy"""
error_lower = error_message.lower() if error_message else ""
if action.action_type == "move_to":
if "collision" in error_lower or "obstacle" in error_lower:
return "move_failure"
elif "timeout" in error_lower:
return "move_failure"
elif action.action_type == "grasp":
if "failed" in error_lower or "slipped" in error_lower:
return "grasp_failure"
elif action.action_type == "navigate":
if "path" in error_lower or "blocked" in error_lower:
return "navigation_failure"
return "general_failure"
def _recover_from_move_failure(self, action: RobotAction,
failure_result: ActionExecutionResult) -> Dict[str, Any]:
"""Recovery strategy for move failures"""
print(f"Attempting to recover from move failure: {failure_result.error}")
# Try alternative path or position
current_pos = self.robot_state.position
target_pos = action.parameters.get("location", current_pos)
# Try a small offset from target
alternative_pos = [
target_pos[0] + 0.1,
target_pos[1],
target_pos[2]
]
print(f"Trying alternative position: {alternative_pos}")
return {
"success": True,
"action": "try_alternative_position",
"details": f"Attempted recovery by trying position {alternative_pos}"
}
def _recover_from_grasp_failure(self, action: RobotAction,
failure_result: ActionExecutionResult) -> Dict[str, Any]:
"""Recovery strategy for grasp failures"""
print(f"Attempting to recover from grasp failure: {failure_result.error}")
# Try different grasp parameters
current_force = action.parameters.get("force", 10)
new_force = min(current_force + 5, 50) # Increase force slightly
print(f"Retrying with increased force: {new_force}")
return {
"success": True,
"action": "increase_grasp_force",
"details": f"Attempted recovery by increasing grasp force to {new_force}"
}
def _recover_from_navigation_failure(self, action: RobotAction,
failure_result: ActionExecutionResult) -> Dict[str, Any]:
"""Recovery strategy for navigation failures"""
print(f"Attempting to recover from navigation failure: {failure_result.error}")
# Request environment update and replanning
print("Requesting updated environment map for replanning")
return {
"success": True,
"action": "request_environment_update",
"details": "Requested updated environment information for replanning"
}
def _recover_from_communication_error(self, action: RobotAction,
failure_result: ActionExecutionResult) -> Dict[str, Any]:
"""Recovery strategy for communication errors"""
print(f"Attempting to recover from communication error: {failure_result.error}")
# Try to reestablish connection
print("Attempting to reestablish communication with robot")
return {
"success": True,
"action": "reconnect_robot",
"details": "Attempted to reestablish connection with robot"
}
# Example usage
recovery_engine = RecoveryActionEngine()
recovery_engine.start_engine()
# Submit actions including one that might fail
actions = [
RobotAction("move_to", {"location": [1.0, 1.0, 0.0]}, "Move to [1,1,0]"),
RobotAction("grasp", {"object": "red_cup", "force": 5}, "Grasp with low force"),
]
for action in actions:
action_id = recovery_engine.submit_action(action)
print(f"Submitted: {action_id} - {action.description}")
time.sleep(15) # Wait for actions to complete
recovery_engine.stop_engine()
6. Complete VLA Action Execution System
class VLAActionExecutionSystem:
def __init__(self, api_key: str = None):
"""
Complete VLA action execution system integrating with LLM planning
Args:
api_key: OpenAI API key for LLM integration (optional)
"""
self.action_engine = RecoveryActionEngine()
self.vision_context = {}
self.llm_client = None
self.command_history = []
if api_key:
import openai
self.llm_client = openai.OpenAI(api_key=api_key)
def start_system(self):
"""Start the VLA action execution system"""
self.action_engine.start_engine()
print("VLA Action Execution System started")
def stop_system(self):
"""Stop the VLA action execution system"""
self.action_engine.stop_engine()
print("VLA Action Execution System stopped")
def execute_command(self, natural_language_command: str,
vision_context: Dict[str, Any] = None) -> Dict[str, Any]:
"""
Execute a natural language command through the VLA pipeline
Args:
natural_language_command: Command in natural language
vision_context: Current visual context from environment
Returns:
Dict: Execution results
"""
if vision_context:
self.vision_context = vision_context
try:
# Step 1: Plan actions using LLM (simulated here)
plan = self._plan_actions(natural_language_command, vision_context)
# Step 2: Execute the plan
execution_results = []
for action in plan:
action_id = self.action_engine.submit_action(action)
# Wait for action completion (in practice, you'd have a better async approach)
time.sleep(action.duration_estimate + 1.0) # Add buffer time
# Get action status
status = self.action_engine.get_robot_state() # Simplified for example
execution_results.append({
"action_id": action_id,
"action": action.description,
"status": "completed", # In practice, check actual status
"timestamp": time.time()
})
# Record command execution
self.command_history.append({
"command": natural_language_command,
"plan": [a.description for a in plan],
"results": execution_results,
"timestamp": time.time()
})
return {
"success": True,
"command": natural_language_command,
"plan": [a.description for a in plan],
"execution_results": execution_results,
"robot_state": self.action_engine.get_robot_state().__dict__,
"timestamp": time.time()
}
except Exception as e:
return {
"success": False,
"command": natural_language_command,
"error": str(e),
"timestamp": time.time()
}
def _plan_actions(self, command: str, vision_context: Dict[str, Any]) -> List[RobotAction]:
"""
Plan actions based on natural language command and vision context
In a real system, this would call an LLM planner
"""
# This is a simplified planner - in practice, you'd use an LLM-based planner
if "pick up" in command.lower() or "grasp" in command.lower():
# Extract object info from vision context or command
target_object = self._extract_target_object(command, vision_context)
if target_object:
object_info = self._find_object_in_vision(target_object, vision_context)
if object_info:
return [
RobotAction(
"move_to",
{"location": [object_info["position"][0], object_info["position"][1], 0.0]},
f"Move to {target_object} location"
),
RobotAction(
"grasp",
{"object": target_object, "position": object_info["position"]},
f"Grasp the {target_object}"
)
]
elif "move to" in command.lower() or "go to" in command.lower():
# Extract location info
target_location = self._extract_target_location(command, vision_context)
if target_location:
return [
RobotAction(
"move_to",
{"location": target_location},
f"Move to {target_location}"
)
]
elif "detect" in command.lower() or "find" in command.lower():
target_object = self._extract_target_object(command, vision_context)
return [
RobotAction(
"detect_object",
{"object_type": target_object},
f"Detect {target_object}"
)
]
# Default fallback
return [
RobotAction(
"speak",
{"text": f"I don't understand the command: {command}"},
f"Speak command misunderstanding: {command}"
)
]
def _extract_target_object(self, command: str, vision_context: Dict[str, Any]) -> str:
"""Extract target object from command"""
# Simple keyword matching
command_lower = command.lower()
object_keywords = ["cup", "bottle", "book", "box", "chair", "table", "apple", "water"]
for keyword in object_keywords:
if keyword in command_lower:
return keyword
# If not found in command, look in vision context
if vision_context:
objects = vision_context.get("objects_detected", [])
if objects:
return objects[0].get("name", objects[0].get("type", "object"))
return "object"
def _extract_target_location(self, command: str, vision_context: Dict[str, Any]) -> List[float]:
"""Extract target location from command"""
# Simple location matching
command_lower = command.lower()
# Predefined locations
locations = {
"kitchen": [1.0, 0.0, 0.0],
"living room": [2.0, 1.0, 0.0],
"bedroom": [0.0, 2.0, 0.0],
"office": [1.5, 1.5, 0.0]
}
for location_name, coordinates in locations.items():
if location_name in command_lower:
return coordinates
# If not found in command, use first location from vision context
if vision_context:
named_locations = vision_context.get("named_locations", {})
if named_locations:
return list(named_locations.values())[0]
return [0.0, 0.0, 0.0]
def _find_object_in_vision(self, object_name: str,
vision_context: Dict[str, Any]) -> Dict[str, Any]:
"""Find an object in the vision context"""
if not vision_context:
return None
objects = vision_context.get("objects_detected", [])
for obj in objects:
if (object_name.lower() in obj.get("name", "").lower() or
object_name.lower() in obj.get("type", "").lower()):
return obj
return None
def get_system_status(self) -> Dict[str, Any]:
"""Get the current status of the VLA system"""
return {
"action_engine_status": "running" if hasattr(self.action_engine, 'running') and self.action_engine.running else "stopped",
"robot_state": self.action_engine.get_robot_state().__dict__,
"command_history_length": len(self.command_history),
"recent_commands": self.command_history[-5:], # Last 5 commands
"timestamp": time.time()
}
# Example usage
vla_system = VLAActionExecutionSystem()
vla_system.start_system()
# Simulated vision context
vision_ctx = {
"objects_detected": [
{"name": "red_cup", "type": "cup", "position": [1.0, 0.5, 0.0]},
{"name": "wooden_table", "type": "table", "position": [1.0, 0.6, 0.0]}
],
"named_locations": {
"kitchen_counter": [1.0, 0.5, 0.8]
}
}
# Execute commands
result1 = vla_system.execute_command("Pick up the red cup", vision_ctx)
print(f"Command 1 result: {result1['success']}")
result2 = vla_system.execute_command("Move to the kitchen counter", vision_ctx)
print(f"Command 2 result: {result2['success']}")
# Get system status
status = vla_system.get_system_status()
print(f"System status: {status['action_engine_status']}")
vla_system.stop_system()
These examples demonstrate various aspects of action execution engines for VLA systems:
- Basic Action Engine: Simple action execution with threading
- Safety-Enabled Engine: Action execution with safety validation and monitoring
- Concurrent Engine: Multiple actions executing simultaneously with resource management
- Stateful Engine: Action execution with robot state tracking and management
- Recovery Engine: Action execution with error handling and automatic recovery
- Complete VLA System: Integration of all components into a complete VLA action execution system
Each example builds on the previous ones, showing how to incorporate more sophisticated capabilities like safety validation, concurrent execution, state management, error recovery, and integration with the broader VLA system.