Esempio n. 1
0
def test3D_HBS_svmlearnedGammas(x_target, x_initial, gamma_type):
    # Create 3D Dataset
    grid_size = 15
    X, Y, c_labels = create_franka_dataset(dimension=3,
                                           grid_size=grid_size,
                                           plot_training_data=0)
    gamma_svm = 20
    c_svm = 20
    grid_limits_x = [0.1, 1.0]
    grid_limits_y = [-0.8, 0.8]
    grid_limits_z = [0.55, 1.1]
    dt = 0.03

    # Learn Gamma Function/s!
    if not gamma_type:
        # Same SVM for all Gammas (i.e. normals will be the same)
        learned_obstacles = learn_gamma_fn.create_obstacles_from_data(
            data=X,
            label=Y,
            plot_raw_data=False,
            gamma_svm=gamma_svm,
            c_svm=c_svm,
            cluster_labels=c_labels)
    else:
        # Independent SVMs for each Gammas (i.e. normals will be different at each grid state)
        learned_obstacles = learn_gamma_fn.create_obstacles_from_data_multi(
            data=X,
            label=Y,
            plot_raw_data=False,
            gamma_svm=gamma_svm,
            c_svm=c_svm,
            cluster_labels=c_labels)

    # -- Draw Normal Vector Field of Gamma Functions and Modulated DS --- #
    grid_limits_x = [0.1, 1.0]
    grid_limits_y = [-0.8, 0.8]
    grid_limits_z = [0.55, 1.3]

    xx, yy, zz = np.meshgrid(
        np.linspace(grid_limits_x[0], grid_limits_x[1], grid_size),
        np.linspace(grid_limits_y[0], grid_limits_y[1], grid_size),
        np.linspace(grid_limits_z[0], grid_limits_z[1], grid_size))
    positions = np.c_[xx.ravel(), yy.ravel(), zz.ravel()].T

    if not gamma_type:
        # This will use the single gamma function formulation (which is not entirely correct due to handling of the reference points)
        classifier = learned_obstacles['classifier']
        max_dist = learned_obstacles['max_dist']
        reference_points = learned_obstacles['reference_points']
        gamma_svm = learned_obstacles['gamma_svm']
        n_obstacles = learned_obstacles['n_obstacles']
        filename = "./dynamical_system_modulation_svm/figures/svmlearnedGamma_combined_3D"

        print("Computing Gammas and Normals..")
        gamma_vals = learn_gamma_fn.get_gamma(positions,
                                              classifier,
                                              max_dist,
                                              reference_points,
                                              dimension=3)
        normal_vecs = learn_gamma_fn.get_normal_direction(positions,
                                                          classifier,
                                                          reference_points,
                                                          max_dist,
                                                          gamma_svm=gamma_svm,
                                                          dimension=3)

        ########################################################################################
        # ------ Visualize Gammas and Normal Vectors! -- Make self-contained function ------ #
        fig = plt.figure()
        ax = plt.axes(projection='3d')
        plt.title("$\Gamma$-Score")
        ax.set_xlim3d(grid_limits_x[0], grid_limits_x[1])
        ax.set_ylim3d(grid_limits_y[0], grid_limits_y[1])
        ax.set_zlim3d(grid_limits_z[0], grid_limits_z[1])
        gamma_score = gamma_vals - 1  # Subtract 1 to have differentiation boundary at 1
        ax.scatter3D(X[0, :],
                     X[1, :],
                     X[2, :],
                     '.',
                     c=gamma_score,
                     cmap=plt.cm.coolwarm,
                     alpha=0.10)

        normal_vecs = normal_vecs.reshape(X.shape)
        ax.quiver(X[0, :],
                  X[1, :],
                  X[2, :],
                  normal_vecs[0, :],
                  normal_vecs[1, :],
                  normal_vecs[2, :],
                  length=0.05,
                  normalize=True)
        ax.view_init(30, -40)
        ax.set_xlabel('$x_1$', fontsize=15)
        ax.set_ylabel('$x_2$', fontsize=15)
        ax.set_zlabel('$x_3$', fontsize=15)
        plt.savefig(filename + ".png", dpi=300)
        plt.savefig(filename + ".pdf", dpi=300)
        plt.show()
        print("DONE")
        ########################################################################################

        ########################################################################################
        # ------ Visualize Gammas and Vector Field! -- Make self-contained function ------ #
        fig1 = plt.figure()
        ax1 = plt.axes(projection='3d')
        plt.title('HBS Modulated DS', fontsize=15)
        ax.set_xlim3d(grid_limits_x[0], grid_limits_x[1])
        ax.set_ylim3d(grid_limits_y[0], grid_limits_y[1])
        ax.set_zlim3d(grid_limits_z[0], grid_limits_z[1])
        filename = "./dynamical_system_modulation_svm/figures/svmlearnedGamma_combined_modDS_3D"

        X_OBS = X[:, Y == 1]
        # GAMMA_SCORE_OBS = gamma_score[Y == 1]
        ax1.scatter3D(X_OBS[0, :],
                      X_OBS[1, :],
                      X_OBS[2, :],
                      edgecolor="r",
                      facecolor="gold")

        # Add vector field!
        print("Computing the Vector Field.")
        # NOT NECESSARY!!!!
        # Create data for 3D Visualization of vector fields
        xx, yy, zz = np.meshgrid(
            np.linspace(grid_limits_x[0], grid_limits_x[1], grid_size),
            np.linspace(grid_limits_y[0], grid_limits_y[1], grid_size),
            np.linspace(grid_limits_z[0], grid_limits_z[1], grid_size))
        normal_vecs = normal_vecs.reshape(3, xx.shape[0], xx.shape[1],
                                          xx.shape[2])
        uu, vv, ww = np.meshgrid(
            np.linspace(grid_limits_x[0], grid_limits_x[1], grid_size),
            np.linspace(grid_limits_y[0], grid_limits_y[1], grid_size),
            np.linspace(grid_limits_z[0], grid_limits_z[1], grid_size))
        gamma_vals = gamma_vals.reshape(xx.shape)
        print(gamma_vals.shape)
        normal_vecs = normal_vecs.reshape(3, xx.shape[0], xx.shape[1],
                                          xx.shape[2])
        for i in range(grid_size):
            for j in range(grid_size):
                for k in range(grid_size):
                    x_query = np.array([xx[i, j, k], yy[i, j, k], zz[i, j, k]])
                    orig_ds = modulation_svm.linear_controller(
                        x_query, x_target)
                    print(gamma_vals[i, j, k])
                    print("orig_ds", orig_ds)
                    mod_x_dot = modulation_svm.modulation_singleGamma_HBS_multiRef(
                        query_pt=x_query,
                        orig_ds=orig_ds,
                        gamma_query=gamma_vals[i, j, k],
                        normal_vec_query=normal_vecs[:, i, j, k],
                        obstacle_reference_points=reference_points,
                        repulsive_gammaMargin=0.01)
                    x_dot_norm = mod_x_dot / np.linalg.norm(mod_x_dot +
                                                            epsilon) * 0.05
                    uu[i, j] = x_dot_norm[0]
                    vv[i, j] = x_dot_norm[1]
                    ww[i, j] = x_dot_norm[2]

        ax1.quiver(xx,
                   yy,
                   zz,
                   uu,
                   vv,
                   ww,
                   length=0.05,
                   normalize=True,
                   alpha=0.1)
        ax1.view_init(30, -40)
        print("Done plotting vector field.")

        # Integrate trajectories from initial point
        repetitions = 10
        for i in range(repetitions):
            x_target = rand_target_loc()
            x, x_dot = modulation_svm.forward_integrate_singleGamma_HBS(
                x_initial,
                x_target,
                learned_obstacles,
                dt=0.05,
                eps=0.03,
                max_N=10000)
            ax1.scatter(x.T[0, :],
                        x.T[1, :],
                        x.T[2, :],
                        edgecolor="b",
                        facecolor="blue")

        print("reference_points", reference_points)
        reference_point = reference_points[0]
        print("reference point 1: ", reference_point)
        ax1.scatter3D([reference_point[0]], [reference_point[1]],
                      [reference_point[2]],
                      edgecolor="r",
                      facecolor="red")

        reference_point = reference_points[1]
        print("reference point 2: ", reference_point)
        ax1.scatter3D([reference_point[0]], [reference_point[1]],
                      [reference_point[2]],
                      edgecolor="r",
                      facecolor="red")

        ax1.view_init(30, -40)
        ax1.set_xlabel('$x_1$', fontsize=15)
        ax1.set_ylabel('$x_2$', fontsize=15)
        ax1.set_zlabel('$x_3$', fontsize=15)
        plt.savefig(filename + ".png", dpi=300)
        plt.savefig(filename + ".pdf", dpi=300)
        plt.show()
        print("DONE")

    else:
        # -- Implement the other option of using multiple gamma functions later --- #:
        print("TODO: Implement plotting of multiple gamma functions..")
