Build One Trajectory

build_one_trajectory.py

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/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()

# 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=2,
                speed_factor=1.0,
                invert_direction=[1,3])

print("Final Interpolation Time: %f"%(final_time))

# Get the cycle function for the specified run parameters
cycle_fn = interp.get_cycle_function(
                num_reps=2,
                speed_factor=1.0,
                invert_direction=[1,3])

# Plot the actuation function vs. time
times = np.linspace(-1,20,2000)
vals = actuation_fn(times)

plt.plot(times, vals)
plt.show()