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)