From c854e1f3ae000dc61954ca3f5d28bc668e839e6d Mon Sep 17 00:00:00 2001 From: Maxim Vochten Date: Thu, 21 Nov 2024 14:08:38 +0100 Subject: [PATCH] add tolerance parameter to discretized invariants --- ...calculate_invariants_position_longtrial.py | 65 ++++-- ...ti_calculate_vector_invariants_rotation.py | 8 +- .../discretized_vector_invariants.py | 216 +++++------------- .../kinematics/orientation_kinematics.py | 6 +- invariants_py/ocp_initialization.py | 17 +- 5 files changed, 132 insertions(+), 180 deletions(-) diff --git a/examples/calculate_invariants_position_longtrial.py b/examples/calculate_invariants_position_longtrial.py index 9fbde89..c0cae9d 100644 --- a/examples/calculate_invariants_position_longtrial.py +++ b/examples/calculate_invariants_position_longtrial.py @@ -5,15 +5,29 @@ import matplotlib.pyplot as plt from invariants_py.data_handler import find_data_path from invariants_py import invariants_handler +from invariants_py import plotters # Load the CSV file df = pd.read_csv(find_data_path('trajectory_long.csv')) -# Plot xyz coordinates with respect to timestamp +# Downsample the trajectory to 400 samples +nb_samples = 200 +trajectory = np.column_stack((df['x'], df['y'], df['z'])) +timestamps = df['timestamp'].values +downsampled_indices = np.linspace(0, len(trajectory) - 1, nb_samples, dtype=int) +#trajectory = trajectory[0:200] # correction: the time step should be around 0.1s +#timestamps = timestamps[0:200]*10 +trajectory = trajectory[downsampled_indices] +timestamps = timestamps[downsampled_indices]*10 + +# print(timestamps) +# print(trajectory) + +#Plot xyz coordinates with respect to timestamp plt.figure() -plt.plot(df['timestamp'], df['x'], label='x') -plt.plot(df['timestamp'], df['y'], label='y') -plt.plot(df['timestamp'], df['z'], label='z') +plt.plot(timestamps, trajectory[:,0], label='x') +plt.plot(timestamps, trajectory[:,1], label='y') +plt.plot(timestamps, trajectory[:,2], label='z') plt.xlabel('Timestamp') plt.ylabel('Coordinates') plt.legend() @@ -23,7 +37,7 @@ # Plot the trajectory in 3D fig = plt.figure() ax = fig.add_subplot(111, projection='3d') -ax.plot(df['x'], df['y'], df['z']) +ax.plot(trajectory[:,0], trajectory[:,1], trajectory[:,2]) ax.set_aspect('equal') ax.set_xlabel('X') ax.set_ylabel('Y') @@ -31,20 +45,41 @@ ax.set_title('3D Trajectory') plt.show() -# Downsample the trajectory to 400 samples -nb_samples = 200 -trajectory = np.column_stack((df['x'], df['y'], df['z'])) -timestamps = df['timestamp'].values -#downsampled_indices = np.linspace(0, len(trajectory) - 1, nb_samples, dtype=int) -trajectory = trajectory[0:200] # correction: the time step should be around 0.1s -timestamps = timestamps[0:200]*10 -print(timestamps) -print(trajectory) - #/*********************************************************************************************************************/ #/* Option 1: Calculate invariants using discretized analytical formulas #/*********************************************************************************************************************/ +from invariants_py import discretized_vector_invariants as dvi +invariants_init, trajectory_init, moving_frames_init = dvi.calculate_discretized_invariants(trajectory, timestamps) + +print(invariants_init) +plotters.plot_moving_frames(trajectory, moving_frames_init) # calculated moving frames along trajectory + +# Plot the invariants_init +fig, axs = plt.subplots(3, 1, figsize=(10, 8)) + +axs[0].plot(timestamps, invariants_init[:, 0], label='Invariant 1') +axs[0].plot(0, 0, label='Invariant 1') +axs[0].set_xlabel('Timestamp') +axs[0].set_ylabel('Invariant 1') +axs[0].legend() +axs[0].set_title('Calculated Geometric Invariant 1') + +axs[1].plot(timestamps, invariants_init[:, 1], label='Invariant 2') +axs[1].set_xlabel('Timestamp') +axs[1].set_ylabel('Invariant 2') +axs[1].legend() +axs[1].set_title('Calculated Geometric Invariant 2') + +axs[2].plot(timestamps, invariants_init[:, 2], label='Invariant 3') +axs[2].set_xlabel('Timestamp') +axs[2].set_ylabel('Invariant 3') +axs[2].legend() +axs[2].set_title('Calculated Geometric Invariant 3') + +plt.tight_layout() +plt.show() + #/*********************************************************************************************************************/ diff --git a/invariants_py/calculate_invariants/opti_calculate_vector_invariants_rotation.py b/invariants_py/calculate_invariants/opti_calculate_vector_invariants_rotation.py index 614f729..db6de78 100644 --- a/invariants_py/calculate_invariants/opti_calculate_vector_invariants_rotation.py +++ b/invariants_py/calculate_invariants/opti_calculate_vector_invariants_rotation.py @@ -99,8 +99,8 @@ def __init__(self, window_len = 100, bool_unsigned_invariants = False, rms_error def calculate_invariants(self,trajectory_meas,stepsize, choice_initialization=2): - from invariants_py.ocp_initialization import calculate_velocity_from_discrete_rotations, estimate_vector_invariants - from invariants_py.discretized_vector_invariants import estimate_movingframes + from invariants_py.ocp_initialization import calculate_velocity_from_discrete_rotations, calculate_vector_invariants + from invariants_py.discretized_vector_invariants import calculate_moving_frames from invariants_py.dynamics_vector_invariants import reconstruct_rotation_traj from invariants_py.kinematics.screw_kinematics import average_vector_orientation_frame @@ -136,8 +136,8 @@ def calculate_invariants(self,trajectory_meas,stepsize, choice_initialization=2) # Initialization by estimating moving frames with discrete analytical equations Rdiff = calculate_velocity_from_discrete_rotations(measured_orientation,timestamps=np.arange(N)) invariants = np.hstack((1*np.ones((N-1,1)),1e-1*np.ones((N-1,2)))) - R_r_traj = estimate_movingframes(Rdiff) - invariants = estimate_vector_invariants(R_r_traj,Rdiff,stepsize) + R_r_traj = calculate_moving_frames(Rdiff) + invariants = calculate_vector_invariants(R_r_traj,Rdiff,stepsize) R_obj_traj = measured_orientation elif choice_initialization == 3: diff --git a/invariants_py/discretized_vector_invariants.py b/invariants_py/discretized_vector_invariants.py index 58bd107..1fd15f7 100644 --- a/invariants_py/discretized_vector_invariants.py +++ b/invariants_py/discretized_vector_invariants.py @@ -1,6 +1,5 @@ import numpy as np import invariants_py.kinematics.orientation_kinematics as SO3 -from invariants_py.reparameterization import interpR from math import atan2 def calculate_velocity_from_discrete_rotations(R, timestamps): @@ -42,7 +41,7 @@ def calculate_velocity_from_discrete_rotations(R, timestamps): return rot_velocity -def calculate_tangent(vector_traj): +def calculate_tangent(vector_traj, tolerance_singularity): """ Estimate the first axis of the moving frame based on the given trajectory. The first axis is calculated by normalizing the trajectory vector. @@ -62,7 +61,7 @@ def calculate_tangent(vector_traj): norm_vector = np.linalg.norm(vector_traj, axis=1) # Find the index of the first non-zero norm using np.isclose to account for numerical precision issues - first_nonzero_norm_index = np.where(~np.isclose(norm_vector, 0))[0] + first_nonzero_norm_index = np.where(~np.isclose(norm_vector, 0, atol=tolerance_singularity))[0] if first_nonzero_norm_index.size == 0: # If all norms are zero, set the tangent to [1, 0, 0] for all samples @@ -72,18 +71,18 @@ def calculate_tangent(vector_traj): # For each sample starting from the first non-zero norm index for i in range(first_nonzero_norm_index, N): - if not np.isclose(norm_vector[i], 0): - tangent[i, :] = vector_traj[i, :] / norm_vector[i] + if not np.isclose(norm_vector[i], 0, atol=tolerance_singularity): + tangent[i,:] = vector_traj[i,:] / norm_vector[i] else: - tangent[i, :] = tangent[i-1, :] + tangent[i,:] = tangent[i-1,:] # For each sample before the first non-zero norm index for i in range(first_nonzero_norm_index): - tangent[i, :] = tangent[first_nonzero_norm_index, :] + tangent[i,:] = tangent[first_nonzero_norm_index,:] return tangent -def calculate_binormal(vector_traj,tangent, reference_vector=None): +def calculate_binormal(vector_traj,tangent, tolerance_singularity, reference_vector=None, ): """ Estimate the third axis of the moving frame based on the given trajectory. The third axis is calculated by normalizing the cross product of the trajectory vector and the tangent. @@ -110,10 +109,10 @@ def calculate_binormal(vector_traj,tangent, reference_vector=None): norm_binormal_vec = np.linalg.norm(binormal_vec, axis=1) # Find the index of the first non-zero norm using np.isclose to account for numerical precision issues - first_nonzero_norm_index = np.where(~np.isclose(norm_binormal_vec, 0))[0] + first_nonzero_norm_index = np.where(~np.isclose(norm_binormal_vec, 0, atol=tolerance_singularity))[0] if first_nonzero_norm_index.size == 0: # choose a non-collinear vector - a = np.array([0, 0, 1]) if not np.isclose(tangent[0,2], 1) else np.array([0, 1, 0]) + a = np.array([0, 0, 1]) if not np.isclose(tangent[0,2], 1, atol=tolerance_singularity) else np.array([0, 1, 0]) # take cross-product to get perpendicular perp = np.cross(tangent[0,:], a, axis=0) @@ -126,7 +125,7 @@ def calculate_binormal(vector_traj,tangent, reference_vector=None): # For each sample starting from the first non-zero norm index for i in range(first_nonzero_norm_index, N): - if not np.isclose(norm_binormal_vec[i], 0): + if not np.isclose(norm_binormal_vec[i], 0, atol=tolerance_singularity): binormal[i, :] = binormal_vec[i,:] / np.linalg.norm(binormal_vec[i,:]) else: binormal[i, :] = binormal[i-1, :] @@ -142,30 +141,25 @@ def calculate_binormal(vector_traj,tangent, reference_vector=None): return binormal -def estimate_movingframes(vector_traj): +def calculate_moving_frames(vector_traj, tolerance_singularity = 1e-2): - # Calculate - - # Estimate first axis - e_tangent = calculate_tangent(vector_traj) - - # Calculate binormal vector N = np.size(vector_traj, 0) - # binormal_vec = np.zeros((N,3)) - # for i in range(N-1): - # binormal_vec[i,:] = np.cross(vector_traj[i,:],vector_traj[i+1,:]) - # binormal_vec[-1,:] = binormal_vec[-2,:] - # Estimate second axis - e_binormal = calculate_binormal(vector_traj,e_tangent,reference_vector=np.array([0,0,1])) + # Calculate first axis + e_tangent = calculate_tangent(vector_traj, tolerance_singularity) # Calculate third axis + e_binormal = calculate_binormal(vector_traj,e_tangent,tolerance_singularity,reference_vector=np.array([0,0,1])) + + # Calculate second axis as cross product of third and first axis e_normal = np.array([ np.cross(e_binormal[i,:],e_tangent[i,:]) for i in range(N) ]) - R = np.zeros((N,3,3)) + # Assemble the moving frame + R_mf = np.zeros((N,3,3)) for i in range(N): - R[i,:,:] = np.column_stack((e_tangent[i,:],e_normal[i,:],e_binormal[i,:])) - return R + R_mf[i,:,:] = np.column_stack((e_tangent[i,:],e_normal[i,:],e_binormal[i,:])) + + return R_mf def angle_between_vectors(u, v, rot_axis = None): """ @@ -174,151 +168,53 @@ def angle_between_vectors(u, v, rot_axis = None): Input: u: first vector v: second vector + rot_axis: rotation axis (optional) Output: angle: angle [rad] between the two vectors """ - + # Calculate absolute value of angle which is between 0 and pi cross_prod = np.cross(u,v) angle = atan2(np.linalg.norm(cross_prod),np.dot(u,v)) + # If the direction of the rotation axis is given, the sign of the angle can be determined if rot_axis is not None: sign = np.sign(np.dot(cross_prod, rot_axis)) angle = sign*angle return angle -def estimate_vector_invariants(R_mf_traj,vector_traj,stepsize): - ''' +def calculate_vector_invariants(R_mf_traj,vector_traj,progress_step): + """ + Calculate the vector invariants of a measured position trajectory + based on a discrete approximation of the moving frame. - ''' + Input: + R_mf_traj: trajectory of the moving frame (Nx3x3) + vector_traj: trajectory vector (Nx3) + progress_steps: progress steps (Nx1) + Output: + invariants: invariants (Nx3) + """ N = np.size(vector_traj,0) invariants = np.zeros((N,3)) - # first invariant is the norm of the dot-product between the trajectory vector and the first axis of the moving frame for i in range(N): + # first invariant is the norm of the dot-product between the trajectory vector and the first axis of the moving frame invariants[i,0] = np.dot(vector_traj[i,:],R_mf_traj[i,:,0]) - # second invariant is the angle between successive first axes of the moving frame for i in range(N-1): - invariants[i,1] = angle_between_vectors(R_mf_traj[i,:,0], R_mf_traj[i+1,:,0], R_mf_traj[i,:,2])/stepsize - - # third invariant is the angle between successive third axes of the moving frame - for i in range(N-1): - invariants[i,2] = angle_between_vectors(R_mf_traj[i,:,2], R_mf_traj[i+1,:,2], R_mf_traj[i,:,0])/stepsize + # second invariant is the change in angle of the first axis of the moving frame along the third axis over the progress step + invariants[i,1] = angle_between_vectors(R_mf_traj[i,:,0], R_mf_traj[i+1,:,0], R_mf_traj[i,:,2]) / progress_step + # third invariant is the change in angle of the third axis of the moving frame along the first axis over the progress step + invariants[i,2] = angle_between_vectors(R_mf_traj[i,:,2], R_mf_traj[i+1,:,2], R_mf_traj[i,:,0]) / progress_step - invariants[-1,1:] = invariants[-2,1:] # copy last values - #print(invariants) + # Repeat last values for the last sample to have the same length as the input + invariants[-1,1:] = invariants[-2,1:] return invariants -def estimate_initial_frames(vector_traj): - # Estimate initial moving frames based on measurements - - N = np.size(vector_traj,0) - - #TODO this is not correct yet, ex not perpendicular to ey + not robust for singularities, these parts must still be transferred from Matlab - - ex = vector_traj / (np.linalg.norm(vector_traj,axis=1).reshape(N,1)+0.000001) - ez = np.tile( np.array((0,0,1)), (N,1) ) - ey = np.array([ np.cross(ez[i,:],ex[i,:]) for i in range(N) ]) - - return ex,ey,ez - -def initialize_VI_pos(input_trajectory): - - if input_trajectory.shape[1] == 3: - measured_positions = input_trajectory - else: - measured_positions = input_trajectory[:,:3,3] - - N = np.size(measured_positions,0) - - Pdiff = np.diff(measured_positions,axis=0) - ex = Pdiff / np.linalg.norm(Pdiff,axis=1).reshape(N-1,1) - ex = np.vstack((ex,[ex[-1,:]])) - ez = np.tile( np.array((0,0,1)), (N,1) ) - ey = np.array([np.cross(ez[i,:],ex[i,:]) for i in range(N)]) - - R_t = np.zeros((3,3*N)) - for i in range(N-1): - R_t[:,3*i:3*(i+1)] = np.array([ex[i,:],ey[i,:],ez[i,:]]) - - p_obj_sol = measured_positions.T - invars = np.vstack((1e0*np.ones((1,N-1)),1e-1*np.ones((1,N-1)), 1e-12*np.ones((1,N-1)))) - return [invars, p_obj_sol, R_t] - -def initialize_VI_pos2(measured_positions,stepsize): - - N = np.size(measured_positions,0) - Pdiff = np.diff(measured_positions, axis=0) - Pdiff = np.vstack((Pdiff, Pdiff[-1])) - - [ex,ey,ez] = estimate_initial_frames(Pdiff) - - R_t_init2 = np.zeros((N,3,3)) - for i in range(N): - R_t_init2[i,:,:] = np.column_stack((ex[i,:],ey[i,:],ez[i,:])) - #print(R_t_init2) - invars = estimate_vector_invariants(R_t_init2,Pdiff,stepsize) + 1e-12*np.ones((N,3)) - #print(invars) - - R_t_init = np.zeros((9,N)) - for i in range(N): - R_t_init[:,i] = np.hstack([ex[i,:],ey[i,:],ez[i,:]]) - - p_obj_sol = measured_positions.T - #invars = np.vstack((1e0*np.ones((1,N-1)),1e-1*np.ones((1,N-1)), 1e-12*np.ones((1,N-1)))) - - return [invars[:-1,:].T, p_obj_sol, R_t_init] - -def initialize_VI_rot(input_trajectory): - - if input_trajectory.shape[1] == 3: - measured_orientation = input_trajectory - else: - measured_orientation = input_trajectory[:,:3,:3] - - N = np.size(measured_orientation,0) - Rdiff = calculate_velocity_from_discrete_rotations(measured_orientation,timestamps=np.arange(N)) - - [ex,ey,ez] = estimate_initial_frames(Rdiff) - - R_r = np.zeros((3,3*N)) - R_obj = np.zeros((3,3*N)) - for i in range(N-1): - R_r[:,3*i:3*(i+1)] = np.array([ex[i,:],ey[i,:],ez[i,:]]) - R_obj[:,3*i:3*(i+1)] = np.array([measured_orientation[i,0],measured_orientation[i,1],measured_orientation[i,2]]) - - invars = np.vstack((1e0*np.ones((1,N-1)),1e-1*np.ones((1,N-1)), 1e-12*np.ones((1,N-1)))) - - return [invars, R_obj, R_r], measured_orientation - -def initialize_VI_rot2(measured_orientation): - - N = np.size(measured_orientation,0) - Rdiff = calculate_velocity_from_discrete_rotations(measured_orientation,timestamps=np.arange(N)) - - #print(Rdiff) - - [ex,ey,ez] = estimate_initial_frames(Rdiff) - - # R_t_init = np.zeros((9,N)) - # for i in range(N): - # R_t_init[:,i] = np.hstack([ex[i,:],ey[i,:],ez[i,:]]) - - R_r = np.zeros((9,N)) - R_obj = np.zeros((9,N)) - for i in range(N): - R_r[:,i] = np.hstack([ex[i,:],ey[i,:],ez[i,:]]) - R_obj[:,i] = np.hstack([measured_orientation[i,:,0],measured_orientation[i,:,1],measured_orientation[i,:,2]]) - - invars = np.vstack((1e0*np.ones((1,N-1)),1e-1*np.ones((1,N-1)), 1e-12*np.ones((1,N-1)))) - - return [invars, R_obj, R_r] - - -def discrete_approximation_invariants(measured_positions, stepsize): +def calculate_discretized_invariants(measured_positions, progress, tolerance_singularity = 1e0): """ Calculate the vector invariants of a measured position trajectory based on a discrete approximation of the moving frame. @@ -331,18 +227,24 @@ def discrete_approximation_invariants(measured_positions, stepsize): trajectory: trajectory of the moving frame (Nx3) mf: moving frame (Nx3x3) """ - N = np.size(measured_positions, 0) - Pdiff = np.diff(measured_positions,axis=0) - - # Calculate the trajectory of the moving frame - R_mf_traj = estimate_movingframes(Pdiff) + progress_step = np.mean(np.diff(progress))#.reshape(-1,1) + + # Calculate velocity vector in a discretized way + pos_vector_differences = np.diff(measured_positions,axis=0) + vel_vector = pos_vector_differences / progress_step + #print(vel_vector) + + # Calculate the moving frame + R_mf_traj = calculate_moving_frames(vel_vector, tolerance_singularity) + #print(R_mf_traj) - # Calculate the invariants based on the trajectory - invariants = estimate_vector_invariants(R_mf_traj,Pdiff/stepsize,stepsize) + 1e-6*np.ones((N-1,3)) - invariants = np.vstack((invariants, invariants[-1,:])) + # Calculate the invariants + invariants = calculate_vector_invariants(R_mf_traj,vel_vector,progress_step) + # print(invariants) - # append last value to mf - R_mf_traj = np.concatenate((R_mf_traj, R_mf_traj[-1,:,:][np.newaxis, :, :]), axis=0) + # Repeat last values for the last sample to have the same length as the input + R_mf_traj = np.concatenate((R_mf_traj, R_mf_traj[-1,:,:][np.newaxis, :, :]), axis=0) + invariants = np.vstack((invariants, invariants[-1,:])) return invariants, measured_positions, R_mf_traj \ No newline at end of file diff --git a/invariants_py/kinematics/orientation_kinematics.py b/invariants_py/kinematics/orientation_kinematics.py index 01b07d4..5a8bff1 100644 --- a/invariants_py/kinematics/orientation_kinematics.py +++ b/invariants_py/kinematics/orientation_kinematics.py @@ -254,13 +254,15 @@ def logm(R): # Special case: rotation angle = pi or -pi if np.isclose(ca,-1): - _,_,VT = np.linalg.svd(R - np.eye(3)) # R*v = v --> (R-I)*v = 0 + # The following relation holds when the angle is +/-pi: R*v = v ===> (R-I)*v = 0 + # Therefore, the rotation axis can be found from a singular value decomposition + _,_,VT = np.linalg.svd(R - np.eye(3)) # rotation_vec = VT[-1,:] rotation_vec = rotation_vec/np.linalg.norm(rotation_vec) alpha = np.pi return crossmat(rotation_vec)*alpha - # General case + # General case (rotation angle is not 0 or pi) else: axis = crossvec(R) sa = norm(axis) diff --git a/invariants_py/ocp_initialization.py b/invariants_py/ocp_initialization.py index 0d117cc..88cef46 100644 --- a/invariants_py/ocp_initialization.py +++ b/invariants_py/ocp_initialization.py @@ -1,7 +1,7 @@ import numpy as np import invariants_py.kinematics.orientation_kinematics as SO3 from invariants_py.reparameterization import interpR -from invariants_py.discretized_vector_invariants import estimate_vector_invariants, estimate_initial_frames, calculate_velocity_from_discrete_rotations +from invariants_py.discretized_vector_invariants import calculate_vector_invariants, calculate_velocity_from_discrete_rotations def initial_trajectory_movingframe_rotation(R_obj_start,R_obj_end,N=100): @@ -120,6 +120,19 @@ def initialize_VI_pos(input_trajectory): invars = np.vstack((1e0*np.ones((1,N-1)),1e-1*np.ones((1,N-1)), 1e-12*np.ones((1,N-1)))) return [invars, p_obj_sol, R_t] +def estimate_initial_frames(vector_traj): + # Estimate initial moving frames based on measurements + + N = np.size(vector_traj,0) + + #TODO this is not correct yet, ex not perpendicular to ey + not robust for singularities, these parts must still be transferred from Matlab + + ex = vector_traj / (np.linalg.norm(vector_traj,axis=1).reshape(N,1)+0.000001) + ez = np.tile( np.array((0,0,1)), (N,1) ) + ey = np.array([ np.cross(ez[i,:],ex[i,:]) for i in range(N) ]) + + return ex,ey,ez + def initialize_VI_pos2(measured_positions,stepsize): N = np.size(measured_positions,0) @@ -132,7 +145,7 @@ def initialize_VI_pos2(measured_positions,stepsize): for i in range(N): R_t_init2[i,:,:] = np.column_stack((ex[i,:],ey[i,:],ez[i,:])) #print(R_t_init2) - invars = estimate_vector_invariants(R_t_init2,Pdiff,stepsize) + 1e-12*np.ones((N,3)) + invars = calculate_vector_invariants(R_t_init2,Pdiff,stepsize) + 1e-12*np.ones((N,3)) #print(invars) R_t_init = np.zeros((9,N))