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