Esempio n. 2
0
def test2D_HBS_svmlearnedGammas(x_target, gamma_type, which_data):
    # Using 2D data from epfl-lasa dataset
    if (which_data == 0):
        grid_size = 50
        X, Y = learn_gamma_fn.read_data_lasa(
            "dynamical_system_modulation_svm/data/twoObstacles_environment.txt"
        )
        gamma_svm = 20
        c_svm = 20
        grid_limits_x = [0, 1]
        grid_limits_y = [0, 1]
        c_labels = []
        dt = 0.03
        x_initial = np.array([0.0, 0.65])

    # Using 2D data from irg-frank/table setup
    if (which_data == 1):
        grid_size = 50
        X, Y, c_labels = create_franka_dataset(dimension=2,
                                               grid_size=grid_size,
                                               plot_training_data=0)
        gamma_svm = 20
        c_svm = 10
        grid_limits_x = [-0.8, 0.8]
        grid_limits_y = [0.55, 1.3]
        dt = 0.03
        x_initial = np.array([0.0, 1.221])

    # Learn Gamma Function/s!
    if not gamma_type:
        # Same SVM for all Gammas (i.e. normals will be the same)
        learned_obstacles = learn_gamma_fn.create_obstacles_from_data(
            data=X,
            label=Y,
            plot_raw_data=False,
            gamma_svm=gamma_svm,
            c_svm=c_svm,
            cluster_labels=c_labels)
    else:
        # Independent SVMs for each Gammas (i.e. normals will be different at each grid state)
        learned_obstacles = learn_gamma_fn.create_obstacles_from_data_multi(
            data=X,
            label=Y,
            plot_raw_data=False,
            gamma_svm=gamma_svm,
            c_svm=c_svm,
            cluster_labels=c_labels)

    # Create Data for plotting
    xx, yy = np.meshgrid(
        np.linspace(grid_limits_x[0], grid_limits_x[1], grid_size),
        np.linspace(grid_limits_y[0], grid_limits_y[1], grid_size))
    positions = np.c_[xx.ravel(), yy.ravel()].T

    # -- Draw Normal Vector Field of Gamma Functions and Modulated DS --- #
    if not gamma_type:
        # This will use the single gamma function formulation (which is not entirely correct due to handling of the reference points)
        classifier = learned_obstacles['classifier']
        max_dist = learned_obstacles['max_dist']
        reference_points = learned_obstacles['reference_points']
        gamma_svm = learned_obstacles['gamma_svm']
        n_obstacles = learned_obstacles['n_obstacles']
        filename = "./dynamical_system_modulation_svm/figures/svmlearnedGamma_combined_2D"

        normal_vecs = learn_gamma_fn.get_normal_direction(positions,
                                                          classifier,
                                                          reference_points,
                                                          max_dist,
                                                          gamma_svm=gamma_svm)
        fig, ax = learn_gamma_fn.draw_contour_map(classifier,
                                                  max_dist,
                                                  reference_points,
                                                  gamma_value=True,
                                                  normal_vecs=normal_vecs,
                                                  grid_limits_x=grid_limits_x,
                                                  grid_limits_y=grid_limits_y,
                                                  grid_size=grid_size,
                                                  data=X[:, Y == 1])
        fig.savefig(filename + ".png", dpi=300)
        fig.savefig(filename + ".pdf", dpi=300)

        gamma_vals = learn_gamma_fn.get_gamma(positions, classifier, max_dist,
                                              reference_points)
        normal_vecs = learn_gamma_fn.get_normal_direction(positions,
                                                          classifier,
                                                          reference_points,
                                                          max_dist,
                                                          gamma_svm=gamma_svm)
        gamma_vals = gamma_vals.reshape(xx.shape)
        normal_vecs = normal_vecs.reshape(2, xx.shape[0], xx.shape[1])

        filename = "./dynamical_system_modulation_svm/figures/svmlearnedGamma_combined_modDS_2D"

        # MODULATION CONSIDERS ALL REFERENCE POINTS
        modulation_svm.draw_modulated_svmGamma_HBS(x_target,
                                                   reference_points,
                                                   gamma_vals,
                                                   normal_vecs,
                                                   n_obstacles,
                                                   grid_limits_x,
                                                   grid_limits_y,
                                                   grid_size,
                                                   x_initial,
                                                   learned_obstacles,
                                                   dt,
                                                   filename,
                                                   data=X[:, Y == 1])

    else:
        for oo in range(len(learned_obstacles)):
            classifier = learned_obstacles[oo]['classifier']
            max_dist = learned_obstacles[oo]['max_dist']
            reference_point = learned_obstacles[oo]['reference_point']
            gamma_svm = learned_obstacles[oo]['gamma_svm']

            filename = './dynamical_system_modulation_svm/figures/svmlearnedGamma_obstacle_{}_2D.png'.format(
                oo)

            normal_vecs = learn_gamma_fn.get_normal_direction(
                positions,
                classifier,
                reference_point,
                max_dist,
                gamma_svm=gamma_svm)
            fig, ax = learn_gamma_fn.draw_contour_map(classifier,
                                                      max_dist,
                                                      reference_point,
                                                      gamma_value=True,
                                                      normal_vecs=normal_vecs,
                                                      grid_limits=grid_limits,
                                                      grid_size=grid_size)
            fig.savefig(filename)

            print("Doing modulation for obstacle {}".format(oo))
            gamma_vals = learn_gamma_fn.get_gamma(positions, classifier,
                                                  max_dist, reference_point)

            # This will use the single gamma function formulation (which
            gamma_vals = gamma_vals.reshape(xx.shape)
            normal_vecs = normal_vecs.reshape(2, xx.shape[0], xx.shape[1])
            filename = "./dynamical_system_modulation_svm/figures/svmlearnedGamma_obstacle_{}_modDS_2D.png".format(
                oo)
            modulation_svm.raw_modulated_svmGamma_HBS(x_target,
                                                      reference_point,
                                                      gamma_vals, normal_vecs,
                                                      1, grid_limits,
                                                      grid_size, filename)

        print("Doing combined modulation of all obstacles")
        filename = "./dynamical_system_modulation_svm/figures/multisvmlearnedGamma_ALLobstacles_modDS_2D.png"
        modulation_svm.draw_modulated_multisvmGamma_HBS(
            x_target, learned_obstacles, grid_limits, grid_size, filename)
