16 from __future__
import print_function
19 import matplotlib.pyplot
as plt
20 import uuv_trajectory_generator
22 from geometry_msgs.msg
import Point
23 from mpl_toolkits.mplot3d
import Axes3D
25 roslib.load_manifest(
'uuv_trajectory_control')
28 Demo file to demonstrate the waypoint interpolation method with generation of 29 velocity and acceleration profile using a constant rate. 34 gen = uuv_trajectory_generator.WPTrajectoryGenerator(full_dof=
False)
35 gen.set_interp_method(interp_method)
36 gen.init_waypoints(waypoint_set)
44 for ti
in np.arange(-2, gen.get_max_time(), dt):
46 pnts.append(gen.interpolate(ti))
51 print(
'Average processing time [s] =', avg_time)
54 ax = fig.add_subplot(111, projection=
'3d')
57 ax.plot([p.x
for p
in pnts], [p.y
for p
in pnts], [p.z
for p
in pnts],
'b')
60 ax.plot(waypoint_set.x, waypoint_set.y, waypoint_set.z,
'r.')
63 ax.plot(waypoint_set.x, waypoint_set.y, waypoint_set.z,
'g--')
65 for i
in range(1, len(pnts), 100):
67 p1 = p0.pos + np.dot(p0.rot_matrix, [2, 0, 0])
68 ax.plot([p0.pos[0], p1[0]], [p0.pos[1], p1[1]], [p0.pos[2], p1[2]],
'c', linewidth=2)
70 ax.set_title(interp_method)
74 ax = fig.add_subplot(211)
76 ax.plot([p.t
for p
in pnts], [p.pos[i]
for p
in pnts], label=
'%d' % i)
79 ax.set_title(
'Position - ' + interp_method)
80 ax.set_xlim(pnts[0].t, pnts[-1].t)
82 ax = fig.add_subplot(212)
84 ax.plot([p.t
for p
in pnts], [p.rot[i] * 180 / np.pi
for p
in pnts], label=
'%d' % i)
87 ax.set_title(
'Rotation - ' + interp_method)
88 ax.set_xlim(pnts[0].t, pnts[-1].t)
91 ax = fig.add_subplot(211)
93 ax.plot([p.t
for p
in pnts], [p.vel[i]
for p
in pnts], label=
'%d' % i)
96 ax.set_title(
'Linear velocity - ' + interp_method)
97 ax.set_xlim(pnts[0].t, pnts[-1].t)
99 ax = fig.add_subplot(212)
101 ax.plot([p.t
for p
in pnts], [p.vel[i+3]
for p
in pnts], label=
'%d' % i)
104 ax.set_title(
'Angular velocity - ' + interp_method)
105 ax.set_xlim(pnts[0].t, pnts[-1].t)
108 ax = fig.add_subplot(211)
110 ax.plot([p.t
for p
in pnts], [p.acc[i]
for p
in pnts], label=
'%d' % i)
113 ax.set_title(
'Linear accelerations - ' + interp_method)
114 ax.set_xlim(pnts[0].t, pnts[-1].t)
116 ax = fig.add_subplot(212)
118 ax.plot([p.t
for p
in pnts], [p.acc[i+3]
for p
in pnts], label=
'%d' % i)
121 ax.set_title(
'Angular accelerations - ' + interp_method)
122 ax.set_xlim(pnts[0].t, pnts[-1].t)
124 if __name__ ==
'__main__':
126 wp_set = uuv_trajectory_generator.WaypointSet()
128 wp_set.add_waypoint(uuv_trajectory_generator.Waypoint(-10, -12, -36, 0.5),
129 add_to_beginning=
True)
130 wp_set.add_waypoint(uuv_trajectory_generator.Waypoint(-13, -15, -44, 0.5),
131 add_to_beginning=
True)
132 wp_set.add_waypoint(uuv_trajectory_generator.Waypoint(-20, -24, -48, 0.5),
133 add_to_beginning=
True)
134 wp_set.add_waypoint(uuv_trajectory_generator.Waypoint(-10, 10, -5, 0.5))
135 wp_set.add_waypoint(uuv_trajectory_generator.Waypoint(-20, 20, -5, 0.5))
136 wp_set.add_waypoint(uuv_trajectory_generator.Waypoint(-30, 60, -50, 0.5))
137 wp_set.add_waypoint(uuv_trajectory_generator.Waypoint(-40, 70, -55, 0.5))
138 wp_set.add_waypoint(uuv_trajectory_generator.Waypoint(-40, 80, -30, 0.5))
def run_generator(waypoint_set, interp_method)