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)