def learn2D_HBS_svmlearnedGammas(x_target, sim_type):

    grid_size = 50
    print('Generating Dataset')
    if sim_type == 'gaz':
        X, Y, c_labels = create_franka_dataset_gaz_coord(dimension=2,
                                                         grid_size=grid_size,
                                                         plot_training_data=0,
                                                         with_wall=0)
    elif sim_type == 'pyb':
        X, Y, c_labels = create_franka_dataset_pyb_coord(dimension=2,
                                                         grid_size=grid_size,
                                                         plot_training_data=0,
                                                         with_wall=0)
    print('DONE')

    # Same for both reference frames
    gamma_svm = 20
    c_svm = 10
    grid_limits_x = [-0.8, 0.8]
    grid_limits_y = [0.55, 1.3]
    dt = 0.03
    x_initial = np.array([0.0, 1.221])

    # Same SVM for all Gammas (i.e. normals will be the same)
    print('Learning Gamma function')
    learned_obstacles = learn_gamma_fn.create_obstacles_from_data(
        data=X,
        label=Y,
        plot_raw_data=False,
        gamma_svm=gamma_svm,
        c_svm=c_svm,
        cluster_labels=c_labels)
    print('Done')

    # Create Data for plotting
    xx, yy = np.meshgrid(
        np.linspace(grid_limits_x[0], grid_limits_x[1], grid_size),
        np.linspace(grid_limits_y[0], grid_limits_y[1], grid_size))
    positions = np.c_[xx.ravel(), yy.ravel()].T

    # -- Draw Normal Vector Field of Gamma Functions and Modulated DS with integrated trajectories--- #
    classifier = learned_obstacles['classifier']
    max_dist = learned_obstacles['max_dist']
    reference_points = learned_obstacles['reference_points']
    gamma_svm = learned_obstacles['gamma_svm']
    n_obstacles = learned_obstacles['n_obstacles']
    filename = "./dynamical_system_modulation_svm/figures/svmlearnedGamma_combined_2D"
    if sim_type == 'gaz':
        filename = filename + "_gaz"
    else:
        filename = filename + "_pyb"

    normal_vecs = learn_gamma_fn.get_normal_direction(positions,
                                                      classifier,
                                                      reference_points,
                                                      max_dist,
                                                      gamma_svm=gamma_svm)
    fig, ax = learn_gamma_fn.draw_contour_map(classifier,
                                              max_dist,
                                              reference_points,
                                              gamma_value=True,
                                              normal_vecs=normal_vecs,
                                              grid_limits_x=grid_limits_x,
                                              grid_limits_y=grid_limits_y,
                                              grid_size=grid_size)
    fig.savefig(filename + ".png", dpi=300)
    fig.savefig(filename + ".pdf", dpi=300)

    gamma_vals = learn_gamma_fn.get_gamma(positions, classifier, max_dist,
                                          reference_points)
    # normal_vecs = learn_gamma_fn.get_normal_direction(positions, classifier, reference_points, max_dist, gamma_svm=gamma_svm)
    gamma_vals = gamma_vals.reshape(xx.shape)
    normal_vecs = normal_vecs.reshape(2, xx.shape[0], xx.shape[1])

    filename = filename + "_modDS"

    # MODULATION CONSIDERS ALL REFERENCE POINTS
    modulation_svm.draw_modulated_svmGamma_HBS(x_target, reference_points,
                                               gamma_vals, normal_vecs,
                                               n_obstacles, grid_limits_x,
                                               grid_limits_y, grid_size,
                                               x_initial, learned_obstacles,
                                               dt, filename)
