Dubins interpolator
%matplotlib inline
import numpy as np
import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import Axes3D
from uuv_waypoints import Waypoint, WaypointSet
from uuv_trajectory_generator import DubinsInterpolator
q_x = [0, -10, 20, 40, 50, 60]
q_y = [0, 20, 30, 30, -20, 0]
q_z = [0, 1, 0, 0, 2, 2]
q = np.vstack((q_x, q_y, q_z)).T
print(q)
[[ 0 0 0]
[-10 20 1]
[ 20 30 0]
[ 40 30 0]
[ 50 -20 2]
[ 60 0 2]]
waypoints = WaypointSet()
for i in range(q.shape[0]):
waypoints.add_waypoint(Waypoint(q[i, 0], q[i, 1], q[i, 2], max_forward_speed=0.5))
interpolator = DubinsInterpolator()
interpolator.init_waypoints(waypoints)
interpolator.init_interpolator()
2019-06-20 21:12:58,827 | INFO | path_generator | Setting initial rotation as=[0 0 0 1]
True
pnts = interpolator.get_samples(max_time=None)
fig = plt.figure(figsize=(15, 15))
ax = fig.add_subplot(111, projection='3d')
ax.plot(
[p.x for p in pnts],
[p.y for p in pnts],
[p.z for p in pnts])
ax.plot(q_x, q_y, q_z, 'ro')
ax.set_zlim([-5, 5])
ax.set_title('Dubins interpolator')
ax.set_xlabel('X [m]')
ax.set_ylabel('Y [m]')
ax.set_zlabel('Z [m]')
