def get_traj_builder(filename):
traj_builder = sorotraj.TrajBuilder()
traj_builder.load_traj_def(os.path.join(folder_to_use, filename))
return traj_builder
import sorotraj
import numpy as np
import matplotlib.pyplot as plt
#file_to_use = 'traj_setup/setpoint_traj_demo.yaml' # Basic demo
#file_to_use = 'traj_setup/setpoint_traj_demo_err0.yaml' # duplicate time (will throw exception)
#file_to_use = 'traj_setup/setpoint_traj_demo_err1.yaml' # non-monotonic time (will throw exception)
#file_to_use = 'traj_setup/setpoint_traj_demo_0.yaml' # empty prefix
#file_to_use = 'traj_setup/setpoint_traj_demo_1.yaml' # single line prefix
file_to_use = 'traj_setup/setpoint_traj_demo_2.yaml' # No prefix or suffix
#file_to_use = 'traj_setup/waveform_traj_demo.yaml' # single prefix line
# Build the trajectory from the definition file
builder = sorotraj.TrajBuilder()
builder.load_traj_def(file_to_use)
traj = builder.get_trajectory()
for key in traj:
print(key)
print(traj[key])
# Plot the trajectory
builder.plot_traj(fig_kwargs={'figsize': (8, 6), 'dpi': 150})
# Make an interpolator from the trajectory
interp = sorotraj.Interpolator(traj)
# Get the actuation function for the specified run parameters
actuation_fn, final_time = interp.get_traj_function(num_reps=1,
speed_factor=1.0,
invert_direction=False)
# Define a line-by-line conversion function to use
# This example converts from orthogonal axes to differential actuation.
def linear_conversion(traj_line, weights):
traj_length = len(traj_line) - 1
traj_line_new = [0] * (traj_length + 1)
traj_line_new[0] = traj_line[0] # Use the same time point
for idx in range(int(traj_length / 2)):
idx_list = [2 * idx + 1, 2 * idx + 2]
traj_line_new[idx_list[0]] = weights[0] * traj_line[
idx_list[0]] + weights[1] * traj_line[idx_list[1]]
traj_line_new[idx_list[1]] = weights[0] * traj_line[
idx_list[0]] - weights[1] * traj_line[idx_list[1]]
return traj_line_new
# Set up the specific version of the conversion function to use
weights = [0.9, 1.0]
conversion_fun = lambda line: linear_conversion(line, weights)
# Build the trajectories, convert them , and save them
traj = sorotraj.TrajBuilder(graph=False)
for file in files_to_use:
traj.load_traj_def(os.path.join(setup_location, file))
traj.save_traj(
os.path.join(os.path.abspath(build_location), file + '_orig'))
traj.convert(conversion_fun)
traj.save_traj(os.path.join(os.path.abspath(build_location), file))
traj.plot_traj()
for idx in range(int(traj_length/2)):
idx_list = [2*idx+1, 2*idx+2]
traj_line_new[idx_list[0]] = weights[0]*traj_line[idx_list[0]] + weights[1]*traj_line[idx_list[1]]
traj_line_new[idx_list[1]] = weights[0]*traj_line[idx_list[0]] - weights[1]*traj_line[idx_list[1]]
return traj_line_new
# Set up the specific version of the conversion function to use
weights = [1.0, 0.5]
conversion_fun = lambda line: linear_conversion(line, weights)
# Test the conversion
traj_line_test = [0.00, 5,15, 5,15 ,-10,0, -10,0]
print(traj_line_test)
print(conversion_fun(traj_line_test))
# Build the trajectories, convert them , and save them
traj = sorotraj.TrajBuilder()
for file in files_to_use:
traj.load_traj_def(os.path.join(setup_location,file))
traj.convert_traj(conversion_fun)
traj.save_traj(os.path.join(build_location,file+'_convert'))
# Convert the definitions if possible
traj = sorotraj.TrajBuilder()
for file in files_to_use:
traj.load_traj_def(os.path.join(setup_location,file))
traj.convert_definition(conversion_fun)
traj.save_definition(os.path.join(setup_location,file+'_convert'))