if __name__ == "__main__":
pose = SE3.identity()
pose = SE3.from_list([1, 2, 3], "x")
pose2 = SE3.from_list([4, 5, 6], "x")
fig = plt.figure()
ax = fig.add_subplot(111, projection='3d')
artist = plotFrame(pose)
plt.pause(1)
artist.set_pose_data(pose2)
plt.show()
plt.cla()
yaw_tf = SO3.from_list(np.array([0, 0, 0.5]), 'r')
att = SO3.from_list(np.array([0.1, 0, 0.]), 'r')
att2 = SO3.from_list(np.array([0.2, 0, 0]), 'r')
for i in range(10):
plt.cla()
att = SO3.from_list(np.array([0.1 * i, 0.0, 0]), 'r')
artist = plotArtificialHorizon(att)
plt.pause(0.2)
for i in range(10):
plt.cla()
att = SO3.from_list(np.array([0.0, -0.05 * i, 0]), 'r')
artist = plotArtificialHorizon(att)
plt.pause(0.2)
plt.show()
with open(args.file, 'r') as f:
reader = csv.reader(f, delimiter=args.delim)
# Ignore the header
for _ in range(args.srow):
next(reader)
# Read the attitudes and animate trajectory
attitudes = []
counter = -1
for line in reader:
counter += 1
if args.skip != 0 and counter % args.skip != 0:
continue
attitude = SO3.from_list(line[args.scol:], args.fspec)
attitudes.append(attitude)
fig = plt.figure()
ax = fig.add_subplot()
attitudes = [attitudes[0].inv() * att for att in attitudes]
horizon_artist = plotting.plotArtificialHorizon(attitudes[0])
ani = FuncAnimation(fig,
update_animation,
frames=len(attitudes) + 500,
interval=20,
fargs=[attitudes, horizon_artist])
plt.show()
