visuomotor-diffusion-policy-engine / diffusion_policy_engine.py
At1uri's picture
Upload folder using huggingface_hub
f375d1c verified
Raw History Blame Contribute Delete
7.4 kB
"""
Visuomotor Diffusion Policy Engine (DDIM Action Chunking + Inverse Kinematics)
===============================================================================
Author: Ramprasad Atluri (At1uri)
Repository: At1uri/visuomotor-diffusion-policy-engine
License: Apache-2.0
Implements conditional Denoising Diffusion Implicit Models (DDIM) over
receding-horizon robotic action sequences with multimodal obstacle avoidance
and Damped Least Squares Inverse Kinematics.
"""
import os
import json
from typing import Dict, Any, List, Optional, Tuple
import numpy as np
class VisuomotorDiffusionPolicyEngine:
"""
Conditional 1D Temporal Diffusion Policy for receding-horizon robotic control.
"""
def __init__(self, model_dir: Optional[str] = None):
self.model_dir = model_dir or os.path.dirname(__file__)
self.config = self._load_config()
self.horizon = self.config.get("prediction_horizon", 16)
self.num_train_steps = self.config.get("num_train_timesteps", 100)
self.alphas_cumprod = self._cosine_schedule(self.num_train_steps)
# 3-Link planar arm segment lengths (meters)
self.link_lengths = np.array([0.45, 0.35, 0.25])
def _load_config(self) -> Dict[str, Any]:
cfg_path = os.path.join(self.model_dir, "config.json")
if os.path.exists(cfg_path):
with open(cfg_path, "r", encoding="utf-8") as f:
return json.load(f)
return {"prediction_horizon": 16, "num_train_timesteps": 100}
@staticmethod
def _cosine_schedule(timesteps: int, s: float = 0.008) -> np.ndarray:
steps = np.linspace(0, timesteps, timesteps + 1)
f_t = np.cos(((steps / timesteps) + s) / (1.0 + s) * np.pi * 0.5) ** 2
alphas_cumprod = f_t / f_t[0]
return np.clip(alphas_cumprod, 1e-4, 0.9999)
def _generate_multimodal_modes(self, start_xy: np.ndarray, goal_xy: np.ndarray, obs_xy: np.ndarray, obs_radius: float) -> Tuple[np.ndarray, np.ndarray]:
"""
Constructs the two collision-free topological action modes (upper & lower bypass)
parameterized by minimum-jerk quintic splines.
"""
s = np.linspace(0.0, 1.0, self.horizon)
tau = 10 * (s ** 3) - 15 * (s ** 4) + 6 * (s ** 5)
diff = goal_xy - start_xy
dist = np.linalg.norm(diff) + 1e-8
normal = np.array([-diff[1], diff[0]]) / dist
clearance_amp = obs_radius + 0.14
base_line = start_xy[None, :] + tau[:, None] * diff[None, :]
lobe = np.sin(np.pi * tau)[:, None] * normal[None, :]
mode_upper = base_line + clearance_amp * lobe
mode_lower = base_line - clearance_amp * lobe
return mode_upper, mode_lower
def sample_action_chunk_ddim(
self,
start_xy: List[float],
goal_xy: List[float],
obs_xy: List[float],
obs_radius: float = 0.12,
num_inference_steps: int = 10,
seed: Optional[int] = 42
) -> Dict[str, Any]:
"""
Executes accelerated deterministic DDIM reverse denoising from a_T ~ N(0, I)
to a collision-free action trajectory a_0 belonging to one of the valid modes.
"""
if seed is not None:
np.random.seed(seed)
start = np.array(start_xy, dtype=np.float64)
goal = np.array(goal_xy, dtype=np.float64)
obs = np.array(obs_xy, dtype=np.float64)
mode_upper, mode_lower = self._generate_multimodal_modes(start, goal, obs, obs_radius)
# Initialize Gaussian noise action chunk a_T ~ N(0, I)
a_t = np.random.normal(0.0, 0.5, size=(self.horizon, 2))
timesteps = np.linspace(self.num_train_steps - 1, 0, num_inference_steps).astype(int)
denoising_history = [a_t.copy()]
for idx, t in enumerate(timesteps):
alpha_bar = self.alphas_cumprod[t]
alpha_bar_prev = self.alphas_cumprod[timesteps[idx + 1]] if idx + 1 < len(timesteps) else 1.0
# Compute multimodal score responsibility (log-sum-exp posterior weights over modes)
scale = np.sqrt(alpha_bar)
var = max(1e-3, 1.0 - alpha_bar)
d_upper = np.sum((a_t - scale * mode_upper) ** 2) / (2.0 * var)
d_lower = np.sum((a_t - scale * mode_lower) ** 2) / (2.0 * var)
min_d = min(d_upper, d_lower)
w_upper = np.exp(-(d_upper - min_d))
w_lower = np.exp(-(d_lower - min_d))
p_upper = w_upper / (w_upper + w_lower + 1e-12)
p_lower = 1.0 - p_upper
# Conditional clean action prediction x0_hat (Tweedie's formula)
x0_hat = p_upper * mode_upper + p_lower * mode_lower
eps_hat = (a_t - np.sqrt(alpha_bar) * x0_hat) / np.sqrt(1.0 - alpha_bar + 1e-8)
# DDIM deterministic update step
a_t = np.sqrt(alpha_bar_prev) * x0_hat + np.sqrt(max(0.0, 1.0 - alpha_bar_prev)) * eps_hat
denoising_history.append(a_t.copy())
# Compute obstacle clearance metrics
clearances = np.linalg.norm(a_t - obs[None, :], axis=1) - obs_radius
min_clearance = float(np.min(clearances))
selected_branch = "upper_bypass" if np.linalg.norm(a_t - mode_upper) < np.linalg.norm(a_t - mode_lower) else "lower_bypass"
return {
"selected_mode": selected_branch,
"ddim_steps": num_inference_steps,
"action_horizon": self.horizon,
"min_obstacle_clearance_m": round(min_clearance, 4),
"collision_free": bool(min_clearance > 0.0),
"terminal_error_m": round(float(np.linalg.norm(a_t[-1] - goal)), 5),
"trajectory_xy": np.round(a_t, 4).tolist()
}
def solve_inverse_kinematics(self, target_xy: List[float], init_angles: Optional[List[float]] = None, max_iter: int = 80, damping: float = 0.05) -> Dict[str, Any]:
"""
Solves 3-DOF planar robotic arm Inverse Kinematics via Damped Least Squares (Levenberg-Marquardt).
"""
target = np.array(target_xy, dtype=np.float64)
q = np.array(init_angles if init_angles is not None else [0.5, -0.8, 0.3], dtype=np.float64)
L = self.link_lengths
for _ in range(max_iter):
a1 = q[0]
a2 = q[0] + q[1]
a3 = q[0] + q[1] + q[2]
# Forward kinematics end-effector position
x = L[0] * np.cos(a1) + L[1] * np.cos(a2) + L[2] * np.cos(a3)
y = L[0] * np.sin(a1) + L[1] * np.sin(a2) + L[2] * np.sin(a3)
err = target - np.array([x, y])
if np.linalg.norm(err) < 1e-4:
break
# Analytical 2x3 Jacobian matrix
j00 = -L[0] * np.sin(a1) - L[1] * np.sin(a2) - L[2] * np.sin(a3)
j01 = -L[1] * np.sin(a2) - L[2] * np.sin(a3)
j02 = -L[2] * np.sin(a3)
j10 = L[0] * np.cos(a1) + L[1] * np.cos(a2) + L[2] * np.cos(a3)
j11 = L[1] * np.cos(a2) + L[2] * np.cos(a3)
j12 = L[2] * np.cos(a3)
J = np.array([[j00, j01, j02], [j10, j11, j12]])
# Damped Least Squares update: dq = J^T (J J^T + lambda^2 I)^-1 err
dq = J.T @ np.linalg.solve(J @ J.T + (damping ** 2) * np.eye(2), err)
q += 0.5 * dq
return {
"joint_angles_rad": [round(float(ang), 4) for ang in q],
"position_error_m": round(float(np.linalg.norm(err)), 5)
}