def plot(self):
plt.figure()
ax = plt.subplot(111, projection="3d", aspect="equal")
#ax.scatter(self.x0[0], self.x0[1], self.x0[2], c="r", s=100)
for viapoint in self.via_points:
ax.scatter(viapoint[1], viapoint[2], viapoint[3], c="k", s=100)
# TODO sort by time
ax.plot(self.via_points[:, 1],
self.via_points[:, 2],
self.via_points[:, 3],
c="k",
alpha=0.5)
#plot_trajectory(ax=ax, P=self.P, s=0.05, lw=2, c="k",
# show_direction=False)
ax.plot(self.P[:, 0], self.P[:, 1], self.P[:, 2], c="k")
key_frames = np.linspace(0, self.P.shape[0] - 1, 10).astype(np.int)
s = 0.1
for p in self.P[key_frames]:
R = matrix_from_quaternion(p[3:])
for d in range(3):
ax.plot([p[0], p[0] + s * R[0, d]], [p[1], p[1] + s * R[1, d]],
[p[2], p[2] + s * R[2, d]],
color="k")
ax.set_xlim((-0.4, 0.4))
ax.set_ylim((-0.9, 0.1))
ax.set_zlim((0.2, 1.0))
ax.set_xlabel("x")
ax.set_ylabel("y")
ax.set_zlabel("z")
return ax
def approx_trajectory(P, w, fk, q_bounds):
Q = np.empty((P.shape[0], 7))
q = np.zeros(7)
for t in range(P.shape[0]):
R = pyrot.matrix_from_quaternion(P[t, 3:])
q, _ = approxInvKin(w, R, P[t, :3], q, q_bounds, fk)
Q[t, :] = q
return Q
def plot(self):
plt.figure()
ax = plt.subplot(111, projection="3d", aspect="equal")
ax.scatter(self.x0[0],
self.x0[1],
self.x0[2],
marker="^",
c="k",
s=100)
ax.scatter(self.g[0], self.g[1], self.g[2], c="k", s=100)
is_colliding = self.get_obstacle_is_colliding()
for i, obstacle in enumerate(self.obstacles):
phi, theta = np.mgrid[0.0:np.pi:100j, 0.0:2.0 * np.pi:100j]
x = obstacle[0] + self.obstacle_dist * np.sin(phi) * np.cos(theta)
y = obstacle[1] + self.obstacle_dist * np.sin(phi) * np.sin(theta)
z = obstacle[2] + self.obstacle_dist * np.cos(phi)
color = "r" if is_colliding[i] else "k"
ax.plot_surface(x,
y,
z,
rstride=5,
cstride=5,
color=color,
alpha=0.1,
linewidth=0)
ax.plot_wireframe(x,
y,
z,
rstride=20,
cstride=20,
color=color,
alpha=0.1)
#plot_trajectory(ax=ax, P=self.P, s=0.1, lw=2, c="k",
# show_direction=False)
ax.plot(self.P[:, 0], self.P[:, 1], self.P[:, 2], c="k")
key_frames = np.linspace(0, self.P.shape[0] - 1, 10).astype(np.int)
s = 0.1
for p in self.P[key_frames]:
R = matrix_from_quaternion(p[3:])
for d in range(3):
ax.plot([p[0], p[0] + s * R[0, d]], [p[1], p[1] + s * R[1, d]],
[p[2], p[2] + s * R[2, d]],
color="k")
ax.set_xlim((-0.4, 0.4))
ax.set_ylim((-0.9, -0.1))
ax.set_zlim((0.4, 1.2))
ax.set_xlabel("x")
ax.set_ylabel("y")
ax.set_zlabel("z")
return ax
def exact_trajectory(P, ik):
Q = np.empty((P.shape[0], 7))
ik.q = np.zeros(7)
for t in range(P.shape[0]):
R = pyrot.matrix_from_quaternion(P[t, 3:])
ik.set_point(R=R, p=P[t, :3])
if ik.check_reachable():
Q[t, :] = ik.get_q()
else:
Q[t, :] = np.nan
return Q
def plot_pose_trajectory(P_original,
P,
ax=None,
show_plot=False,
color="k",
direction=True,
alpha=1.0,
plot_handler=plot_handler,
window_offset=True):
if len(P) == 0:
raise ValueError("Trajectory does not contain any elements.")
ax = _make_3d_axis(ax, plot_handler, window_offset)
P[np.logical_not(np.isfinite(P))] = 0.0
ax.plot(P_original[:, 0],
P_original[:, 1],
P_original[:, 2],
lw=3,
color=color,
alpha=0.3)
ax.plot(P[:, 0], P[:, 1], P[:, 2], lw=3, color=color, alpha=alpha)
for t in range(P.shape[0]):
#print np.round(P[t], 2), np.round(P_original[t], 2)
ax.plot([P[t, 0], P_original[t, 0]], [P[t, 1], P_original[t, 1]],
[P[t, 2], P_original[t, 2]],
color="r",
ls="o")
for p in P[::(P.shape[0] - 1) / 10]:
pyrot.plot_basis(ax,
pyrot.matrix_from_quaternion(p[3:]),
p[:3],
s=0.03,
alpha=alpha)
if direction:
s = P[0, :3]
g = P[-1, :3]
start = s + 0.2 * (g - s)
goal = g - 0.2 * (g - s)
ax.add_artist(
Arrow3D([start[0], goal[0]], [start[1], goal[1]],
[start[2], goal[2]],
mutation_scale=20,
lw=1,
arrowstyle="-|>",
color="k"))
if show_plot:
plt.show()
return ax
def plot_pose_trajectory(
P_original, P, ax=None, show_plot=False, color="k", direction=True,
alpha=1.0, plot_handler=plot_handler):
if len(P) == 0:
raise ValueError("Trajectory does not contain any elements.")
ax = _make_3d_axis(ax, plot_handler)
r = np.all(np.isfinite(P), axis=1) # reachable
ax.plot(P_original[:, 0],
P_original[:, 1],
P_original[:, 2],
lw=5, color=color, alpha=0.3)
P_original_not_reachable = np.copy(P_original)
P_original_not_reachable[r] = np.nan
ax.plot(P_original_not_reachable[:, 0],
P_original_not_reachable[:, 1],
P_original_not_reachable[:, 2],
lw=5, color="r", alpha=0.3)
ax.plot(P[:, 0], P[:, 1], P[:, 2], lw=5, color=color, alpha=alpha)
for t in range(P.shape[0]):
ax.plot([P[t, 0], P_original[t, 0]], [P[t, 1], P_original[t, 1]],
[P[t, 2], P_original[t, 2]], color="r")
step = max(1, (P.shape[0] - 1) / 10)
frame_indices = np.arange(0, P.shape[0], step)
for t in frame_indices:
if not r[t]:
continue
p = P[t]
pyrot.plot_basis(ax, pyrot.matrix_from_quaternion(p[3:]), p[:3],
s=0.1, alpha=alpha)
if direction and r[0] and r[-1]:
s = P[0, :3]
g = P[-1, :3]
start = s + 0.2 * (g - s)
goal = g - 0.2 * (g - s)
ax.add_artist(Arrow3D([start[0], goal[0]],
[start[1], goal[1]],
[start[2], goal[2]],
mutation_scale=20, lw=1, arrowstyle="-|>",
color="k"))
if show_plot:
plt.show()
return ax