Robotics
LeRobot
Safetensors
English
temporal_unet_diffusion_policy
diffusion-policy
embodied-ai
imitation-learning
visuomotor-control
ddim
inverse-kinematics
Eval Results (legacy)
Instructions to use At1uri/visuomotor-diffusion-policy-engine with libraries, inference providers, notebooks, and local apps. Follow these links to get started.
- Libraries
- LeRobot
How to use At1uri/visuomotor-diffusion-policy-engine with LeRobot:
# No code snippets available yet for this library. # To use this model, check the repository files and the library's documentation. # Want to help? PRs adding snippets are welcome at: # https://github.com/huggingface/huggingface.js
- Notebooks
- Google Colab
- Kaggle
File size: 7,404 Bytes
f375d1c | 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109 110 111 112 113 114 115 116 117 118 119 120 121 122 123 124 125 126 127 128 129 130 131 132 133 134 135 136 137 138 139 140 141 142 143 144 145 146 147 148 149 150 151 152 153 154 155 156 157 158 159 160 161 162 163 164 165 166 167 168 | """
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)
}
|