inital release

Dockerfile

@@ -0,0 +1,44 @@
+FROM python:3.11-slim
+
+# non-interactive apt + wheels only (faster, avoids abuse flags)
+ENV DEBIAN_FRONTEND=noninteractive
+ENV PIP_DISABLE_PIP_VERSION_CHECK=1
+ENV PIP_NO_CACHE_DIR=1
+# ENV PIP_ONLY_BINARY=:all:  # Commented out to allow building ruckig from source
+
+# Install build dependencies for ruckig
+RUN apt-get update && apt-get install -y --no-install-recommends \
+    libgl1-mesa-dev \
+    libglib2.0-0 \
+    git \
+    build-essential \
+    cmake \
+    libeigen3-dev \
+ && rm -rf /var/lib/apt/lists/*
+ 
+RUN apt-get update && apt-get install -y --no-install-recommends \
+    nginx \
+ && rm -rf /var/lib/apt/lists/*
+
+WORKDIR /workspace
+
+# your working versions
+COPY requirements.txt ./
+RUN apt-get update && apt-get install -y --no-install-recommends git && \
+    pip install --upgrade pip &&\
+    pip install --verbose -r requirements.txt
+
+RUN echo ${PWD} && ls -lR
+
+# Copy planning utilities first
+COPY meshcat_shapes.py planning_utils.py ./
+
+# app + nginx config + entrypoint
+COPY app.py robot.py nginx.conf run.sh  ws_bridge.py ./
+RUN chmod +x /workspace/run.sh \
+ && rm -f /etc/nginx/nginx.conf \
+ && ln -s /workspace/nginx.conf /etc/nginx/nginx.conf
+
+EXPOSE 7860
+CMD ["/workspace/run.sh"]
+

README.md

@@ -1,10 +1,60 @@
 ---
 title: Capacity Aware Trajectory Planning
-emoji: 👀
-colorFrom: green
+emoji: 🤖
+colorFrom: blue
 colorTo: green
 sdk: docker
 pinned: false
 ---
 
-Check out the configuration reference at https://huggingface.co/docs/hub/spaces-config-reference
+# Capacity-Aware Trajectory Planning Comparison
+
+This space demonstrates a comparison between **capacity-aware real-time trajectory planning** and **TOPPRA** (Time-Optimal Path Parameterization with Reachability Analysis) for robotic manipulators.
+
+## Features
+
+- 🎲 **Random Trajectory Generation**: Generate random Cartesian trajectories with configurable length
+- ⚙️ **Capacity Scaling**: Test different robot capacity levels (0.1 to 1.0)
+- 👀 **Dual Robot Visualization**: Watch both algorithms execute simultaneously
+  - **Left Robot**: Our capacity-aware approach
+  - **Right Robot**: TOPPRA
+- 📊 **Real-time Comparison Plots**:
+  - Position vs Time
+  - Velocity vs Time (with capacity limits)
+  - Acceleration vs Time (with capacity limits)
+  - Tracking Error comparison
+
+## How to Use
+
+1. **Configure Trajectory**:
+   - Set the trajectory length (0.1m to 1.0m)
+   - Set the robot capacity scale (0.1 to 1.0)
+
+2. **Generate**: Click "Generate Random Trajectory" to create a new trajectory and compute both algorithms
+
+3. **Visualize**: 
+   - View the dual robot animation in the MeshCat viewer
+   - Click "Play Animation" to start the synchronized execution
+   - Examine the comparison plots below
+
+4. **Compare**: The info box shows timing comparisons between the two approaches
+
+## Requirements
+
+To run locally, you need the planning utilities from the catp repository.
+
+
+
+## To run the app locally
+
+1. Clone this repository.
+2. build the Docker image using the provided Dockerfile.
+```
+docker build -t catp .
+```
+3. Run the Docker container
+```
+docker run -p 7860:7860 catp
+```
+4. Open your web browser and navigate to `http://localhost:7860` to access the app.
+

app.py

@@ -0,0 +1,269 @@
+import gradio as gr
+import numpy as np
+from robot import TrajectoryPlanner
+import logging
+import plotly.graph_objects as go
+from plotly.subplots import make_subplots
+logging.basicConfig(level=logging.INFO, format="%(asctime)s %(levelname)s %(message)s")
+import time
+import threading
+
+# Initialize trajectory planner
+try:
+    planner = TrajectoryPlanner()
+    PLANNER_AVAILABLE = True
+except Exception as e:
+    logging.error(f"Failed to initialize trajectory planner: {e}")
+    PLANNER_AVAILABLE = False
+
+animation_running = False
+animation_thread = None
+
+css = """
+.control-panel .gr-slider { 
+    margin-top: 4px !important;
+    margin-bottom: 4px !important;
+}
+.control-panel .gr-form {
+    gap: 6px !important;
+}
+.label-override {
+    font-weight: 500 !important;
+    font-size: 1.1em !important;
+    color: #333 !important;
+}
+"""
+
+def create_comparison_plots(plot_data):
+    """Create comparison plots for the trajectories"""
+    if plot_data is None:
+        return None
+    
+    ours = plot_data['ours']
+    toppra = plot_data['toppra']
+    
+    # Create subplots
+    fig = make_subplots(
+        rows=3, cols=2,
+        subplot_titles=('Position', 'Velocity', 'Acceleration', 'Jerk', 'Position Error', 'Orientation Error'),
+        vertical_spacing=0.12,
+        horizontal_spacing=0.1
+    )
+        
+    # Position
+    fig.add_trace(go.Scatter(x=ours['t'], y=ours['x'], name='Ours', 
+                            line=dict(color='#6C8EBF', width=2), showlegend=False), row=1, col=1)
+    fig.add_trace(go.Scatter(x=toppra['t'], y=toppra['x'], name='TOPPRA', 
+                            line=dict(color='#B85450', width=2), showlegend=False), row=1, col=1)
+    
+    # Velocity
+    fig.add_trace(go.Scatter(x=ours['t'], y=ours['dx'], name='Ours', 
+                            line=dict(color='#6C8EBF', width=2), showlegend=False), row=1, col=2)
+    fig.add_trace(go.Scatter(x=ours['t'], y=ours['dx_max'], name='Ours Limits', 
+                            line=dict(color='#6C8EBF', width=1, dash='dash'), showlegend=False), row=1, col=2)
+    fig.add_trace(go.Scatter(x=toppra['t'], y=toppra['dx'], name='TOPPRA', 
+                            line=dict(color='#B85450', width=2), showlegend=False), row=1, col=2)
+    fig.add_trace(go.Scatter(x=toppra['t'], y=toppra['ds_max'], name='TOPPRA Limits', 
+                            line=dict(color='#B85450', width=1, dash='dash'), showlegend=False), row=1, col=2)
+    
+    # Acceleration
+    fig.add_trace(go.Scatter(x=ours['t'], y=ours['ddx'], name='Ours', 
+                            line=dict(color='#6C8EBF', width=2), showlegend=False), row=2, col=1)
+    fig.add_trace(go.Scatter(x=ours['t'], y=ours['ddx_max'], name='Ours Limits', 
+                            line=dict(color='#6C8EBF', width=1, dash='dash'), showlegend=False), row=2, col=1)
+    fig.add_trace(go.Scatter(x=ours['t'], y=ours['ddx_min'], name='Ours Limits', 
+                            line=dict(color='#6C8EBF', width=1, dash='dash'), showlegend=False), row=2, col=1)
+    fig.add_trace(go.Scatter(x=toppra['t'], y=toppra['ddx'], name='TOPPRA', 
+                            line=dict(color='#B85450', width=2), showlegend=False), row=2, col=1)
+    fig.add_trace(go.Scatter(x=toppra['t'], y=toppra['dds_max'], name='TOPPRA Limits', 
+                            line=dict(color='#B85450', width=1, dash='dash'), showlegend=False), row=2, col=1)
+    fig.add_trace(go.Scatter(x=toppra['t'], y=toppra['dds_min'], name='TOPPRA Limits', 
+                            line=dict(color='#B85450', width=1, dash='dash'), showlegend=False), row=2, col=1)
+    
+    # Jerk
+    fig.add_trace(go.Scatter(x=ours['t'], y=ours['dddx'], name='Ours', 
+                            line=dict(color='#6C8EBF', width=2), showlegend=False), row=2, col=2)
+    fig.add_trace(go.Scatter(x=ours['t'], y=ours['dddx_max'], name='Ours Limits', 
+                            line=dict(color='#6C8EBF', width=1, dash='dash'), showlegend=False), row=2, col=2)
+    fig.add_trace(go.Scatter(x=ours['t'], y=ours['dddx_min'], name='Ours Limits', 
+                            line=dict(color='#6C8EBF', width=1, dash='dash'), showlegend=False), row=2, col=2)
+    fig.add_trace(go.Scatter(x=toppra['t'], y=toppra['dddx'], name='TOPPRA', 
+                            line=dict(color='#B85450', width=2), showlegend=False), row=2, col=2)
+    fig.add_trace(go.Scatter(x=toppra['t'], y=toppra['ddds_max'], name='TOPPRA Limits', 
+                            line=dict(color='#B85450', width=1, dash='dash'), showlegend=False), row=2, col=2)  
+    fig.add_trace(go.Scatter(x=toppra['t'], y=toppra['ddds_min'], name='TOPPRA Limits', 
+                            line=dict(color='#B85450', width=1, dash='dash'), showlegend=False), row=2, col=2)  
+    
+    # Position Error
+    fig.add_trace(go.Scatter(x=ours['t'], y=ours['e_pos'], name='Ours', 
+                            line=dict(color='#6C8EBF', width=2), showlegend=False), row=3, col=1)
+    fig.add_trace(go.Scatter(x=toppra['t'], y=toppra['e_pos'], name='TOPPRA', 
+                            line=dict(color='#B85450', width=2), showlegend=False), row=3, col=1)
+    
+    # Orientation Error 
+    fig.add_trace(go.Scatter(x=ours['t'], y=ours['e_rot'], name='Ours',
+                            line=dict(color='#6C8EBF', width=2, dash='dot'), showlegend=False), row=3, col=2)
+    fig.add_trace(go.Scatter(x=toppra['t'], y=toppra['e_rot'], name='TOPPRA',
+                            line=dict(color='#B85450', width=2, dash='dot'), showlegend=False), row=3, col=2)
+    
+    
+    # Update axes
+    # fig.update_xaxes(title_text="Time [s]", row=1, col=1)
+    # fig.update_xaxes(title_text="Time [s]", row=1, col=2)
+    # fig.update_xaxes(title_text="Time [s]", row=2, col=1)
+    # fig.update_xaxes(title_text="Time [s]", row=2, col=2)
+    fig.update_xaxes(title_text="Time [s]", row=3, col=1)
+    fig.update_xaxes(title_text="Time [s]", row=3, col=2)
+    
+    fig.update_yaxes(title_text="s [m]", row=1, col=1)
+    fig.update_yaxes(title_text="ds [m/s]", row=1, col=2)
+    fig.update_yaxes(title_text="dds [m/s²]", row=2, col=1)
+    fig.update_yaxes(title_text="ddds [m/s³]", row=2, col=2)
+    fig.update_yaxes(title_text="Position Error [mm]", row=3, col=1)
+    fig.update_yaxes(title_text="Orientation Error [deg]", row=3, col=2)
+    
+    fig.update_layout(height=600, showlegend=True, legend=dict(x=0.5, y=1.1, orientation='h'))
+    
+    return fig
+
+def animate_trajectory():
+    """Background thread to animate the trajectory"""
+    global animation_running
+    
+    anim_data = planner.get_animation_data()
+    if anim_data is None:
+        return
+    
+    t_max = anim_data['t_max']
+    
+    while animation_running:
+        t0 = time.time()
+        
+        while animation_running and (time.time() - t0) < t_max:
+            t_current = time.time() - t0
+            planner.update_animation(t_current)
+            time.sleep(0.01)
+
+def generate_and_compute(traj_length, capacity_scale, progress=gr.Progress()):
+    """Generate trajectory and compute both algorithms"""
+    if not PLANNER_AVAILABLE:
+        return "Planner not available", None, None
+    
+    progress(0, desc="Generating trajectory...")
+    
+    try:
+        result = planner.generate_trajectory(traj_length, capacity_scale, progress)
+        
+        info = f"""
+        ✅ **Trajectory Generated Successfully**
+        
+        - **Waypoints**: {result['waypoints']}
+        - **Trajectory Length**: {traj_length:.2f} m
+        - **Capacity Scale**: {capacity_scale:.2f}
+        - **TOPPRA Duration**: {result['toppra_duration']:.3f} s
+        - **Ours Duration**: {result['ours_duration']:.3f} s
+        - **Speedup**: {(result['toppra_duration'] / result['ours_duration']):.2f}x
+        """
+        
+        progress(0.8, desc="Creating plots...")
+        
+        plot_data = planner.get_plot_data()
+        plots = create_comparison_plots(plot_data)
+        
+        progress(1.0, desc="Done!")
+        
+        return info, plots, gr.update(interactive=True)
+    
+    except Exception as e:
+        logging.error(f"Error generating trajectory: {e}")
+        return f"❌ Error: {str(e)}", None, gr.update(interactive=False)
+
+def start_animation():
+    """Start the animation"""
+    global animation_running, animation_thread
+    
+    if not PLANNER_AVAILABLE or planner.data is None:
+        return gr.update(value="⚠️ Generate trajectory first")
+    
+    animation_running = True
+    animation_thread = threading.Thread(target=animate_trajectory, daemon=True)
+    animation_thread.start()
+    
+    return gr.update(value="⏸️ Stop Animation", variant="stop")
+
+def stop_animation():
+    """Stop the animation"""
+    global animation_running
+    animation_running = False
+    
+    return gr.update(value="▶️ Play Animation", variant="primary")
+
+def toggle_animation(btn_state):
+    """Toggle animation on/off"""
+    if "Stop" in btn_state:
+        return stop_animation()
+    else:
+        return start_animation()
+
+with gr.Blocks(css=css, title="Capacity-Aware Trajectory Planning") as demo:
+    
+    gr.Markdown("""
+    # 🤖 Capacity-Aware Trajectory Planning Comparison
+    
+    Compare **capacity-aware real-time trajectory planning** vs **TOPPRA** for the Panda robot.
+    
+    **Left Robot**: Our approach | **Right Robot**: TOPPRA
+    """)
+
+    with gr.Row(equal_height=True):
+        # LEFT: MeshCat viewer
+        with gr.Column(scale=2, min_width=500):
+            if PLANNER_AVAILABLE:
+                viewer = gr.HTML(planner.iframe(height=600))
+            else:
+                viewer = gr.HTML("<div style='height:600px; background:#f0f0f0; display:flex; align-items:center; justify-content:center;'><h2>Planner Not Available</h2></div>")
+
+        # RIGHT: Controls
+        with gr.Column(scale=1, min_width=400, elem_classes="control-panel"):
+            gr.Markdown("## 🎛️ Trajectory Configuration")
+            
+            traj_length = gr.Slider(
+                0.1, 1.0, value=0.8, step=0.05,
+                label="Trajectory Length (m)",
+                info="Distance between start and end points"
+            )
+            
+            capacity_scale = gr.Slider(
+                0.1, 1.0, value=0.5, step=0.05,
+                label="Robot Capacity Scale",
+                info="Fraction of maximum robot capacity (1.0 = full capacity)"
+            )
+            
+            generate_btn = gr.Button("🎲 Generate Random Trajectory", variant="primary", size="lg")
+            
+            info_box = gr.Markdown("Click 'Generate' to create a random trajectory")
+            
+            gr.Markdown("## 🎬 Animation Control")
+            
+            play_btn = gr.Button("▶️ Play Animation", variant="primary", interactive=False)
+            
+    with gr.Row():
+        with gr.Column():
+            gr.Markdown("## 📊 Trajectory Comparison")
+            plots = gr.Plot(label="Comparison Plots")
+    
+    # Callbacks
+    generate_btn.click(
+        generate_and_compute,
+        inputs=[traj_length, capacity_scale],
+        outputs=[info_box, plots, play_btn]
+    )
+    
+    play_btn.click(
+        toggle_animation,
+        inputs=[play_btn],
+        outputs=[play_btn]
+    )
+
+if __name__ == "__main__":
+    demo.launch(server_name="0.0.0.0", server_port=8501)

meshcat_shapes.py

@@ -0,0 +1,111 @@
+#!/usr/bin/env python3
+# -*- coding: utf-8 -*-
+#
+# SPDX-License-Identifier: Apache-2.0
+# Copyright 2022 Stéphane Caron
+
+"""Standalone version of meshcat-shapes.
+
+See <https://pypi.org/project/meshcat-shapes/>. We keep this copy in examples/
+so that it can be used by examples that need it without making meshcat-shapes
+(and thus meshcat) a dependency of the project.
+"""
+
+import meshcat
+import numpy as np
+import pinocchio as pin
+import pynocchio as pnc
+
+def __attach_axes(
+    handle: meshcat.Visualizer,
+    length: float = 0.05,
+    thickness: float = 0.002,
+    opacity: float = 1.0,
+) -> None:
+    """Attach a set of three basis axes to a MeshCat handle.
+
+    Args:
+        handle: MeshCat handle to attach the basis axes to.
+        length: Length of axis unit vectors.
+        thickness: Thickness of axis unit vectors.
+        opacity: Opacity of all three unit vectors.
+
+    Note:
+        As per the de-facto standard (Blender, OpenRAVE, RViz, ...), the
+        x-axis is red, the y-axis is green and the z-axis is blue.
+    """
+    direction_names = ["x", "y", "z"]
+    colors = [0xFF0000, 0x00FF00, 0x0000FF]
+    rotation_axes = [[0, 0, 1], [0, 1, 0], [1, 0, 0]]
+    position_cylinder_in_frame = 0.5 * length * np.eye(3)
+    for i in range(3):
+        dir_name = direction_names[i]
+        material = meshcat.geometry.MeshLambertMaterial(
+            color=colors[i], opacity=opacity
+        )
+        transform_cylinder_to_frame = meshcat.transformations.rotation_matrix(
+            np.pi / 2, rotation_axes[i]
+        )
+        transform_cylinder_to_frame[0:3, 3] = position_cylinder_in_frame[i]
+        cylinder = meshcat.geometry.Cylinder(length, thickness)
+        handle[dir_name].set_object(cylinder, material)
+        handle[dir_name].set_transform(transform_cylinder_to_frame)
+
+
+def frame(
+    handle: meshcat.Visualizer,
+    axis_length: float = 0.1,
+    axis_thickness: float = 0.005,
+    opacity: float = 1.0,
+    origin_color: int = 0x000000,
+    origin_radius: float = 0.01,
+) -> None:
+    """Set MeshCat handle to a frame, represented by an origin and three axes.
+
+    Args:
+        handle: MeshCat handle to attach the frame to.
+        axis_length: Length of axis unit vectors, in [m].
+        axis_thickness: Thickness of axis unit vectors, in [m].
+        opacity: Opacity of all three unit vectors.
+        origin_color: Color of the origin sphere.
+        origin_radius: Radius of the frame origin sphere, in [m].
+
+    Note:
+        As per the de-facto standard (Blender, OpenRAVE, RViz, ...), the
+        x-axis is red, the y-axis is green and the z-axis is blue.
+    """
+    material = meshcat.geometry.MeshLambertMaterial(
+        color=origin_color, opacity=opacity
+    )
+    sphere = meshcat.geometry.Sphere(origin_radius)
+    handle.set_object(sphere, material)
+    __attach_axes(handle, axis_length, axis_thickness, opacity)
+    
+    
+def dk(robot, q, tip = None):
+    if tip is None:
+        tip = robot.model.frames[-1].name
+    # joint_id =  robot.model.getFrameId(robot.model.frames[-1].name)
+    joint_id = robot.model.getFrameId(tip)
+    pin.framesForwardKinematics(robot.model, robot.data, q)
+    return robot.data.oMf[robot.model.getFrameId(tip)].translation.copy(), robot.data.oMf[robot.model.getFrameId(tip)].rotation
+
+def display_frame(viz, name, X_frame):
+    frame(viz.viewer[name], opacity=0.5)
+    viz.viewer[name].set_transform(X_frame)
+    
+def display(viz, robot, tip, name, q, T_shift = np.eye(4)):
+    if type(robot) == pnc.RobotWrapper:
+        robot = robot.robot
+    elif type(robot) == pin.RobotWrapper:
+        pass
+    else:
+        raise ValueError("robot must be of type pin.RobotWrapper or pynocchio.RobotWrapper")
+    if len(q) != robot.nq:
+        q = np.array(list(q)+[0]*(robot.nq-len(q)))
+    viz.display(q)
+    dk(robot, q, tip=tip)
+    
+    
+    
+    display_frame(viz, name, (T_shift@robot.data.oMf[robot.model.getFrameId(tip)]))

nginx.conf

@@ -0,0 +1,50 @@
+worker_processes 1;
+events { worker_connections 1024; }
+
+http {
+  include       mime.types;
+  default_type  application/octet-stream;
+  sendfile      on;
+
+  # helper for ws upgrade header
+  map $http_upgrade $connection_upgrade { default upgrade; '' close; }
+
+  # decide if request at "/" is a websocket upgrade:
+  map $http_upgrade $root_backend {
+    default       http://127.0.0.1:8501;  # normal HTTP -> Gradio
+    "~*upgrade"   http://127.0.0.1:8765;  # WS upgrade -> bridge
+    "~*websocket" http://127.0.0.1:8765;
+  }
+
+  upstream app     { server 127.0.0.1:8501; }  # Gradio
+  upstream meshcat { server 127.0.0.1:7000; }  # MeshCat HTTP (/static) & WS at "/"
+
+  server {
+    listen 7860;
+
+    # MeshCat viewer HTML lives under /static/ — mount at /meshcat/
+    location /meshcat/ {
+      proxy_pass http://meshcat/static/;  # trailing slash matters
+      proxy_set_header Host $host;
+    }
+
+    # MeshCat HTML references /static/... absolutely — forward those too
+    location /static/ {
+      proxy_pass http://meshcat/static/;
+      proxy_set_header Host $host;
+    }
+
+    # ROOT "/":
+    #   - normal HTTP (page loads, XHR, etc.) -> Gradio app
+    #   - WebSocket upgrade (opened by MeshCat JS to ws://HOST:7860/) -> WS bridge
+    location / {
+      proxy_http_version 1.1;
+      proxy_set_header Upgrade $http_upgrade;
+      proxy_set_header Connection $connection_upgrade;
+      proxy_set_header Host $host;
+      proxy_read_timeout 3600s;
+      proxy_send_timeout 3600s;
+      proxy_pass $root_backend;
+    }
+  }
+}

planning_utils.py

@@ -0,0 +1,1118 @@
+import qpsolvers
+import numpy as np
+try:
+    SPATIAL_MATH = True
+    from spatialmath import SE3,SO3, base
+    import spatialmath
+except ImportError:
+    SPATIAL_MATH = False
+    print("spatialmath is not installed")
+    
+import pinocchio as pin
+import time
+
+from cvxopt import matrix
+import cvxopt.glpk
+
+from scipy.optimize import linprog
+from scipy.linalg import block_diag
+
+from pynocchio import RobotWrapper
+
+def solve_lp(c, A_eq, b_eq, x_min, x_max):
+    # scipy linprog
+    # res = linprog(c, A_eq=A_eq, b_eq=b_eq, bounds=np.array([x_min,x_max]).T)
+    # return res.x
+    
+    # glpk linprog
+    c = matrix(c)
+    A = matrix(A_eq)
+    b = matrix(b_eq)
+    G = matrix(np.vstack((-np.identity(len(x_min)),np.identity(len(x_min)))))
+    h = matrix(np.hstack((list(-np.array(x_min)),x_max)))
+    solvers_opt={'tm_lim': 100000, 'msg_lev': 'GLP_MSG_OFF', 'it_lim':10000}
+    res = cvxopt.glpk.lp(c=c,  A=A, b=b, G=G,h=h, options=solvers_opt)
+    return np.array(res[1]).reshape((-1,))
+
+
+def solve_qp(A,s,x_min,x_max, grad = None, reg_w=1e-7, solver=None):
+    
+    if (solver is None) or('cvxopt' in solver ) :
+        A =np.matrix(A)
+        s = np.matrix(s)
+
+        P = A.T@A
+        if grad is not None:
+            grad = np.matrix(grad).reshape(1,-1)
+            q = matrix(-A.T@s + reg_w*grad.T)
+            P = P + np.eye(len(grad))*reg_w
+        else:
+            q = matrix(-A.T@s)
+        P = matrix(P)
+        G = matrix(np.vstack((-np.identity(len(x_max)),np.identity(len(x_min)))))
+        h = matrix(np.hstack((list(-np.array(x_min)),x_max)))
+        return np.array(cvxopt.solvers.qp(P, q, G, h)['x']).flatten()
+    
+    else:
+
+        P = A.T@A
+        if grad is not None:
+            q = -A.T@s + reg_w*grad[:,None]
+            P= P+np.eye(len(grad))*reg_w
+        else:
+            q = -A.T@s
+        G = np.vstack((-np.identity(len(x_max)),np.identity(len(x_min))))
+        h = np.hstack((list(-np.array(x_min)),x_max))
+
+        return qpsolvers.solve_qp(P, q.flatten(), G,h, solver=solver)
+    
+from copy import copy
+from pathlib import Path
+from sys import path
+import time
+
+from ruckig import InputParameter, OutputParameter, Result, Ruckig
+
+class TrajData():
+    name = ''
+    
+    
+def compute_capacity_aware_trajectory(X_init, X_final,q0,  robot=None, robot_pyn=None, lims=[], scale=1.0, options = None , verbose=False):
+    
+    
+    # check the type of the X_init and X_final
+    if type(X_init) == np.ndarray:
+        X_init = pin.SE3(X_init[:3,:3], X_init[:3,3])
+        X_final = pin.SE3(X_final[:3,:3], X_final[:3,3])
+    elif type(X_init) == pin.SE3:
+        pass
+    elif SPATIAL_MATH and (type(X_init) == spatialmath.pose3d.SE3):
+        X_init = pin.SE3(X_init.R, X_init.t)
+        X_final = pin.SE3(X_final.R, X_final.t)
+    else:
+        print("Unknown type of X_init")
+        return
+    
+    if robot_pyn is not None:
+        X_r = robot_pyn.forward(q0)
+        n_dof = robot_pyn.n
+    elif robot is not None:
+        X_r = robot.fkine(q0)
+        n_dof = robot.n
+    else:
+        print("Please provide either robot or robot_pyn")
+        return
+
+
+    if options is not None and 'uptate_current_position' in options.keys():
+        uptate_current_position = options['uptate_current_position']
+    else: 
+        uptate_current_position = False
+    
+    if options is not None and 'qp_form' in options.keys():
+        qp_form = options['qp_form']
+    else: 
+        qp_form = 'acceleration'
+    
+    
+    if options is not None and 'calculate_limits' in options.keys():
+        calculate_limits = options['calculate_limits']
+    else: 
+        calculate_limits = True
+        
+    if options is not None and 'scale_limits' in options.keys():
+        scale_limits = options['scale_limits']
+    else: 
+        scale_limits = True
+
+    if options is not None and 'stop_on_singular' in options.keys():
+        stop_on_singular = options['stop_on_singular']
+    else: 
+        stop_on_singular = False
+    
+    if options is not None and 'downsampling_ratio' in options.keys():
+        n_ruckig = options['downsampling_ratio']
+    else:
+        n_ruckig = 1.0
+
+    if options is not None and 'jerk_scale' in options.keys():
+        jerk_scale = options['jerk_scale']
+    else:
+        jerk_scale =scale
+    
+    if lims is None:
+        print('no limits specified')
+        data = []
+        return
+    else:
+        if scale_limits:
+            dq_max = scale*lims['dq_max'].copy()
+            ddq_max = scale*lims['ddq_max'].copy()
+            dddq_max = jerk_scale*lims['dddq_max'].copy()
+            t_max = scale*lims['t_max'].copy()
+        else:
+            dq_max = lims['dq_max'].copy()
+            ddq_max = lims['ddq_max'].copy()
+            dddq_max = lims['dddq_max'].copy()
+            t_max = lims['t_max'].copy()
+        q_min, q_max = lims['q_min'].copy(), lims['q_max'].copy()
+        dq_min = -dq_max
+        ddq_min = -ddq_max
+        dddq_min = -dddq_max
+        t_min = -t_max
+        
+        # cartesian space
+        dddx_max = scale*lims['dddx_max'].copy()
+        dddx_min = -dddx_max
+        ddx_max = scale*lims['ddx_max'].copy()
+        ddx_min = -ddx_max
+        dx_max = scale*lims['dx_max'].copy()
+        dx_min = -dx_max   
+    
+    if options is not None and 'scaled_qp_limits' in options.keys():
+        scaled_qp_limits = options['scaled_qp_limits']
+    else:
+        scaled_qp_limits = True
+        
+    if options is not None and 'Kp' in options.keys():
+        Kp = options['Kp']
+        Kd = options['Kd']
+        Ki = options['Ki']
+    else:
+        Kp = 170
+        Kd = 40       
+        Ki = 0
+        
+    if options is not None and 'clamp_velocity' in options.keys():
+        clamp_velocity = options['clamp_velocity']
+    else: 
+        clamp_velocity = True
+        
+    if options is not None and 'clamp_min_accel' in options.keys():
+        clamp_min_accel = options['clamp_min_accel']
+    else: 
+        clamp_min_accel = False
+        
+        
+        
+    if options is not None and 'override_acceleration' in options.keys():
+        override_acceleration = options['override_acceleration']
+    else: 
+        override_acceleration = True
+    
+
+    if options is not None and 'use_manip_grad' in options.keys():
+        use_manip_grad = options['use_manip_grad']
+    else:
+        use_manip_grad = False
+    
+    if use_manip_grad and robot is None:
+        print("We need RTB model to calculate the manipulability gradient")
+        return
+
+    if options is not None and 'manip_grad_w' in options.keys():
+        manip_grad_w = options['manip_grad_w']
+    else:
+        manip_grad_w =  50000.0
+
+    data = TrajData()
+    u = np.zeros(6)
+    u[:3] = X_final.translation-X_init.translation
+    d = np.linalg.norm(u)
+    u = u/d
+
+    U,S,V = np.linalg.svd(u[:,None])
+    V2 = U[:,1:].T
+
+    U,S,V = np.linalg.svd(u[:3,None])
+    V23 = U[:,1:].T
+    
+    if options is not None and 'dt' in options.keys():
+        dt = options['dt']
+    else:
+        dt =  0.001
+    # n_ruckig = 10.0
+    dt_ruckig = n_ruckig*dt
+
+    
+    if options is not None and 'Tf' in options.keys():
+        Tf = options['Tf']
+        alpha = Tf/(Tf + dt)
+    else:
+        Tf = 0.00
+        alpha = Tf/(Tf + dt)
+
+    # Create instances: the Ruckig OTG as well as input and output parameters
+    otg = Ruckig(1, dt_ruckig)  # DoFs, control cycle
+    inp = InputParameter(1)
+    out = OutputParameter(1)
+
+    # Set input parameters
+    inp.current_position = [0.0]
+    inp.current_velocity = [0.0]
+    inp.current_acceleration = [0.0]
+
+    inp.target_position = [d]
+    inp.target_velocity = [0.0]
+    inp.target_acceleration = [0.0]
+
+    inp.max_velocity = [dx_max[0]]
+    inp.max_acceleration = [ddx_max[0]]
+    inp.max_jerk = [dddx_max[0]]
+
+    print('\t'.join(['t'] + [str(i) for i in range(otg.degrees_of_freedom)]))
+
+    # Generate the trajectory within the control loop
+    data.qr_list = []
+    data.x_list, data.dx_list, data.ddx_list, data.dddx_list = [], [], [], []
+    data.x_q_list, data.dx_q_list, data.ddx_q_list, data.dddx_q_list = [], [], [], []
+    data.dx_max_list, data.ddx_max_list, data.dddx_max_list = [], [], []
+    data.dx_min_list, data.ddx_min_list, data.dddx_min_list = [], [], []
+    data.e_pos_list_ruckig, data.e_rot_list_ruckig = [], []
+    data.x3d_ruckig = []
+    
+    out_list = []
+    res = Result.Working
+
+    s = time.time()
+
+    if robot_pyn is None:
+        sol = robot.ikine_LM(X_final.np, q0)         # solve IK
+        q_final = sol.q
+        J = robot.jacob0(q_final)
+    else:
+        q_final = robot_pyn.ik(X_final, q0, qlim=True, verbose=False)
+        J = robot_pyn.jacobian(q_final)
+    c = u@J
+    Aeq = V2@J
+    beq = np.zeros(5)
+    c_ext = np.hstack((c,c,c,-c))
+    Aeq_ext=block_diag(Aeq,Aeq,Aeq,Aeq)
+    beq_ext = b_eq=np.hstack((beq,beq,beq,beq))
+    x_min_ext = np.hstack((dq_min,ddq_min,dddq_min,ddq_min))
+    x_max_ext = np.hstack((dq_max,ddq_max,dddq_max,ddq_max))
+    q_ext = solve_lp(-c_ext, Aeq_ext, beq_ext, x_min_ext, x_max_ext)
+    ds_final = (c@q_ext[:n_dof])
+    dds_final = (c@q_ext[n_dof:(2*n_dof)])
+    ddds_final = (c@q_ext[(2*n_dof):(3*n_dof)])
+    dds_min_final = (c@q_ext[(3*n_dof):(4*n_dof)])
+
+
+    # bias compensaiton
+    db_old, dds_min_old, dds_max_old = 0, 0, 0
+    alpha_ds = 1.0
+     
+    err_sum = 0
+        
+    data.solved= True
+    
+    ruckig_current_accel, old_accel = [0], 0
+    
+    
+    if robot_pyn is None:
+        sol = robot.ikine_LM(X_init.np, q0)         # solve IK
+        q_c = sol.q
+    else:
+        q_c = robot_pyn.ik(X_init, q0, qlim=True, verbose=False)
+    
+    dq_c, ddq_c = np.zeros(7), np.zeros(7)
+    dddq_c = np.zeros(7)
+    t_sim = 0
+    t_sim_ruckig, t_old_ruckig = 0, -1
+    out_position = 0.0
+    out_velocity = 0.0
+    out_acceleration = 0.0
+    beq_filt = np.zeros(5)
+    t0 = time.time()
+    while res == Result.Working :# or data.dx_q_list[-1] > 1e-2:
+
+        if t_sim > 10000: # more than 10sec
+            print("ERROR: 10 sec simulation time exceeded! Stopping!")
+            data.solved = False
+            break
+        
+        if robot_pyn is None:
+            J = robot.jacob0(q_c)
+            X_c = robot.fkine(q_c)
+            X_c = pin.SE3(X_c.R, X_c.t)
+            J_dot = robot.jacob0_dot(q_c, dq_c)
+        else:
+            J = robot_pyn.jacobian(q_c)
+            J_dot = robot_pyn.jacobian_dot(q_c, dq_c)
+            X_c = robot_pyn.forward(q_c)
+
+
+        # u[:3] = X_final.translation-X_c.translation
+        # d = np.linalg.norm(u)
+        # u = u/d
+    
+        # U,S,V = np.linalg.svd(u[:,None])
+        # V2 = U[:,1:].T
+    
+        # U,S,V = np.linalg.svd(u[:3,None])
+        # V23 = U[:,1:].T
+        
+        data.qr_list.append(q_c)
+        data.x_q_list.append(u[:3]@(X_c.translation-X_init.translation))  
+        data.x3d_ruckig.append(X_c.translation)
+        data.dx_q_list.append(u@J@dq_c)     
+        data.ddx_q_list.append(u@J@ddq_c + u@J_dot@dq_c)   
+        data.dddx_q_list.append(u@J@dddq_c + u@J_dot@ddq_c) 
+
+
+        # data.e_pos_list_ruckig.append(np.linalg.norm(V23@(X_c.translation-X_init.translation)))
+        # data.e_rot_list_ruckig.append(np.linalg.norm(pin.log3(X_init.R.T@X_c.R)))
+
+
+        if t_old_ruckig != t_sim_ruckig:
+            if len(data.x_list) == 0:
+                delta_x = (inp.current_position[0] )/dt_ruckig
+                delta_dx = (inp.current_velocity[0])/dt_ruckig
+                delta_ddx = (inp.current_acceleration[0])/dt_ruckig
+
+                data.x_list.append([delta_x*dt])#+delta_dx*dt**2/2+delta_ddx*dt**2/6])
+                data.dx_list.append([delta_dx*dt])#+delta_ddx*dt**2/2])
+                data.ddx_list.append([delta_ddx*dt])
+                data.dddx_list.append([delta_ddx])
+            else:
+
+                delta_x = (inp.current_position[0] - data.x_list[-1][0])/dt_ruckig
+                delta_dx = (inp.current_velocity[0] - data.dx_list[-1][0])/dt_ruckig
+                delta_ddx = (inp.current_acceleration[0] - data.ddx_list[-1][0])/dt_ruckig
+                data.x_list.append([data.x_list[-1][0] + delta_x*dt])
+                data.dx_list.append([data.dx_list[-1][0]+ delta_dx*dt])
+                data.ddx_list.append([data.ddx_list[-1][0] + delta_ddx*dt])
+                data.dddx_list.append([delta_ddx])
+
+            out_position = inp.current_position[0]
+            out_velocity = inp.current_velocity[0]
+            out_acceleration = inp.current_acceleration[0]
+
+            t_old_ruckig = t_sim_ruckig
+        else:
+
+            data.x_list.append([data.x_list[-1][0] + delta_x*dt])
+            data.dx_list.append([data.dx_list[-1][0] + delta_dx*dt])
+            data.ddx_list.append([data.ddx_list[-1][0] + delta_ddx*dt])  
+            data.dddx_list.append([delta_ddx])
+
+        if t_sim % n_ruckig == 0:
+            if ds_final > 1e-1: ds_p=[ds_final]
+            else: ds_p= []
+            if dds_final > 1e-1: dds_p=[dds_final]
+            else: dds_p= []
+            if ddds_final > 1e-1: ddds_p=[ddds_final]
+            else: ddds_p= []
+            if dds_min_final < -1e-1: dds_min_p=[dds_min_final]
+            else: dds_min_p= []
+
+            # print(ds_p,dds_p,ddds_p)
+            c = u@J
+            Aeq = V2@J
+            try:
+                c_ext = np.hstack((c,c,c,-c))
+                Aeq_ext=block_diag(Aeq,Aeq,Aeq,Aeq)
+                
+                #beq_filt = alpha*beq_filt  + (1 - alpha)*-V2@J_dot@dq_c
+                beq_filt = -V2@J_dot@dq_c
+
+                # alpha_ds = 1.0
+                # db = u@J_dot@dq_c
+                # ddb = (db - db_old) # gradient of the bias term
+                # dds_min = c@dddq_min
+                # if ddb:
+                #     dt_dds_min_to_zero = dds_min/ddb
+                # else:
+                #     dt_dds_min_to_zero = 1.0
+                # if dt_dds_min_to_zero < 0 and dt_dds_min_to_zero >- 1:
+                #     alpha_ds = 1.0 - np.abs(dt_dds_min_to_zero)
+                # else:
+                #     alpha_ds = 1.0
+                #     print(alpha_ds)
+                # db_old = u@J_dot@dq_c
+
+                # dds_max = c@dddq_max
+                # if ddb:
+                #     dt_dds_max_to_zero = dds_p[-1]/np.abs(ddb)
+                # else:
+                #     dt_dds_max_to_zero = 1.0
+                # if ddb < 0 and dt_dds_max_to_zero > 0:# and dt_dds_max_to_zero < 10:
+                #     alpha_ds = np.min([alpha_ds, (dt_dds_max_to_zero)/1000.0])
+                #     print(alpha_ds, dt_dds_max_to_zero)
+                # else:
+                #     alpha_ds = np.min([alpha_ds, 1.0])
+                
+                beq_ext = np.hstack((beq,-V2@J_dot@dq_c,-V2@J_dot@ddq_c,beq_filt))
+                #beq_ext = np.hstack((beq,beq,beq,beq))
+                x_min_ext = np.hstack((alpha_ds*dq_min,ddq_min,dddq_min,ddq_min))
+                x_max_ext = np.hstack((alpha_ds*dq_max,ddq_max,dddq_max,ddq_max))
+                q_ext = solve_lp(-c_ext, Aeq_ext, beq_ext, x_min_ext, x_max_ext)
+                ds_p.append(c@q_ext[:n_dof])
+                dds_p.append(c@q_ext[n_dof:(2*n_dof)] + u@J_dot@dq_c)
+                ddds_p.append(c@q_ext[(2*n_dof):(3*n_dof)] + u@J_dot@ddq_c)
+                dds_min_p.append(c@q_ext[(3*n_dof):(4*n_dof)] + u@J_dot@dq_c)
+
+
+                dt_to_zero = 1.0
+                
+                # dds_min_grad = dds_min_p[-1] - dds_min_old
+                # if dds_min_grad :
+                #     dt_dds_min_to_zero = np.abs(dds_p[-1])/np.abs(dds_min_grad)
+                # else:
+                #     dt_dds_min_to_zero = 10000.0
+                # if dds_min_grad > 0 and dt_dds_min_to_zero > 0 and dt_dds_min_to_zero < dt_to_zero:
+                #     alpha_ds = np.min([alpha_ds, 1-(dt_dds_min_to_zero)/dt_to_zero])
+                # else:
+                #     alpha_ds = np.min([alpha_ds, 1.0])
+                    
+                # dds_max_grad = (dds_p[-1] - dds_max_old)/0.001*dt_to_zero
+                # # alpha_ds_new = 1.0
+                # if dds_max_grad :
+                #     dt_dds_max_to_zero = dds_p[-1]/np.abs(dds_max_grad)
+                # else:
+                #     dt_dds_max_to_zero = 10000.0
+                # if dds_p[-1] + dds_max_grad < 0 or dds_p[-1] < 0:    
+                #     # alpha_ds_new = np.min([alpha_ds_new, np.max([0.01, 0.1])])
+                #     alpha_ds = alpha_ds-0.05
+                # else:
+                #     # alpha_ds_new = np.min([alpha_ds_new, 1.0])
+                #     alpha_ds = alpha_ds+0.005
+                
+                # # alpha = 0.5
+                # # alpha_ds = (1-alpha)* alpha_ds + alpha*alpha_ds_new          
+                # alpha_ds = np.clip(alpha_ds, 0.01, 1)
+                
+            except:
+                if verbose: print("except dds")
+                # return
+                beq = np.zeros(5)
+                # ds_p.append(c@solve_lp(-c, A_eq=Aeq, b_eq=np.zeros(5), x_min=dq_min, x_max=dq_max))
+                # dds_p.append(c@solve_lp(-c, A_eq=Aeq, b_eq=np.zeros(5), x_min=ddq_min, x_max=ddq_max) + u@J_dot@dq_c)
+                # ddds_p.append(c@solve_lp(-c, A_eq=Aeq, b_eq=np.zeros(5), x_min=dddq_min, x_max=dddq_max) + u@J_dot@ddq_c)
+                # dds_min_p.append(c@solve_lp(c, A_eq=Aeq, b_eq=np.zeros(5), x_min=ddq_min, x_max=ddq_max) + u@J_dot@dq_c)
+                
+                # ds_p.append(ds_p[-1])
+                # dds_p.append(dds_p[-1])
+                # ddds_p.append(ddds_p[-1])
+                # dds_min_p.append(dds_min_p[-1])
+
+                # beq_filt = alpha*beq_filt  + (1 - alpha)*-V2@J_dot@dq_c
+                beq_filt = -V2@J_dot@dq_c
+                try:
+                    ds_p.append(c@solve_lp(-c, A_eq=Aeq, b_eq=beq, x_min=dq_min, x_max=dq_max))
+                except:
+                    ds_p.append(ds_p[-1])
+                try:
+                    dds_p.append(c@solve_lp(-c, A_eq=Aeq, b_eq=-V2@J_dot@dq_c, x_min=ddq_min, x_max=ddq_max) + u@J_dot@dq_c)
+                except:
+                    done = False
+                    for i in range(1,10):
+                        try:
+                            dds_p.append(c@solve_lp(-c, A_eq=Aeq, b_eq=-V2@J_dot@dq_c/i, x_min=ddq_min, x_max=ddq_max) + u@J_dot@dq_c)
+                            done = True   
+                            break 
+                        except:
+                            continue
+                    if not done:
+                        dds_p.append(dds_p[-1])
+                try:
+                   ddds_p.append(c@solve_lp(-c, A_eq=Aeq, b_eq=-V2@J_dot@ddq_c, x_min=dddq_min, x_max=dddq_max) + u@J_dot@ddq_c)
+                except:
+                    ddds_p.append(ddds_p[-1])
+                try:
+                    dds_min_p.append(c@solve_lp(c, A_eq=Aeq, b_eq=beq_filt, x_min=ddq_min, x_max=ddq_max) + u@J_dot@dq_c)
+                except:
+                    done = False
+                    for i in range(1,10):
+                        try:
+                            dds_min_p.append(c@solve_lp(c, A_eq=Aeq, b_eq=beq_filt/i, x_min=ddq_min, x_max=ddq_max) + u@J_dot@dq_c)
+                            done = True   
+                            break
+                        except:
+                            continue
+                    if not done:
+                        dds_min_p.append(dds_min_p[-1])
+
+                
+        # if len(data.dx_max_list):
+        #     ds_p[-1] = 0.85*data.dx_max_list[-1] + 0.15*ds_p[-1]
+        # if len(data.ddx_max_list) :
+        #     dds_p[-1] = 0.85*data.ddx_max_list[-1] + 0.15*dds_p[-1]
+        # if len(data.ddx_min_list):
+        #     dds_min_p[-1] = 0.85*data.ddx_min_list[-1] + 0.15*dds_min_p[-1]
+            
+        data.dx_max_list.append(ds_p[-1])
+        data.ddx_max_list.append(dds_p[-1])
+        data.dddx_max_list.append(ddds_p[-1])
+        data.dx_min_list.append(-ds_p[-1])
+        data.ddx_min_list.append(dds_min_p[-1])
+        data.dddx_min_list.append(-ddds_p[-1])
+
+        if ds_p[-1]<=1e-4 :
+            if verbose: print("Error - sinularity or infeasible position for ds")
+            data = None 
+            return
+        if dds_p[-1]<=1e-4: 
+            if verbose: print("Error - sinularity or infeasible position for dds")
+            if stop_on_singular:
+                data = None 
+                return
+            else:
+                dds_p[-1] = 0.05
+        if ddds_p[-1]<=1e-4:
+            if verbose: print("Error - sinularity or infeasible position for ddds")
+            if stop_on_singular:
+                data = None 
+                return
+            else:
+                ddds_p[-1] = 0.05
+                
+        if dds_min_p[-1]>=-1e-4: 
+            if verbose: print("Error - sinularity or infeasible position for dds_min")
+            if stop_on_singular:
+                data = None 
+                return
+            else:
+                dds_min_p[-1] = -0.05
+
+
+        if uptate_current_position:
+            # if current position is less than 2cm from the target one
+            # update the current position
+            # else dont update
+            if np.abs(data.x_q_list[-1] - inp.target_position[0]) > 0.01:
+                inp.current_position = [data.x_q_list[-1]]
+            
+        if t_sim % n_ruckig == 0:
+        
+                     
+            tmp_max_vel =  inp.max_velocity[0]
+            tmp_current_vel =  inp.current_velocity[0]
+            
+            if calculate_limits:
+                if override_acceleration:
+                    inp.current_acceleration = ruckig_current_accel
+            
+            clamped = False
+            if calculate_limits:
+                inp.max_velocity =[(ds_p[-1])]
+                inp.max_acceleration=[(dds_p[-1])]
+                inp.max_jerk=[(ddds_p[-1])]
+                if clamp_min_accel:
+                    inp.min_acceleration=[np.max(dds_min_p)]
+                else:
+                    inp.min_acceleration=[(dds_min_p[-1])]
+            
+                if clamp_velocity:
+                    if inp.current_velocity[0] > inp.max_velocity[0]:
+                        clamped = True
+                        inp.current_velocity = inp.max_velocity
+
+            res = otg.update(inp, out)
+            out.pass_to_input(inp)
+            # print(inp.current_acceleration)
+        
+            if calculate_limits:
+                if clamped:
+                    inp.current_acceleration = [np.max([(inp.max_velocity[0] - tmp_max_vel)/dt_ruckig, inp.min_acceleration[0]])]
+                    
+                
+                if override_acceleration:
+                    ruckig_current_accel = inp.current_acceleration
+                    inp.current_acceleration = [(inp.current_velocity[0] - tmp_current_vel)/dt_ruckig]
+
+            inp.current_acceleration = [(1-alpha)*inp.current_acceleration[0] + alpha*old_accel]
+            old_accel = inp.current_acceleration[0]
+            t_sim_ruckig = t_sim_ruckig + out.time;
+
+
+        if 'acceleration' in qp_form:
+            t_h = 0.01
+            if not scaled_qp_limits:
+                ddq_ub = np.vstack(
+                    [ 
+                        lims['ddq_max'],
+                        (t_h*lims['dddq_max'] + ddq_c).flatten(), 
+                        ((lims['dq_max'] - dq_c)/t_h).flatten(), 
+                        (2*(lims['q_max'] - dq_c*t_h - q_c)/t_h**2).flatten()
+                    ]).min(axis=0)
+                ddq_lb = np.vstack(
+                    [ 
+                        -lims['ddq_max'], 
+                        (t_h*-lims['dddq_max'] + ddq_c).flatten(), 
+                        ((-lims['dq_max'] - dq_c)/t_h).flatten(), 
+                        (2*(lims['q_min'] - dq_c*t_h - q_c)/t_h**2).flatten()
+                    ]).max(axis=0)
+            else:
+                ddq_ub = np.vstack(
+                    [ 
+                        ddq_max, 
+                        (t_h*dddq_max + ddq_c).flatten(), 
+                        ((dq_max - dq_c)/t_h).flatten(), 
+                        (2*(q_max - dq_c*t_h - q_c)/t_h**2).flatten()
+                    ]).min(axis=0)
+                ddq_lb = np.vstack(
+                    [ 
+                        ddq_min, 
+                        (t_h*dddq_min + ddq_c).flatten(), 
+                        ((dq_min - dq_c)/t_h).flatten(), 
+                        (2*(q_min - dq_c*t_h - q_c)/t_h**2).flatten()
+                    ]).max(axis=0)
+        elif 'velocity' in qp_form:
+            t_h = dt
+            if not scaled_qp_limits:
+                dq_ub = np.vstack(
+                    [ 
+                        lims['dq_max'], 
+                        (t_h*lims['ddq_max'] + dq_c).flatten(), 
+                        ((lims['q_max'] - q_c)/t_h).flatten(), 
+                        (0.5*((t_h*5)**2)*lims['dddq_max'] + t_h*ddq_c + dq_c).flatten()
+                    ]
+                ).min(axis=0)
+                dq_lb = np.vstack(
+                    [
+                        -lims['dq_max'], 
+                        (t_h*-lims['ddq_max'] + dq_c).flatten(), 
+                        ((lims['q_min'] - q_c)/t_h).flatten(), 
+                        ((0.5*((t_h*5)**2)*-lims['dddq_max'] + t_h*ddq_c + dq_c).flatten())
+                    ]
+                 ).max(axis=0)
+            else:
+                dq_ub = np.vstack(
+                    [ 
+                        dq_max, 
+                        (t_h*ddq_max + dq_c).flatten(), 
+                        ((q_max - q_c)/t_h).flatten(), 
+                        (0.5*((t_h*5)**2)*dddq_max + t_h*ddq_c + dq_c).flatten()
+                    ]
+                ).min(axis=0)
+                dq_lb = np.vstack(
+                    [
+                        dq_min, 
+                        (t_h*ddq_min + dq_c).flatten(), 
+                        ((q_min - q_c)/t_h).flatten(), 
+                        ((0.5*((t_h*5)**2)*dddq_min + t_h*ddq_c + dq_c).flatten())
+                    ]
+                 ).max(axis=0)
+        # print('sc',dq_ub, dq_lb)
+        # J = robot.jacob0(q_c)
+        c = u@J
+        q_c_old = q_c.copy()
+        dq_c_old = dq_c.copy()
+        ddq_c_old = ddq_c.copy()
+
+        if 'acceleration' in qp_form:
+            # acceleration feed-forward + pd
+            ddx_des = data.ddx_list[-1]*u[:,None]
+            ddx_des = ddx_des + Kd*(data.dx_list[-1]*u - J@dq_c_old)[:,None]
+            # # only translation
+            # # ddx_des[:3] = ddx_des[:3] + 70*SE3((np.array(data.x_list[-1]*u[:3]) - (robot.fkine(q_c_old).t - X_init.translation)).log(True)[:3,None]
+            # translation + rotation
+            X_dk = pin.SE3(X_init.rotation, X_init.translation+np.array(data.x_list[-1]*u[:3]))
+            X_wa = pin.SE3(X_init.rotation, np.zeros(3))
+            X_rk = pin.SE3(X_c.rotation, X_c.translation)
+            X_log = X_dk.actInv(X_rk)
+            log_dk = pin.log6(X_log)
+            err = (-(X_wa.toActionMatrix()@log_dk)[:,None])
+            err_sum = err_sum + err*dt
+            ddx_des =  ddx_des + Kp*err + Ki*err_sum
+            ddx_des = ddx_des - (J_dot@dq_c_old)[:,None]
+            grad = (q0-q_c_old) + 2*(-dq_c_old)
+            if use_manip_grad:
+                grad = manip_grad_w*robot.jacobm(q_c).reshape((-1,))
+    
+            ddq_c = solve_qp(J,ddx_des, ddq_lb, ddq_ub, grad=-grad, reg_w=5*0.00001, solver='cvxopt')
+            if ddq_c is None:
+                print("No QP solution found")
+                data.solved= False
+                break
+    
+            dq_c = dq_c + ddq_c*dt
+            q_c = np.clip(dq_c*dt + ddq_c*(dt**2)/2 + q_c, q_min,q_max)
+            dq_c = np.clip(dq_c, dq_min, dq_max)
+            dddq_c = (ddq_c - ddq_c_old)/dt
+
+        elif 'velocity' in qp_form:
+            # translation + rotation
+            dx_des = data.dx_list[-1]*u[:,None]
+            X_dk = pin.SE3(X_init.rotation, X_init.translation+np.array(data.x_list[-1]*u[:3]))
+            X_rk = pin.SE3(X_c.rotation, X_c.translation)
+            X_log = X_dk.actInv(X_rk)
+            log_dk = pin.log6(X_log)
+            dx_des = dx_des + Kp/1000.0*(-(X_dk.toActionMatrix()@log_dk)[:,None])
+            
+            grad = ((q_min+q_max)/2-q_c_old) + 0.1*(-dq_c_old)
+            if use_manip_grad:
+                grad = manip_grad_w*robot.jacobm(q_c).reshape((-1,))
+
+            dq_c = solve_qp(J, dx_des, dq_lb, dq_ub, grad=-grad, reg_w=5*0.00001, solver='quadprog')
+            if dq_c is None:
+                print("No QP solution found")
+                data.solved= False
+                break
+            
+            
+            # robot simulation
+            if np.any(dq_c > dq_max) or np.any(dq_c < dq_min) : 
+                print("vel outside limits")
+            ddq_c = (dq_c - dq_c_old)/dt
+            if np.any(ddq_c > 1.01*ddq_max) or np.any(ddq_c < 1.01*ddq_min) : 
+                #print(ddq_c, ddq_max, ddq_min, ddq_c > ddq_max, ddq_c < ddq_min)
+                print("accel outside limits")
+            dddq_c = (ddq_c - ddq_c_old)/dt
+            if np.any(dddq_c > 1.5*dddq_max) or np.any(dddq_c < 1.5*dddq_min) :
+                #print(dddq_c, dddq_max, dddq_min, dddq_c[dddq_c > dddq_max], dddq_c < dddq_min) 
+                print("jerk outside limits")
+            q_c = np.clip(dq_c_old*dt + ddq_c*(dt**2)/2 + q_c_old, q_min,q_max)
+                
+        
+        t_sim = t_sim+1
+        
+        # data.e_pos_list_ruckig.append(np.linalg.norm(V23@(X_c.t-X_init.translation)))
+        data.e_pos_list_ruckig.append(np.linalg.norm((X_c.translation-X_init.translation-np.array(data.x_list[-1]*u[:3]))))
+        data.e_rot_list_ruckig.append(np.linalg.norm(pin.log3(X_init.rotation.T@X_c.rotation)))
+        
+    print(f'Calculation duration: {time.time()-s} [s]')
+    data.t_ruckig = np.array(range(0,len(data.x_list)))*dt
+    print(f'Trajectory duration: {data.t_ruckig[-1]:0.4f} [s]')
+    return data
+    
+
+
+def rand_num(delta):
+    return np.random.rand()*2*delta - delta
+
+def find_random_poses_with_distance(distance, robot, q0, iterations = 10, joint_limits=True, verify_line = False , n_waypoints = 10 ,angle =np.pi/6):
+    X_r = robot.fkine(q0)
+    found = False
+    while not found:
+        print("searching")
+        X_init = None
+        while X_init is None or np.linalg.norm(robot.fkine(robot.ikine_LM(X_init, q0, joint_limits=joint_limits).q).t-X_init.t) > 1e-5:
+            X_init = SE3(np.random.rand(1,3)*distance-0.5*distance+np.array([0.5,0,0.4]))*SE3(SO3(X_r.R))*SE3(SO3.Rx(rand_num(angle)))*SE3(SO3.Ry(rand_num(angle)))
+
+        X_final = None
+        i = 0
+        while (X_final is None or np.linalg.norm(robot.fkine(robot.ikine_LM(X_final, q0, joint_limits=joint_limits).q).t-X_final.t) > 1e-5) and i <= iterations:
+            print("not attainable final")
+            v= np.random.rand(3)*2-1
+            v = v/np.linalg.norm(v)*distance
+            X_final = SE3(X_init.t + v)*SE3(SO3(X_init.R))
+            i = i+1
+        if i < iterations:
+            found = True
+
+        if verify_line:
+            q_line = [ robot.ikine_LM(X_init,q0,joint_limits=joint_limits).q ]
+            X_i = np.linspace(X_init.t, X_final.t, n_waypoints)
+            print(n_waypoints)
+            for x in X_i[1:]:
+                T = SE3(x)*SE3(SO3(X_init.R))
+                sol = robot.ikine_LM(T,q_line[-1],joint_limits=joint_limits)         # solve IK
+                X_found = robot.fkine(sol.q)
+                if np.linalg.norm(robot.fkine(robot.ikine_LM(X_found, joint_limits=joint_limits).q).t-T.t) > 1e-5:
+                    break
+                q_line.append(sol.q)    
+            
+            if len(q_line) < len(X_i):
+                print("staright line not possible")
+                continue
+    
+    
+    
+    return X_init, X_final, q_line
+
+
+def find_random_poses_with_distance_pinocchio(distance, robot, q0, iterations = 10, joint_limits=True, verify_line = False , n_waypoints = 10,angle =np.pi/6):
+    X_r = robot.forward(q0)
+    found = False
+    while not found:
+        print("searching")
+        X_init = None
+        while X_init is None or np.linalg.norm(robot.forward(robot.ik(X_init, q0, qlim=joint_limits, verbose=False)).translation-X_init.translation) > 1e-3:
+            X_init = pin.SE3(X_r.rotation, (np.random.rand(1,3)*distance-0.5*distance+np.array([0.5,0,0.4])).reshape((3,)))
+            x,y = rand_num(angle),rand_num(angle)
+            X_init = X_init*pin.SE3(pin.rpy.rpyToMatrix(x, y,0), np.zeros(3))
+
+        X_final = None
+        i = 0
+        while (X_final is None or np.linalg.norm(robot.forward(robot.ik(X_final, q0, qlim=joint_limits, verbose=False)).translation-X_final.translation) > 1e-3) and i <= iterations:
+            print("not attainable final")
+            v= np.random.rand(3)*2-1
+            v = v/np.linalg.norm(v)*distance
+            X_final = pin.SE3(X_init.rotation, X_init.translation + v)
+            i = i+1
+            
+        if i < iterations:
+            found = True
+        
+        if verify_line:
+            print("verify straitgh line")
+            q_line = [ robot.ik(X_init,q0,qlim=joint_limits, verbose=False) ]
+            X_i = np.linspace(X_init.translation, X_final.translation, n_waypoints)
+            print(n_waypoints)
+            for x in X_i[1:]:
+                T = pin.SE3(X_init.rotation, x)
+                q = robot.ik(T, q_line[-1], qlim=joint_limits, verbose=False)         # solve IK
+                X_found = robot.forward(q)
+                if np.linalg.norm(robot.forward(robot.ik(X_found, qlim=joint_limits, verbose=False)).translation-T.translation) > 1e-3:
+                    print(robot.forward(robot.ik(X_found, qlim=joint_limits, verbose=False)).translation, T.translation)
+                    break
+                q_line.append(q)    
+            
+            if len(q_line) < len(X_i):
+                print("staright line not possible")
+                found = False
+                continue
+    
+    return X_init, X_final, q_line
+
+
+def simulate_toppra(X_init, X_final, ts, qs, qds, qdds, q0,  lims, scale, robot = None, options=None , robot_pyn = None ):
+    s =  time.time()
+    data = TrajData()
+    
+    print('simulating trajectory')
+    
+    # check the type of the X_init and X_final
+    if type(X_init) == np.ndarray:
+        X_init = pin.SE3(X_init[:3,:3], X_init[:3,3])
+        X_final = pin.SE3(X_final[:3,:3], X_final[:3,3])
+    elif type(X_init) == pin.SE3:
+        pass
+    elif SPATIAL_MATH and (type(X_init) == spatialmath.pose3d.SE3):
+        X_init = pin.SE3(X_init.R, X_init.t)
+        X_final = pin.SE3(X_final.R, X_final.t)
+    else:
+        print("Unknown type of X_init")
+        return
+    
+    u = np.zeros(6)
+    u[:3] = X_final.translation-X_init.translation
+    u = u/np.linalg.norm(u)
+    
+    # limtis calculation
+    dq_max = scale*lims['dq_max']
+    ddq_max = scale*lims['ddq_max']
+    dddq_max = scale*lims['dddq_max']
+    q_min, q_max = lims['q_min'], lims['q_max']
+    dq_min = -dq_max
+    ddq_min = -ddq_max
+    dddq_min = -dddq_max
+    
+    if options is not None and 'calculate_limits' in options.keys():
+        calculate_limits = options['calculate_limits']
+    else: 
+        calculate_limits = True
+        
+        
+    if calculate_limits:
+        U,S,V = np.linalg.svd(u[:,None])
+        V2 = U[:,1:].T
+        
+        data.ds_max_list=[]
+        data.dds_max_list=[]
+        data.ddds_max_list=[]
+        data.ds_min_list=[]
+        data.dds_min_list=[]
+        data.ddds_min_list=[]
+    
+    data.x3d_top = []
+    data.x_top = []
+    data.dx_top = []
+    data.ddx_top = []
+    data.dddx_top = []
+    data.qr_list = []
+    
+    U,S,V = np.linalg.svd(u[:3,None])
+    V23 = U[:,1:].T
+    data.e_pos_list, data.e_rot_list = [], []
+    
+    q_c = qs[0]
+    
+    if robot_pyn is not None:
+        dq_c, ddq_c, dddq_c =  np.zeros(robot_pyn.n), np.zeros(robot_pyn.n), np.zeros(robot_pyn.n)
+    elif robot is not None:
+        dq_c, ddq_c, dddq_c =  np.zeros(robot.n), np.zeros(robot.n), np.zeros(robot.n)
+    else:
+        print("No robot model provided")
+        return
+    
+    t_last = 0
+    for t, q,qd,qdd in zip(ts, qs,qds,qdds):
+        
+        
+        if robot_pyn is not None:
+            X_c = robot_pyn.forward(q_c)
+            X_dc = robot_pyn.forward(q)
+            J =robot_pyn.jacobian(q_c)
+            J_dot = robot_pyn.jacobian_dot(q_c,dq_c)    
+            x_t = X_c.translation
+            data.x3d_top.append(x_t)
+            data.x_top.append(u[:3]@(x_t-X_init.translation))
+            c = u@J
+            c_dot = u@J_dot
+            data.dx_top.append(c@dq_c)
+            data.ddx_top.append(c@ddq_c + c_dot@dq_c)
+            data.dddx_top.append(c@dddq_c+ c_dot@ddq_c)
+            
+            data.qr_list.append(q_c)
+            
+            data.e_pos_list.append(np.linalg.norm((X_c.translation-X_dc.translation)))
+            # data.e_pos_list.append(np.linalg.norm(V23@(X_c.t-X_init.t)))
+            data.e_rot_list.append(np.linalg.norm(pin.log3(X_init.rotation.T@X_c.rotation)))
+            
+        elif robot is not None:
+            X_c = robot.fkine(q_c)
+            X_dc = robot.fkine(q)
+            J =robot.jacob0(q_c)
+            J_dot = robot.jacob0_dot(q_c,dq_c)
+                
+            x_t = X_c.t
+            data.x3d_top.append(x_t)
+            data.x_top.append(u[:3]@(x_t-X_init.translation))
+            c = u@J
+            c_dot = u@J_dot
+            data.dx_top.append(c@dq_c)
+            data.ddx_top.append(c@ddq_c + c_dot@dq_c)
+            data.dddx_top.append(c@dddq_c+ c_dot@ddq_c)
+            
+            data.qr_list.append(q_c)
+            
+            data.e_pos_list.append(np.linalg.norm((X_c.t-X_dc.t)))
+            # data.e_pos_list.append(np.linalg.norm(V23@(X_c.t-X_init.t)))
+            data.e_rot_list.append(np.linalg.norm(pin.log3(X_init.rotation.T@X_c.R)))
+        
+        dt = t - t_last
+        t_last = t
+        if not dt: 
+            dt =0.001
+    
+        # calculate position + limit
+        q_c = q #dq_c*dt + q_c #+ ddq_c*(dt**2)/2 + q_c
+        q_c = np.clip(q_c, q_min, q_max)
+        
+        dq_c_old = dq_c
+        ddq_c_old = ddq_c
+        # calculate velocity + limit
+        # dq_c = qd #dq_c + ddq_c*dt
+        dq_c = qd#np.clip(qd, dt*ddq_min+dq_c, dt*ddq_max+dq_c) 
+        dq_c = np.clip(dq_c, dq_min, dq_max)
+        # limlit jerk
+        ddq_c = qdd#np.clip((dq_c-dq_c_old)/dt, dt*dddq_min+ddq_c, dt*dddq_max+ddq_c) 
+        # limlit acceleration
+        # ddq_c = qdd
+        ddq_c = np.clip(ddq_c, ddq_min, ddq_max) 
+        # calculate jerk
+        dddq_c = (ddq_c - ddq_c_old)/dt
+        
+        
+        
+        if calculate_limits:
+            Aeq = V2@J
+            beq = np.zeros(5)
+            try:
+                data.ds_max_list.append(c@solve_lp(-c, A_eq=Aeq, b_eq=beq, x_min=dq_min, x_max=dq_max))
+                data.dds_max_list.append(c@solve_lp(-c, A_eq=Aeq, b_eq=-V2@J_dot@dq_c, x_min=ddq_min, x_max=ddq_max) + u@J_dot@dq_c)
+                data.ddds_max_list.append(c@solve_lp(-c, A_eq=Aeq, b_eq=-V2@J_dot@ddq_c, x_min=dddq_min, x_max=dddq_max) + u@J_dot@ddq_c)
+                data.ds_min_list.append(c@solve_lp(c, A_eq=Aeq, b_eq=beq, x_min=dq_min, x_max=dq_max))
+                data.dds_min_list.append(c@solve_lp(c, A_eq=Aeq, b_eq=-V2@J_dot@dq_c, x_min=ddq_min, x_max=ddq_max) + u@J_dot@dq_c)
+                data.ddds_min_list.append(c@solve_lp(c, A_eq=Aeq, b_eq=-V2@J_dot@ddq_c, x_min=dddq_min, x_max=dddq_max) + u@J_dot@ddq_c)
+            except:
+                print("except dds")
+                data.ds_max_list.append(c@solve_lp(-c, A_eq=Aeq, b_eq=np.zeros(5), x_min=dq_min, x_max=dq_max))
+                data.dds_max_list.append(c@solve_lp(-c, A_eq=Aeq, b_eq=np.zeros(5), x_min=ddq_min, x_max=ddq_max) + u@J_dot@dq_c)
+                data.ddds_max_list.append(c@solve_lp(-c, A_eq=Aeq, b_eq=np.zeros(5), x_min=dddq_min, x_max=dddq_max) + u@J_dot@ddq_c)
+                data.ds_min_list.append(c@solve_lp(c, A_eq=Aeq, b_eq=np.zeros(5), x_min=dq_min, x_max=dq_max))
+                data.dds_min_list.append(c@solve_lp(c, A_eq=Aeq, b_eq=np.zeros(5), x_min=ddq_min, x_max=ddq_max) + u@J_dot@dq_c)
+                data.ddds_min_list.append(c@solve_lp(c, A_eq=Aeq, b_eq=np.zeros(5), x_min=dddq_min, x_max=dddq_max) + u@J_dot@ddq_c)
+    
+    data.t_toppra = ts
+    print('TOPPRA trajecotry simulation time',time.time() - s)
+    print(f'Trajectory duration: {data.t_toppra[-1]:0.4f} [s]')
+    return data
+
+
+import toppra as ta
+import toppra.constraint as constraint
+import toppra.algorithm as algo
+
+def caclulate_toppra_trajectory(X_init, X_final,  q0, lims, scale, d_waypoint, robot=None, robot_pyn=None, data=None):
+    s = time.time()
+    # ta.setup_logging("INFO")
+    
+    
+    if robot_pyn is not None:
+        # check the type of the X_init and X_final
+        if type(X_init) == np.ndarray:
+            X_init = pin.SE3(X_init[:3,:3], X_init[:3,3])
+            X_final = pin.SE3(X_final[:3,:3], X_final[:3,3])
+        elif type(X_init) == pin.SE3:
+            pass
+        elif SPATIAL_MATH and (type(X_init) == spatialmath.pose3d.SE3):
+            X_init = pin.SE3(X_init.R, X_init.t)
+            X_final = pin.SE3(X_final.R, X_final.t)
+        else:
+            print("Unknown type of X_init")
+            return
+    
+    # limtis calculation
+    dq_max = scale*lims['dq_max']
+    ddq_max = scale*lims['ddq_max']
+    
+    # calculate the waypoints
+    if robot_pyn is not None:
+        d = np.linalg.norm(X_final.translation-X_init.translation)
+    else:
+        d = np.linalg.norm(X_final.t-X_init.t)
+    #number of waypoints in joint space
+    n_waypoints  = int(d/d_waypoint)
+    # print(f'traj length: {d}, number of waypoints: {n_waypoints}')
+    
+    if data is not None:
+        print('using provided data')
+        x_np = np.array(data.x_list)
+        x_v = np.linspace(0,x_np[-1], n_waypoints)
+        inds = []
+        for x_wp in x_v:
+            inds.append(np.where(x_np >= x_wp)[0][0])
+        q_line = [data.qr_list[int(i)]  for i in inds]
+        
+    else:
+        if robot is not None:
+            # print('calculation waypoints')
+            X_i = np.linspace(X_init.t,X_final.t,n_waypoints)
+            q_line = [ robot.ikine_LM(X_init, q0).q ]
+            for x in X_i[1:]:
+                T = SE3(x)*SE3(SO3(X_init.R))
+                sol = robot.ikine_LM(T,q_line[-1])#,joint_limits=true)         # solve IK
+                q_line.append(sol.q)
+        else:
+            # print('calculation waypoints')
+            X_i = np.linspace(X_init.translation,X_final.translation,n_waypoints)
+            q_line = [ robot_pyn.ik(X_init, q0, qlim=True, verbose=False) ]
+            for x in X_i[1:]:
+                T= pin.SE3(X_init.rotation, x)
+                q_line.append(robot_pyn.ik(T,q_line[-1], qlim=True, verbose=False))
+            
+
+        
+    # print("Waypoints calculation time:",time.time()-s)
+    s1 = time.time()
+    # calculate the traectory
+    ss = np.linspace(0,1,len(q_line))
+    path = ta.SplineInterpolator(ss, q_line)
+    pc_vel = constraint.JointVelocityConstraint(dq_max)
+    pc_acc = constraint.JointAccelerationConstraint(ddq_max)
+    instance = algo.TOPPRA([pc_vel, pc_acc], path, parametrizer="ParametrizeConstAccel")
+    jnt_traj = instance.compute_trajectory()
+    
+    ts_sample = np.arange(0, jnt_traj.duration, 0.001)
+    qs_sample = jnt_traj(ts_sample)
+    qds_sample = jnt_traj(ts_sample, 1)
+    qdds_sample = jnt_traj(ts_sample, 2)
+    
+    # print("TOPPRA calculation time:",time.time()-s1)
+    # print("Waypoints+TOPPRA calculation time:",time.time()-s)
+    return ts_sample, qs_sample, qds_sample, qdds_sample
+    # return time.time()-s, time.time()-s1

requirements.txt

@@ -0,0 +1,15 @@
+gradio==5.49.1
+plotly>=5.0.0
+meshcat==0.3.2
+pin==3.3.1
+numpy
+websockets==15.0.1
+
+# Trajectory planning dependencies
+example-robot-data==4.1.0
+qpsolvers==4.4.0
+cvxopt==1.3.2
+scipy==1.14.1
+ruckig==0.14.0
+toppra
+pynocchio @ git+https://github.com/askuric/pynocchio.git

robot.py

@@ -0,0 +1,321 @@
+import numpy as np
+import time
+import sys
+import os
+
+import meshcat
+import meshcat.geometry as g
+import logging
+logging.basicConfig(level=logging.INFO, format="%(asctime)s %(levelname)s %(message)s")
+
+import pinocchio as pin
+from pinocchio.visualize import MeshcatVisualizer
+
+# Add catp to path if it exists
+catp_path = os.path.expanduser("~/gitlab/catp")
+if os.path.exists(catp_path):
+    sys.path.insert(0, catp_path)
+
+try:
+    from example_robot_data import load
+    from pynocchio import RobotWrapper
+    PANDA_AVAILABLE = True
+except ImportError:
+    PANDA_AVAILABLE = False
+    logging.warning("Panda robot libraries not available")
+
+
+class TrajectoryPlanner:
+    """
+    Manages Panda robot with dual trajectory planning visualization.
+    """
+    def __init__(self):
+        if not PANDA_AVAILABLE:
+            raise ImportError("Panda robot libraries not available")
+        
+        self.vis = meshcat.Visualizer(zmq_url="tcp://127.0.0.1:6001")
+        logging.info(f"Zmq URL: {self.vis.window.zmq_url}, web URL: {self.vis.window.web_url}")
+        
+        # Load Panda robot
+        self.panda_pin = load('panda')
+        self.panda_pin.data = self.panda_pin.model.createData()
+        
+        self.panda_tip_pin = "panda_hand_tcp"
+        
+        # Import here to avoid circular dependency
+        from planning_utils import find_random_poses_with_distance_pinocchio
+        from planning_utils import compute_capacity_aware_trajectory, caclulate_toppra_trajectory, simulate_toppra
+        from meshcat_shapes import display_frame as display_frame_shapes
+        from meshcat_shapes import display, display_frame
+        
+        self.find_random_poses = find_random_poses_with_distance_pinocchio
+        self.display = display
+        self.display_frame = display_frame
+        self.compute_capacity_aware_trajectory = compute_capacity_aware_trajectory
+        self.caclulate_toppra_trajectory = caclulate_toppra_trajectory
+        self.simulate_toppra = simulate_toppra
+        
+        try:
+            self.panda_pyn = RobotWrapper(
+                robot_wrapper=self.panda_pin, 
+                tip=self.panda_tip_pin, 
+                open_viewer=False, 
+                start_visualisation=True, 
+                viewer=self.vis, 
+                fix_joints=[
+                    self.panda_pin.model.getJointId("panda_finger_joint1"),
+                    self.panda_pin.model.getJointId("panda_finger_joint2")
+                ]
+            )
+            self.viz = self.panda_pyn.viz
+            self.viz.display_collisions = False
+        except Exception as e:
+            logging.error(f"Error initializing robot: {e}")
+            raise
+        
+        # Setup visualization
+        self.vis["/Background"].set_property("top_color", [1, 1, 1])
+        self.vis["/Background"].set_property("bottom_color", [1, 1, 1])
+        self.vis["/Axes"].set_property("visible", False)
+        self.vis['/Grid'].set_property("visible", True)
+        
+        # Default limits for Panda
+        q_min, q_max = self.panda_pyn.model.lowerPositionLimit, self.panda_pyn.model.upperPositionLimit
+        dq_max = self.panda_pyn.model.velocityLimit
+        ddq_max = np.array([15, 7.5, 10, 12.5, 15, 20, 20])
+        dddq_max = np.array([7500, 3750, 5000, 6250, 7500, 10000, 10000])
+        t_max = np.array([87, 87, 87, 87, 20, 20, 20])
+        
+        dddx_max = np.array([6500.0, 6500.0, 6500.0])
+        ddx_max = np.array([13.0, 13, 13])
+        dx_max = np.array([1.7, 1.7, 1.7])
+        
+        self.q0 = (self.panda_pyn.model.upperPositionLimit + self.panda_pyn.model.lowerPositionLimit) / 2
+        
+        self.limits = {
+            'q_min': q_min, 'q_max': q_max, 
+            'dq_max': dq_max, 'ddq_max': ddq_max, 
+            'dddq_max': dddq_max, 't_max': t_max, 
+            'dx_max': dx_max, 'ddx_max': ddx_max, 
+            'dddx_max': dddx_max
+        }
+        
+        self.data = None
+        self.data_top = None
+        self.ts_sample = None
+        self.qs_sample = None
+        self.X_init = None
+        self.X_final = None
+        self.T_shift = None
+        
+        # Setup dual robot visualization
+        self._setup_dual_robots()
+        
+    def _setup_dual_robots(self):
+        """Setup two robots side by side for comparison"""
+        self.panda_ours = self.panda_pyn.robot
+        self.panda_toppra = self.panda_pyn.robot
+        
+        self.panda_toppra.data = self.panda_toppra.model.createData()
+        
+        self.viz_l = self.panda_pyn.viz
+        self.viz_r = MeshcatVisualizer(
+            self.panda_toppra.model, 
+            self.panda_toppra.collision_model, 
+            self.panda_toppra.visual_model
+        )
+        
+        self.viz_r.initViewer(open=False, viewer=self.vis)
+        self.viz_r.loadViewerModel("toppra", color=[0.0, 0.0, 0.0, 0.5])
+        
+        # Shift right robot
+        self.T_shift = np.eye(4)
+        self.T_shift[1, 3] = 0.5  # 0.5 meter along y
+        self.vis["toppra"].set_transform(self.T_shift)
+                
+        # Update visualizations
+        self.display(self.viz_l, self.panda_ours, self.panda_tip_pin, 
+                    "end_effector_ruc", self.q0)
+        self.display(self.viz_r, self.panda_toppra, self.panda_tip_pin, 
+                    "end_effector_toppra", self.q0, self.T_shift)
+    
+    def generate_trajectory(self, traj_length=0.8, scale=0.5, progress=None):
+        """Generate a random trajectory and compute both algorithms"""
+        logging.info(f"Generating trajectory: length={traj_length}, scale={scale}")
+        
+        n_waypoints = int(traj_length / 0.05)
+        q0 = (self.panda_pyn.model.upperPositionLimit + self.panda_pyn.model.lowerPositionLimit) / 2
+        
+        if progress is not None:
+            progress(0.1, desc="Finding random trajectory...")
+            
+        # Generate random trajectory
+        self.X_init, self.X_final, q_line = self.find_random_poses(
+            robot=self.panda_pyn, 
+            distance=traj_length, 
+            q0=q0, 
+            verify_line=True, 
+            n_waypoints=n_waypoints, 
+            angle=np.pi/2
+        )
+        
+        # Display start and end frames        
+        self.display(self.viz_l, self.panda_ours, self.panda_tip_pin, 
+                    "end_effector_ruc", q_line[0])
+        self.display(self.viz_r, self.panda_toppra, self.panda_tip_pin, 
+                    "end_effector_toppra", q_line[0])
+        self.display_frame(self.viz_l, "start", self.X_init.np)
+        self.display_frame(self.viz_l, "end", self.X_final.np)
+        self.display_frame(self.viz_r, "start1", self.T_shift@self.X_init.np)
+        self.display_frame(self.viz_r, "end1", self.T_shift@self.X_final.np)
+        
+        # Draw straight line between start and end frames
+        line_start = self.X_init.np[:3, 3]
+        line_end = self.X_final.np[:3, 3]
+        line_vertices = np.column_stack([line_start, line_end])
+        
+        self.vis["trajectory_line"].set_object(
+            g.Line(g.PointsGeometry(line_vertices), 
+                g.MeshBasicMaterial(color=0x0000ff, linewidth=2))
+        )
+        
+        # Also draw shifted line for right visualization
+        line_start_shifted = (self.T_shift @ self.X_init.np)[:3, 3]
+        line_end_shifted = (self.T_shift @ self.X_final.np)[:3, 3]
+        line_vertices_shifted = np.column_stack([line_start_shifted, line_end_shifted])
+        
+        self.vis["trajectory_line_toppra"].set_object(
+            g.Line(g.PointsGeometry(line_vertices_shifted),
+                g.MeshBasicMaterial(color=0x0000ff, linewidth=2))
+        )
+        
+        if progress is not None:
+            progress(0.3, desc="Calculating capacity aware trajectory")
+            
+        # Compute trajectories
+        options = {
+            'Kp': 600, 'Kd': 150, 'Ki': 0.0, 'Tf': 0.01,             
+            'uptate_current_position': True, 'clamp_velocity': True, 
+            'clamp_min_accel': True, 'scaled_qp_limits': True,
+            'override_acceleration': True, 'scale_limits': True,
+            'calculate_limits': True, 'downsampling_ratio': 1,
+            'use_manip_grad': False, 'manip_grad_w': 5000.0,
+            'dt': 0.001, 'qp_form': 'acceleration'
+        }
+        
+        # Our approach
+        logging.info("Computing capacity-aware trajectory...")
+        self.data = self.compute_capacity_aware_trajectory(
+            self.X_init, self.X_final, 
+            robot=None, robot_pyn=self.panda_pyn,  
+            lims=self.limits, scale=scale, 
+            options=options, q0=q0
+        )
+        
+        if progress is not None:
+            progress(0.6, desc="Calculating TOPPRA trajectory")
+            
+        # TOPPRA
+        logging.info("Computing TOPPRA trajectory...")
+        self.ts_sample, self.qs_sample, qds_sample, qdds_sample = self.caclulate_toppra_trajectory(
+            self.X_init, self.X_final, 
+            robot_pyn=self.panda_pyn, q0=q0, 
+            d_waypoint=0.05, lims=self.limits, scale=scale
+        )
+        
+        self.data_top = self.simulate_toppra(
+            self.X_init, self.X_final, 
+            self.ts_sample, self.qs_sample, qds_sample, qdds_sample, 
+            q0=q0, robot_pyn=self.panda_pyn, 
+            lims=self.limits, scale=scale, options=options
+        )
+        
+        # Setup initial positions
+        self.display(self.viz_l, self.panda_ours, self.panda_tip_pin, 
+                    "end_effector_ruc", self.data.qr_list[0])
+        self.display(self.viz_r, self.panda_toppra, self.panda_tip_pin, 
+                    "end_effector_toppra", self.data_top.qr_list[0])
+        
+        self.display_frame(self.viz_l, "start", self.X_init.np)
+        self.display_frame(self.viz_l, "end", self.X_final.np)
+        self.display_frame(self.viz_r, "start1", self.T_shift @ self.X_init)
+        self.display_frame(self.viz_r, "end1", self.T_shift @ self.X_final)
+        
+        return {
+            'toppra_duration': float(self.ts_sample[-1]),
+            'ours_duration': float(self.data.t_ruckig[-1]),
+            'waypoints': n_waypoints
+        }
+    
+    def get_animation_data(self):
+        """Get data needed for animation"""
+        if self.data is None or self.data_top is None:
+            return None
+        
+        return {
+            't_max': max(self.data.t_ruckig[-1], self.data_top.t_toppra[-1]),
+            't_ruckig': self.data.t_ruckig,
+            't_toppra': self.data_top.t_toppra,
+            'qr_list': self.data.qr_list,
+            'qs_sample': self.qs_sample,
+        }
+    
+    def update_animation(self, t_current):
+        """Update robot positions for given time"""
+        if self.data is None or self.data_top is None:
+            return
+        
+        # Find indices
+        ind_t = 0
+        while ind_t < len(self.data_top.t_toppra) - 1 and self.data_top.t_toppra[ind_t] <= t_current:
+            ind_t += 1
+        
+        ind_r = 0
+        while ind_r < len(self.data.t_ruckig) - 1 and self.data.t_ruckig[ind_r] <= t_current:
+            ind_r += 1
+        
+        # Update visualizations
+        self.display(self.viz_l, self.panda_ours, self.panda_tip_pin, 
+                    "end_effector_ruc", self.data.qr_list[ind_r])
+        self.display(self.viz_r, self.panda_toppra, self.panda_tip_pin, 
+                    "end_effector_toppra", self.qs_sample[ind_t], self.T_shift)
+    
+    def get_plot_data(self):
+        """Get data for plotting"""
+        if self.data is None or self.data_top is None:
+            return None
+        
+        return {
+            'ours': {
+                't': self.data.t_ruckig[1:],
+                'x': np.array(self.data.x_list[1:]).flatten().tolist(),
+                'dx': self.data.dx_q_list[1:],
+                'ddx': self.data.ddx_q_list[1:],
+                'dddx': self.data.dddx_q_list[1:],
+                'dx_max': self.data.dx_max_list[1:],
+                'ddx_max': self.data.ddx_max_list[1:],
+                'ddx_min': self.data.ddx_min_list[1:],
+                'dddx_max': self.data.dddx_max_list[1:],
+                'dddx_min': self.data.dddx_min_list[1:],
+                'e_pos': (np.array(self.data.e_pos_list_ruckig) * 1e3),
+                'e_rot': (np.rad2deg(np.array(self.data.e_rot_list_ruckig))),
+            },
+            'toppra': {
+                't': self.data_top.t_toppra,
+                'x': self.data_top.x_top,
+                'dx': self.data_top.dx_top,
+                'ddx': self.data_top.ddx_top,
+                'dddx': self.data_top.dddx_top,
+                'ds_max': self.data_top.ds_max_list,
+                'dds_min': self.data_top.dds_min_list,
+                'dds_max': self.data_top.dds_max_list,
+                'ddds_max': self.data_top.ddds_max_list,
+                'ddds_min': self.data_top.ddds_min_list,
+                'e_pos': (np.array(self.data_top.e_pos_list) * 1e3),
+                'e_rot': (np.rad2deg(np.array(self.data_top.e_rot_list))),
+            }
+        }
+    
+    def iframe(self, width="100%", height=640):
+        return f'<iframe src="/static/" style="width:{width};height:{height}px;border:0"></iframe>'
+

run.sh

@@ -0,0 +1,18 @@
+#!/usr/bin/env bash
+set -euo pipefail
+set -x
+
+
+# 1) Start MeshCat server (HTTP on :7000, WS at "/", ZMQ default :6000)
+meshcat-server --zmq-url tcp://127.0.0.1:6001 &
+
+# 2) Start the WS bridge (root "/" on 8765 -> 7000)
+exec python ws_bridge.py &
+
+# 3) Nginx reverse proxy (serves public port 7860)
+nginx
+
+# 4) Start your Gradio app on 8501
+exec python app.py
+# Keep the script running
+# wait

ws_bridge.py

@@ -0,0 +1,53 @@
+# WebSocket bridge: browser <-> bridge (:8765) <-> MeshCat WS (:7000)
+# Compatible with websockets >= 11 (incl. 15.0.1). One-arg handler.
+import asyncio
+import logging
+import websockets
+
+UPSTREAM = "ws://127.0.0.1:7000/"
+
+logging.basicConfig(level=logging.INFO, format="%(asctime)s %(levelname)s %(message)s")
+
+async def pump(src, dst, label):
+    try:
+        async for msg in src:
+            await dst.send(msg)
+    except websockets.ConnectionClosedOK:
+        logging.info("%s closed cleanly", label)
+    except websockets.ConnectionClosed as e:
+        logging.info("%s closed: code=%s reason=%s", label, e.code, e.reason)
+    except Exception:
+        logging.exception("%s error", label)
+
+async def handler(client_ws):
+    # Connect to MeshCat’s WS; disable compression to avoid deflate quirks
+    async with websockets.connect(
+        UPSTREAM,
+        ping_interval=20,
+        ping_timeout=20,
+        max_size=None,
+        compression=None,
+    ) as upstream_ws:
+        logging.info("Bridge connected: client <-> %s", UPSTREAM)
+        # bidirectional forwarding
+        c2u = asyncio.create_task(pump(client_ws, upstream_ws, "client->upstream"))
+        u2c = asyncio.create_task(pump(upstream_ws, client_ws, "upstream->client"))
+        done, pending = await asyncio.wait({c2u, u2c}, return_when=asyncio.FIRST_COMPLETED)
+        for t in pending:
+            t.cancel()
+
+async def main():
+    async with websockets.serve(
+        handler,
+        "127.0.0.1",
+        8765,
+        ping_interval=20,
+        ping_timeout=20,
+        max_size=None,
+        compression=None,
+    ):
+        logging.info("WS bridge listening on ws://127.0.0.1:8765/")
+        await asyncio.Future()  # run forever
+
+if __name__ == "__main__":
+    asyncio.run(main())