""" 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) }