diff --git a/examples/calculate_invariants_position_longtrial.py b/examples/calculate_invariants_position_longtrial.py index 55ef813..887017c 100644 --- a/examples/calculate_invariants_position_longtrial.py +++ b/examples/calculate_invariants_position_longtrial.py @@ -39,7 +39,7 @@ stepsize = np.mean(np.diff(timestamps)) # Downsample the trajectory to 100 samples -downsampled_indices = np.linspace(0, len(trajectory) - 1, 200, dtype=int) +downsampled_indices = np.linspace(0, len(trajectory) - 1, 400, dtype=int) trajectory = trajectory[downsampled_indices]/1000 # Convert to meters timestamps = timestamps[downsampled_indices] stepsize = np.mean(np.diff(timestamps)) @@ -52,6 +52,9 @@ ocp = OCP_calc_pos(window_len=len(trajectory),fatrop_solver=True,geometric=True) invariants, reconstructed_trajectory, moving_frames = ocp.calculate_invariants(trajectory, stepsize) +invariants[:,1] = invariants[:,1]/invariants[:,0] # get geometric curvature +invariants[:,2] = invariants[:,2]/invariants[:,0] # get geometric torsion + # Plot the calculated invariants as subplots fig, axs = plt.subplots(3, 1, figsize=(10, 8))