def plotEffectorTrajectoryWithReference(cs_ref, cs, dt):
labels = [
"x (m)", "y (m)", "z (m)", "dx (m/s)", "dy (m/s)", "dz (m/s)", "ddx (m/s^2)", "ddy (m/s^2)", "ddz (m/s^2)"
]
colors = ['r', 'g', 'b']
for eeName in cs.getAllEffectorsInContact():
traj = cs.concatenateEffectorTrajectories(eeName)
if traj.num_curves() > 0:
pos, timeline = discretizeSE3CurveTranslation(traj, dt)
vel = discretizeDerivateCurve(traj, dt, 1)[0][:3, :]
acc = discretizeDerivateCurve(traj, dt, 2)[0][:3, :]
values = np.vstack([pos, vel, acc])
traj_ref = cs_ref.concatenateEffectorTrajectories(eeName)
pos_ref = discretizeSE3CurveTranslation(traj_ref, dt)[0]
vel_ref = discretizeDerivateCurve(traj_ref, dt, 1)[0][:3, :]
acc_ref = discretizeDerivateCurve(traj_ref, dt, 2)[0][:3, :]
values_ref = np.vstack([pos_ref, vel_ref, acc_ref])
fig, ax = plt.subplots(3, 3)
fig.canvas.set_window_title("Trajectory for effector " + eeName + " (dashed = reference)")
fig.suptitle("Trajectory for effector " + eeName + " (dashed = reference)", fontsize=20)
for i in range(3): # line = pos,vel,acc
for j in range(3): # col = x,y,z
ax_sub = ax[i, j]
ax_sub.plot(timeline.T, values[i * 3 + j, :].T, color=colors[j])
ax_sub.plot(timeline.T, values_ref[i * 3 + j, :].T, color=colors[j], linestyle="dashed")
ax_sub.set_xlabel('time (s)')
ax_sub.set_ylabel(labels[i * 3 + j])
addVerticalLineContactSwitch(cs, ax_sub)
ax_sub.grid(False)
plt.show(block=False)
def plotEffectorTrajectory(cs, dt, name_prefix=""):
labels = [
"x (m)", "y (m)", "z (m)", "dx (m/s)", "dy (m/s)", "dz (m/s)",
"ddx (m/s^2)", "ddy (m/s^2)", "ddz (m/s^2)"
]
colors = ['r', 'g', 'b']
for eeName in cs.getAllEffectorsInContact():
traj = cs.concatenateEffectorTrajectories(eeName)
pos, timeline = discretizeSE3CurveTranslation(traj, dt)
vel = discretizeDerivateCurve(traj, dt, 1)[0][:3, :]
acc = discretizeDerivateCurve(traj, dt, 2)[0][:3, :]
values = np.vstack([pos, vel, acc])
fig, ax = plt.subplots(3, 3)
fig.canvas.set_window_title(name_prefix + " trajectory for effector " +
eeName)
fig.suptitle(name_prefix + " trajectory for effector " + eeName,
fontsize=20)
for i in range(3): # line = pos,vel,acc
for j in range(3): # col = x,y,z
ax_sub = ax[i, j]
ax_sub.plot(timeline.T,
values[i * 3 + j, :].T,
color=colors[j])
ax_sub.set_xlabel('time (s)')
ax_sub.set_ylabel(labels[i * 3 + j])
addVerticalLineContactSwitch(cs, ax_sub)
ax_sub.grid(False)
plt.show(block=False)
def plotEffectorError(cs_ref, cs, dt):
labels = ["x (m)", "y (m)", "z (m)"]
colors = ['r', 'g', 'b']
for eeName in cs.getAllEffectorsInContact():
traj = cs.concatenateEffectorTrajectories(eeName)
traj_ref = cs_ref.concatenateEffectorTrajectories(eeName)
assert traj.min() == traj_ref.min(), "In plotEffectorError : reference and real one do not have the same timeline"
assert traj.max() == traj_ref.max(), "In plotEffectorError : reference and real one do not have the same timeline"
pos_ref, timeline = discretizeSE3CurveTranslation(traj_ref, dt)
pos = discretizeSE3CurveTranslation(traj, dt)[0]
error = pos - pos_ref
fig, ax = plt.subplots(3, 1)
fig.canvas.set_window_title("Effector tracking error : " + eeName)
fig.suptitle("Effector tracking error : " + eeName, fontsize=20)
for i in range(3): # line = x,y,z
ax_sub = ax[i]
ax_sub.plot(timeline.T, error[i, :].T, color=colors[i])
ax_sub.set_xlabel('time (s)')
ax_sub.set_ylabel(labels[i])
ax_sub.yaxis.grid(False)
addVerticalLineContactSwitch(cs, ax_sub)