def forward_integrate_singleGamma_HBS(x_initial, x_target, learned_gamma, dt,
eps, max_N):
'''
forward integration of the HBS modulation controller starting from x_initial,
toward x_target, with obstacles given as a list of gamma functions.
integration interval is dt, and N integration steps are performed.
return an (N+1) x d tensor for x trajectory and an N x d tensor for x_dot trajectory.
'''
# Parse Gamma
classifier = learned_gamma['classifier']
max_dist = learned_gamma['max_dist']
reference_points = learned_gamma['reference_points']
dim = len(x_target)
x_traj = []
x_traj.append(x_initial)
x_dot_traj = []
x_cur = x_initial
# print("Before Integration")
for i in range(max_N):
gamma_val = learn_gamma_fn.get_gamma(x_cur,
classifier,
max_dist,
reference_points,
dimension=dim)
normal_vec = learn_gamma_fn.get_normal_direction(x_cur,
classifier,
reference_points,
max_dist,
dimension=dim)
orig_ds = linear_controller(x_cur, x_target)
x_dot = modulation_singleGamma_HBS_multiRef(
query_pt=x_cur,
orig_ds=orig_ds,
gamma_query=gamma_val,
normal_vec_query=normal_vec.reshape(dim),
obstacle_reference_points=reference_points,
repulsive_gammaMargin=0.01)
x_dot = x_dot / np.linalg.norm(x_dot + epsilon) * 0.10
x_dot_traj.append(x_dot)
x_cur = x_cur + x_dot * dt
if np.linalg.norm(x_cur - x_target) < eps:
print("Attractor Reached")
break
x_traj.append(x_cur)
return np.stack(x_traj), np.stack(x_dot_traj)
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 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")
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 _compute_desired_velocities(self):
"""
Compute desired task-space velocities from state-dependent DS
"""
# --- Linear Controller with Modulated DS! --- #
pos_delta = self.ee_pos - self.DS_pos_att
self.pos_error = LA.norm(pos_delta)
# Compute Desired Linear Velocity
# orig_ds = -self.A_p.dot(pos_delta)
orig_ds = -pos_delta
rospy.loginfo("x_dot:{}".format(orig_ds))
gamma_val = learn_gamma_fn.get_gamma(np.array(self.ee_pos),
self.classifier,
self.max_dist,
self.reference_points,
dimension=3)
rospy.loginfo("gamma(x):{}".format(gamma_val))
normal_vec = learn_gamma_fn.get_normal_direction(np.array(self.ee_pos),
self.classifier,
self.reference_points,
self.max_dist,
dimension=3)
rospy.loginfo("gamma_grad(x):{}".format(normal_vec))
if gamma_val < 1:
rospy.loginfo(
'ROBOT IS COLLIDED REGION.. DO SOMETHING, CHARLIE!!!')
# if np.dot(orig_ds, normal_vec) > 0:
# x_dot_mod = orig_ds
# else:
x_dot_mod = modulation_svm.modulation_singleGamma_HBS_multiRef(
query_pt=np.array(self.ee_pos),
orig_ds=np.array(orig_ds),
gamma_query=gamma_val,
normal_vec_query=normal_vec.reshape(3),
obstacle_reference_points=self.reference_points,
repulsive_gammaMargin=0.01)
rospy.loginfo("x_dot_mod:{}".format(x_dot_mod))
lin_vel = self.scale_DS * x_dot_mod
# if self.ee_pos[2] > 1.0 and gamma_val < 1:
# lin_vel[2] = lin_vel[2] + 0.05
rospy.loginfo('Desired linear velocity: {}'.format(lin_vel))
# --- Compute desired attractor aligned to reference --- #
desired_rotation = np.array((3, 3))
if self.DS_attractor[1] < 0:
sign_y = -1
else:
sign_y = 1
R_y = sign_y * orig_ds / np.linalg.norm(orig_ds)
rospy.loginfo('R_y: {}'.format(R_y))
if self.ee_pos[2] > 1:
R_z = np.array([0, 0, -1])
else:
R_z = np.array([0, 0, 1])
rospy.loginfo('R_z: {}'.format(R_z))
# Make it orthogonal to n
R_y -= R_y.dot(R_z) * R_z / np.linalg.norm(R_z)**2
# normalize it
R_y /= np.linalg.norm(R_y)
R_x = np.cross(R_y, R_z)
desired_rotation = np.hstack([R_x, R_y, R_z])
rospy.loginfo('R_mod: {}'.format(desired_rotation))
quat_attr_ = Quaternion(matrix=self.ee_rot)
# --- Quaternion error --- #
# quat_attr_ = Quaternion(array=[self.DS_quat_att[3],self.DS_quat_att[0],self.DS_quat_att[1],self.DS_quat_att[2]])
# Difference between two quaternions
delta_quat = self.ee_quat * quat_attr_.conjugate
delta_log = Quaternion.log(delta_quat)
delta_log_np = np.array([
delta_log.elements[1], delta_log.elements[2], delta_log.elements[3]
])
self.quat_error = LA.norm(delta_log_np)
# --- Compute Desired Angular Velocity --- #
ang_vel = -self.A_o.dot(delta_log_np)
# --- Compute Desired Angular Velocity in ee-reference frame --- #
RT = self.ee_rot.transpose()
# Difference desired quaternion from desired angular velocity
# q_des = quat_mul(quat_exp(0.5 * quat_deriv(omega * self.dt )), q_curr);
delta_quat_des = Quaternion(array=[
0, ang_vel[0] * self.dt, ang_vel[1] * self.dt, ang_vel[2] * self.dt
])
q_des = Quaternion.exp(0.5 * delta_quat_des) * self.ee_quat
RTR_des = RT.dot(q_des.rotation_matrix)
#scale angular velocity
w = 1 - math.tanh(gamma_val)
rospy.loginfo('w: {}'.format(w))
ang_vel_rot = 2 * RTR_des.dot(ang_vel)
rospy.loginfo('omega: {}'.format(ang_vel_rot))
# Comments: there seems to be no difference when using velocity control.. but maybe
# it is necessary for torque-controlled robots... need to check with the kuka simulation or panda?
return lin_vel, ang_vel_rot
def gamma_grad(self, x, dim=3):
return learn_gamma_fn.get_normal_direction(np.array(x),
self.classifier,
self.reference_points,
self.max_dist,
dimension=3)
def modulation_multiGamma_HBS_svm(query_pt,
orig_ds,
learned_gammas,
repulsive_gammaMargin=0.01,
sticky_surface=False):
'''
Computes modulated velocity for an environment described by a single gamma function
and multiple reference points (describing multiple obstacles)
'''
dim = len(query_pt)
gamma_vals = []
for oo in range(len(learned_gammas)):
gamma_vals.append(
learn_gamma_fn.get_gamma(query_pt.reshape(dim, 1),
learned_gammas[oo]['classifier'],
learned_gammas[oo]['max_dist'],
learned_gammas[oo]['reference_point']))
gamma_vals = np.array(gamma_vals)
if (len(gamma_vals) == 1
and gamma_vals < 1.0) or (len(gamma_vals) > 1
and any(gamma_vals < 1.0)):
return np.zeros(orig_ds.shape)
if (len(gamma_vals) == 1
and gamma_vals > 1e9) or (len(gamma_vals) > 1
and any(gamma_vals > 1e9)):
return orig_ds
if len(obstacle_reference_points.shape) > 1:
reference_point = find_closest_reference_point(
query_pt, obstacle_reference_points)
else:
reference_point = obstacle_reference_points
# Add: Move away from center/reference point in case of a collision
if 0 < gamma_query < (1 + repulsive_gammaMargin):
repulsive_power = 5
repulsive_factor = 5
repulsive_gamma = (1 + repulsive_gammaMargin)
repulsive_speed = ((repulsive_gamma / gamma_query)**repulsive_power -
repulsive_gamma) * repulsive_factor
repulsive_reference_direction = query_pt - reference_point
x_dot_mod = repulsive_speed * (
1 / 2) * (repulsive_reference_direction /
np.linalg.norm(repulsive_reference_direction) +
orig_ds / np.linalg.norm(orig_ds))
print('SLIDING ON SURFACE!')
elif gamma_query < 0:
x_dot_mod = np.zeros(orig_ds.shape)
else:
# Calculate real modulated dynamical system
M = modulation_singleGamma_HBS(x=query_pt,
orig_ds=orig_ds,
normal_vec=normal_vec_query,
gamma_pt=gamma_query,
reference_point=reference_point)
x_dot_mod = np.matmul(M, orig_ds.reshape(-1, 1)).flatten()
ms = np.log(gamma_vals - 1 + epsilon)
logprod = ms.sum()
bs = np.exp(logprod - ms)
weights = bs / bs.sum()
weights = weights.T[0]
# calculate modulated dynamical systems
x_dot_mods = []
for oo in range(len(learned_gammas)):
normal_vec = learn_gamma_fn.get_normal_direction(
query_pt.reshape(dim, 1),
classifier=learned_gammas[oo]['classifier'],
reference_point=learned_gammas[oo]['reference_point'],
max_dist=learned_gammas[oo]['max_dist'],
gamma_svm=learned_gammas[oo]['gamma_svm'])
x_dot_mod = modulation_singleGamma_HBS_multiRef(
query_pt=query_pt,
orig_ds=orig_ds,
gamma_query=gamma_vals[oo][0],
normal_vec_query=normal_vec.flatten(),
obstacle_reference_points=learned_gammas[oo]['reference_point'],
repulsive_gammaMargin=0.01)
x_dot_mods.append(x_dot_mod)
# calculate weighted average of magnitude
x_dot_mags = np.stack([np.linalg.norm(d) for d in x_dot_mods])
avg_mag = np.dot(weights, x_dot_mags)
old_way = True
if old_way:
# calculate kappa-space dynamical system and weighted average
kappas = []
es = null_space_bases(orig_ds)
bases = [orig_ds] + es
R = np.stack(bases).T
R = R / np.linalg.norm(R, axis=0)
for x_dot_mod in x_dot_mods:
n_x_dot_mod = x_dot_mod / np.linalg.norm(x_dot_mod)
# cob stands for change-of-basis
n_x_dot_mod_cob = np.matmul(R.T, n_x_dot_mod.reshape(-1,
1)).flatten()
n_x_dot_mod_cob = n_x_dot_mod_cob / 1.001
assert -1-1e-5 <= n_x_dot_mod_cob[0] <= 1+1e-5, \
'n_x_dot_mod_cob[0] = %0.2f?'%n_x_dot_mod_cob[0]
if n_x_dot_mod_cob[0] > 1:
acos = np.arccos(n_x_dot_mod_cob[0] - 1e-5)
elif n_x_dot_mod_cob[0] < -1:
acos = np.arccos(n_x_dot_mod_cob[0] + 1e-5)
else:
acos = np.arccos(n_x_dot_mod_cob[0])
if np.linalg.norm(n_x_dot_mod_cob[1:]) == 0:
kappa = acos * n_x_dot_mod_cob[1:] * 0
else:
kappa = acos * n_x_dot_mod_cob[1:] / np.linalg.norm(
n_x_dot_mod_cob[1:])
kappas.append(kappa)
kappas = np.stack(kappas).T
# matrix-vector multiplication as a weighted sum of columns
avg_kappa = np.matmul(kappas, weights.reshape(-1, 1)).flatten()
# map back to task space
norm = np.linalg.norm(avg_kappa)
if norm != 0:
avg_ds_dir = np.concatenate([
np.expand_dims(np.cos(norm), 0),
avg_kappa * np.sin(norm) / norm
])
else:
avg_ds_dir = np.concatenate(
[np.expand_dims(np.cos(norm), 0), avg_kappa])
avg_ds_dir = np.matmul(R, avg_ds_dir.reshape(-1, 1)).flatten()
else:
x_dot_mods_normalized = np.zeros((dim, len(learned_gammas)))
x_dot_mods_np = np.array(x_dot_mods).T
ind_nonzero = (x_dot_mags > 0)
if np.sum(ind_nonzero):
x_dot_mods_normalized[:,
ind_nonzero] = x_dot_mods_np[:, ind_nonzero] / np.tile(
x_dot_mags[ind_nonzero], (dim, 1))
x_dot_normalized = orig_ds / np.linalg.norm(orig_ds)
avg_ds_dir = get_directional_weighted_sum(
reference_direction=x_dot_normalized,
directions=x_dot_mods_normalized,
weights=weights,
total_weight=1)
x_mod_final = avg_mag * avg_ds_dir.squeeze()
x_mod_final = avg_mag * avg_ds_dir