From b4af6c5f0b2503b21edd6f78400d6c70e813a52d Mon Sep 17 00:00:00 2001
From: Rafael Rojas <rojas@imfusion.com>
Date: Thu, 29 Aug 2024 11:30:00 +0200
Subject: [PATCH] automatic scrip generator to run python scripts and new
 example

---
 examples/python/example.py | 47 +++++++++++++++++++++++++++-----------
 1 file changed, 34 insertions(+), 13 deletions(-)

diff --git a/examples/python/example.py b/examples/python/example.py
index 2aed6e4..9493b1c 100644
--- a/examples/python/example.py
+++ b/examples/python/example.py
@@ -5,17 +5,23 @@
 """
 import gsplines
 import gsplines.plot as gplot
-import pathlib
-import sys
 import unittest
 import numpy as np
 
 try:
     import opstop
 except ImportError:
-    import pyopstop as opstop
+    import pathlib
 
-from gsplines.optimization import minimum_jerk_path
+    cwd = pathlib.Path(__file__).parent.absolute()
+    setup_script = pathlib.Path(
+        cwd, '../../', 'setup_python_env.py')
+    exec(setup_script.read_text())
+    try:
+        import opstop
+    except ImportError:
+        import sys
+        sys.exit(1)
 
 
 def main():
@@ -26,20 +32,35 @@ def main():
     model_file = pathlib.Path(__file__).absolute(
     ).parents[2].joinpath('tests', 'urdf', 'panda_arm.urdf')
 
-    print(str(model_file))
-
-    dim = 7
-    intervals = 4
-    waypoints = np.random.rand(intervals+1, dim)
-    trj = minimum_jerk_path(waypoints).linear_scaling_new_execution_time(5.0)
-
-    stop_ti = trj.get_domain()[1]*0.6
+    waypoints = \
+        np.array([[-0.237,  0.034,  0.246,  -1.631, -0.007, 1.705, 0.898],
+                  [-0.270, -0.010, 0.246,  -1.631, -0.007, 1.705, 0.898],
+                  [-0.286, 0.008,  0.222,  -1.616, 0.024,  1.692, 0.856],
+                  [-0.377, 0.123,  0.074,  -1.526, 0.210,  1.614, 0.606],
+                  [-0.387, 0.145,  0.058,  -1.517, 0.250,  1.592, 0.565],
+                  [-0.491, 0.365,  -0.102, -1.426, 0.646,  1.365, 0.158]]
+                 )
+    numberOfWaypoints, dimensionOfAmbientSpace = waypoints.shape
+    ni = numberOfWaypoints - 1
+    execution_time = 4 * ni / np.sqrt(2)
+    k = 1.5
+    alpha = np.power(k, 4) / (1.0 + np.power(k, 4))
+    basis = gsplines.basis.Basis0101(alpha)
+    trj = gsplines.optimization.optimal_sobolev_norm(
+        waypoints, basis, [(1, alpha), (3, 1-alpha)], execution_time)
+
+    trj = trj.linear_scaling_new_execution_time(2.00053)
+
+    stop_ti = 1.45928
+
+    alpha_acc_bound = 1.98
 
     optimal_parametrization = opstop.minimum_time_bounded_acceleration(
-        trj, stop_ti, 1.5, str(model_file), 8)
+        trj, stop_ti, alpha_acc_bound, str(model_file), 8)
 
     stop_trj = trj.compose(optimal_parametrization)
 
+    print(alpha)
     gplot.plot_compare([stop_trj, trj], ['red', 'blue'], [
                        'Emergency Stop Trajectory',
                        'Original Trajectory'], _show=True, _up_to_deriv=2)