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..")
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")
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),