def learn3D_HBS_svmlearnedGammas(x_target, x_initial, sim_type, with_wall):
    # Create 3D Dataset
    grid_size = 30
    view_normals = 0
    view_streamlines = 1
    override_refPts = 1

    print('Generating Dataset')

    if override_refPts:
        if sim_type == 'pyb':
            reference_points = np.array([[0, -0.122,
                                          0.975], [0.0, 0.39, 0.776],
                                         [0.0, -0.122, 0.61]])
        else:
            reference_points = np.array([[0.478, 0, 0.975], [0.99, 0, 0.776],
                                         [0.478, 0, 0.61]])
    else:
        reference_points = []

    if sim_type == 'gaz':
        X, Y, c_labels = create_franka_dataset_gaz_coord(dimension=3,
                                                         grid_size=grid_size,
                                                         plot_training_data=0,
                                                         with_wall=with_wall)
        grid_limits_x = [0.1, 1.0]
        grid_limits_y = [-0.8, 0.8]
        grid_limits_z = [0.55, 1.3]
    elif sim_type == 'pyb':
        X, Y, c_labels = create_franka_dataset_pyb_coord(dimension=3,
                                                         grid_size=grid_size,
                                                         plot_training_data=0,
                                                         with_wall=with_wall)
        grid_limits_x = [-0.8, 0.8]
        grid_limits_y = [-0.5, 0.4]
        grid_limits_z = [0.55, 1.3]
    print('DONE')

    gamma_svm = 20
    c_svm = 20
    dt = 0.03

    # Same SVM for all Gammas (i.e. normals will be the same)
    print('Learning Gamma function')
    learned_obstacles = learn_gamma_fn.create_obstacles_from_data(
        data=X,
        label=Y,
        plot_raw_data=False,
        gamma_svm=gamma_svm,
        c_svm=c_svm,
        cluster_labels=c_labels,
        reference_points=reference_points)
    print('Done')

    # Save model!
    gamma_svm_model = (learned_obstacles, gamma_svm, c_svm)
    if with_wall:
        filename = "./dynamical_system_modulation_svm/models/gammaSVM_frankaROCUS_bounded"
    else:
        filename = "./dynamical_system_modulation_svm/models/gammaSVM_frankaROCUS"

    if sim_type == 'gaz':
        pickle.dump(gamma_svm_model, open(filename + "_gaz.pkl", 'wb'))
    else:
        pickle.dump(gamma_svm_model, open(filename + "_pyb.pkl", 'wb'))

    # -- Draw Normal Vector Field of Gamma Functions and Modulated DS --- #
    xx, yy, zz = np.meshgrid(
        np.linspace(grid_limits_x[0], grid_limits_x[1], grid_size),
        np.linspace(grid_limits_y[0], grid_limits_y[1], grid_size),
        np.linspace(grid_limits_z[0], grid_limits_z[1], grid_size))
    positions = np.c_[xx.ravel(), yy.ravel(), zz.ravel()].T

    classifier = learned_obstacles['classifier']
    max_dist = learned_obstacles['max_dist']
    reference_points = learned_obstacles['reference_points']
    gamma_svm = learned_obstacles['gamma_svm']
    n_obstacles = learned_obstacles['n_obstacles']
    filename = "./dynamical_system_modulation_svm/figures/svmlearnedGamma_combined_3D"
    if sim_type == 'gaz':
        filename = filename + "_gaz"
    else:
        filename = filename + "_pyb"

    print("Computing Gammas and Normals..")
    gamma_vals = learn_gamma_fn.get_gamma(positions,
                                          classifier,
                                          max_dist,
                                          reference_points,
                                          dimension=3)
    normal_vecs = learn_gamma_fn.get_normal_direction(positions,
                                                      classifier,
                                                      reference_points,
                                                      max_dist,
                                                      gamma_svm=gamma_svm,
                                                      dimension=3)

    if view_normals:
        ########################################################################################
        # ------ Visualize Gammas and Normal Vectors! -- Make self-contained function ------ #
        fig = plt.figure()
        ax = plt.axes(projection='3d')
        plt.title("$\Gamma$-Score")
        ax.set_xlim3d(grid_limits_x[0], grid_limits_x[1])
        ax.set_ylim3d(grid_limits_y[0], grid_limits_y[1])
        ax.set_zlim3d(grid_limits_z[0], grid_limits_z[1])
        ax.scatter3D(X[0, :],
                     X[1, :],
                     X[2, :],
                     '.',
                     c=gamma_vals,
                     cmap=plt.cm.coolwarm,
                     alpha=0.10)

        normal_vecs = normal_vecs.reshape(X.shape)
        ax.quiver(X[0, :],
                  X[1, :],
                  X[2, :],
                  normal_vecs[0, :],
                  normal_vecs[1, :],
                  normal_vecs[2, :],
                  length=0.05,
                  normalize=True)
        ax.view_init(30, 160)
        ax.set_xlabel('$x_1$', fontsize=15)
        ax.set_ylabel('$x_2$', fontsize=15)
        ax.set_zlabel('$x_3$', fontsize=15)
        plt.savefig(filename + ".png", dpi=300)
        plt.savefig(filename + ".pdf", dpi=300)
        plt.show()
        print("DONE")
        ########################################################################################

    if view_streamlines:
        ########################################################################################
        # ------ Visualize Gammas and Vector Field! -- Make self-contained function ------ #
        fig1 = plt.figure()
        ax1 = plt.axes(projection='3d')
        plt.title('HBS Modulated DS', fontsize=15)
        ax1.set_xlim3d(grid_limits_x[0], grid_limits_x[1])
        ax1.set_ylim3d(grid_limits_y[0], grid_limits_y[1])
        ax1.set_zlim3d(grid_limits_z[0], grid_limits_z[1])
        filename = filename + "_modDS"
        X_OBS = X[:, Y == 1]

        # GAMMA_SCORE_OBS = gamma_score[Y == 1]
        # ax1.scatter3D(X_OBS[0,:],X_OBS[1,:], X_OBS[2,:],edgecolor="r", facecolor="gold");

        vertical_wall = [0, 0.1, 0.825], [0.01, 0.8, 0.4]
        horizontal_wall = [0, 0.1, 1.025], [0.7, 0.8, 0.01]
        table_top = [0, 0, 0.6], [1.5, 1, 0.05]
        plot_box(ax1, *vertical_wall, **{'color': 'C1'})
        plot_box(ax1, *horizontal_wall, **{'color': 'C1'})
        plot_box(ax1, *table_top, **{'color': 'C1'})

        # Integrate trajectories from initial point
        repetitions = 10
        for i in range(repetitions):
            x_target = rand_target_loc(sim_type)
            x, x_dot = modulation_svm.forward_integrate_singleGamma_HBS(
                x_initial,
                x_target,
                learned_obstacles,
                dt=0.05,
                eps=0.03,
                max_N=10000)
            ax1.scatter(x.T[0, :],
                        x.T[1, :],
                        x.T[2, :],
                        edgecolor="b",
                        facecolor="blue")

        for oo in range(len(reference_points)):
            reference_point = reference_points[oo]
            print("reference point 1: ", reference_point)
            ax1.scatter3D([reference_point[0]], [reference_point[1]],
                          [reference_point[2]],
                          edgecolor="r",
                          facecolor="red")

        ax1.view_init(0, 180)
        ax1.set_xlabel('$x_1$', fontsize=15)
        ax1.set_ylabel('$x_2$', fontsize=15)
        ax1.set_zlabel('$x_3$', fontsize=15)
        plt.savefig(filename + ".png", dpi=300)
        plt.savefig(filename + ".pdf", dpi=300)
        plt.show()
        print("DONE")
