update

README.md

@@ -11,33 +11,10 @@ pinned: false
 
 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
+This space implements the paper: 
+**Online approach to near time-optimal task-space trajectory planning**
+by Antun Skuric, Nicolas Torres Alberto, Lucas Joseph, Vincent Padois, David Daney
+Link: https://inria.hal.science/hal-04576076 
 
 ## Requirements
 

app.py

@@ -52,19 +52,19 @@ def create_comparison_plots(plot_data):
         
     # 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)
+                            line=dict(color='#6C8EBF', width=2), showlegend=True), 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)
+                            line=dict(color='#B85450', width=2), showlegend=True), 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)
+                            line=dict(color='#6C8EBF', width=1, dash='dash'), showlegend=True), 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)
+                            line=dict(color='#B85450', width=1, dash='dash'), showlegend=True), row=1, col=2)
     
     # Acceleration
     fig.add_trace(go.Scatter(x=ours['t'], y=ours['ddx'], name='Ours', 
@@ -102,16 +102,12 @@ def create_comparison_plots(plot_data):
     
     # 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)
+                            line=dict(color='#6C8EBF', width=2), 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)
+                            line=dict(color='#B85450', width=2), 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)
     
@@ -136,13 +132,18 @@ def animate_trajectory():
     
     t_max = anim_data['t_max']
     
-    while animation_running:
+    for i in range(10):
+        if not animation_running:
+            return
         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)
+            
+    animation_running = False  
+    
 
 def generate_and_compute(traj_length, capacity_scale, progress=gr.Progress()):
     """Generate trajectory and compute both algorithms"""
@@ -151,32 +152,32 @@ def generate_and_compute(traj_length, capacity_scale, progress=gr.Progress()):
     
     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)
+    # 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)
+    #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"""

planning_utils.py

@@ -892,6 +892,7 @@ def simulate_toppra(X_init, X_final, ts, qs, qds, qdds, q0,  lims, scale, robot
     u = u/np.linalg.norm(u)
     
     # limtis calculation
+    scale = float(scale)
     dq_max = scale*lims['dq_max']
     ddq_max = scale*lims['ddq_max']
     dddq_max = scale*lims['dddq_max']
@@ -1020,13 +1021,21 @@ def simulate_toppra(X_init, X_final, ts, qs, qds, qdds, q0,  lims, scale, robot
                 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)
-    
+                try:
+                    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)
+                except:
+                    print("except dds 2")
+                    data.ds_max_list.append(data.ds_max_list[-1])
+                    data.dds_max_list.append(data.dds_max_list[-1])
+                    data.ddds_max_list.append(data.ddds_max_list[-1])
+                    data.ds_min_list.append(data.ds_min_list[-1])
+                    data.dds_min_list.append(data.dds_min_list[-1])
+                    data.ddds_min_list.append(data.ddds_min_list[-1])
     data.t_toppra = ts
     print('TOPPRA trajecotry simulation time',time.time() - s)
     print(f'Trajectory duration: {data.t_toppra[-1]:0.4f} [s]')