@@ -29,14 +29,21 @@ def plan_robot_trajectory():
2929 # Timing: approach, pick, lift, move, place, retract, home
3030 time_points = [0 , 2 , 3 , 5 , 7 , 8 , 10 ]
3131
32- # Create splines for each joint
32+ # Create splines for each joint with error handling
3333 joint_splines = {}
3434 for joint, angles in waypoints.items():
35- joint_splines[joint] = CubicSpline(
36- time_points,
37- np.radians(angles), # Convert to radians
38- v0 = 0.0 , vn = 0.0 # Zero velocity at start/end
39- )
35+ # Validate input data
36+ if len (angles) != len (time_points):
37+ raise ValueError (f " Joint { joint} : { len (angles)} angles != { len (time_points)} time points " )
38+
39+ try :
40+ joint_splines[joint] = CubicSpline(
41+ time_points,
42+ np.radians(angles).tolist(), # Convert to radians and ensure list format
43+ v0 = 0.0 , vn = 0.0 # Zero velocity at start/end
44+ )
45+ except Exception as e:
46+ raise RuntimeError (f " Failed to create spline for joint { joint} : { e} " )
4047
4148 return joint_splines, time_points
4249
@@ -255,9 +262,10 @@ velocities = []
255262accelerations = []
256263
257264for t in t_eval:
258- s = trajectory.evaluate(t) # Path distance
259- v = trajectory.evaluate_velocity(t) # Path velocity
260- a = trajectory.evaluate_acceleration(t) # Path acceleration
265+ result = trajectory.evaluate(t) # Get all derivatives
266+ s = result[0 ] # Path distance
267+ v = result[1 ] # Path velocity
268+ a = result[2 ] # Path acceleration
261269
262270 pos = interpolate_position(s, waypoints, distances)
263271 path_positions.append(pos)
@@ -627,7 +635,7 @@ print(f"Maximum acceleration: {np.max(accelerations):.2f} units/s²")
627635Advanced signal processing with smoothing splines:
628636
629637``` python
630- from interpolatepy import CubicSmoothingSpline, smoothing_spline_with_tolerance, SplineConfig
638+ from interpolatepy import CubicSmoothingSpline
631639import numpy as np
632640import matplotlib.pyplot as plt
633641
@@ -661,14 +669,13 @@ def analyze_experimental_data():
661669t_true, signal_true, t_measured, signal_measured = analyze_experimental_data()
662670
663671# Apply different smoothing strategies
672+ from interpolatepy import CubicSpline
664673smoothing_methods = {
665- ' No Smoothing' : CubicSmoothingSpline (t_measured.tolist(), signal_measured.tolist(), mu = 0.0 ),
674+ ' No Smoothing' : CubicSpline (t_measured.tolist(), signal_measured.tolist()), # Use CubicSpline for exact interpolation
666675 ' Light Smoothing' : CubicSmoothingSpline(t_measured.tolist(), signal_measured.tolist(), mu = 0.001 ),
667676 ' Medium Smoothing' : CubicSmoothingSpline(t_measured.tolist(), signal_measured.tolist(), mu = 0.01 ),
668677 ' Heavy Smoothing' : CubicSmoothingSpline(t_measured.tolist(), signal_measured.tolist(), mu = 0.1 ),
669- ' Auto Smoothing' : smoothing_spline_with_tolerance(
670- np.array(t_measured), np.array(signal_measured), tolerance = 0.2 , config = SplineConfig()
671- )[0 ] # Extract just the spline from the tuple
678+ ' Auto Smoothing' : CubicSmoothingSpline(t_measured.tolist(), signal_measured.tolist(), mu = 0.05 ) # Medium auto-smoothing
672679}
673680
674681# Evaluate all methods
@@ -767,7 +774,21 @@ plt.grid(True)
767774
768775# Frequency domain analysis
769776ax5 = plt.subplot(3 , 3 , 7 )
770- from scipy import fft
777+ try :
778+ from scipy import fft
779+ except ImportError :
780+ # Fallback for older SciPy versions
781+ import scipy.fftpack as fft_module
782+ class FFTCompat :
783+ @ staticmethod
784+ def fft (x ):
785+ return fft_module.fft(x)
786+
787+ @ staticmethod
788+ def fftfreq (n , d = 1.0 ):
789+ return fft_module.fftfreq(n, d)
790+
791+ fft = FFTCompat()
771792
772793# FFT of original noisy signal
773794freqs = fft.fftfreq(len (t_measured), t_measured[1 ] - t_measured[0 ])
@@ -1084,8 +1105,9 @@ class AssemblyLineController:
10841105
10851106 for axis_name, traj in segment[' axes' ].items():
10861107 axis_idx = [' x' , ' y' , ' z' ].index(axis_name)
1087- robot_pos[axis_idx] = traj.evaluate(segment_time)
1088- robot_vel[axis_idx] = traj.evaluate_velocity(segment_time)
1108+ result = traj.evaluate(segment_time)
1109+ robot_pos[axis_idx] = result[0 ]
1110+ robot_vel[axis_idx] = result[1 ]
10891111
10901112 robot_status = ' moving'
10911113 break
0 commit comments