Esempio n. 5
0
rospy.Subscriber("DS_target", PointStamped, get_DS_target)
rospy.Subscriber("curr_ee_pos", PointStamped, get_ee_position)
pub_gamma = rospy.Publisher("gamma_values", PointCloud2, queue_size=2)
pub_fw_int = rospy.Publisher("DS_path", Path, queue_size=2)
# pub_fw_int = rospy.Publisher("/DS/forward_integration", PointCloud2, queue_size=2)

if re_learn:
    # Create Environment Dataset and Learn Gamma!
    X, Y, c_labels = test_modulation_svm.create_franka_dataset(
        dimension=3, grid_size=grid_size, plot_training_data=0)
    gamma_svm = 20
    c_svm = 20
    learned_gamma = learn_gamma_fn.create_obstacles_from_data(
        data=X,
        label=Y,
        plot_raw_data=False,
        gamma_svm=gamma_svm,
        c_svm=c_svm,
        cluster_labels=c_labels)
else:
    # Load Pre-Learned Model
    learned_gamma, gamma_svm, c_svm = pickle.load(
        open(
            "/home/nbfigueroa/code/bayes-probe-robotics/dynamical_system_modulation_svm/models/gammaSVM_frankaROCUS.pkl",
            'rb'))

points = create_points_gamma()
fields = [
    PointField('x', 0, PointField.FLOAT32, 1),
    PointField('y', 4, PointField.FLOAT32, 1),
    PointField('z', 8, PointField.FLOAT32, 1),