def __init__(self, figure, ax, num_data, bfname, traj_dir):
self.figure = figure
self.ax = ax
self.press = False
self.vmp = VMP(dim=2, kernel_num=10)
self.bfname = bfname
self.traj_dir = traj_dir
if not os.path.exists(traj_dir):
try:
os.makedirs(traj_dir)
except OSError:
raise
self.id = 0
self.num_data = num_data
self.trqueries = np.random.uniform(low=(0, 0, 0, 10, 10, 0),
high=(10, 10, 2 * np.pi, 20, 20,
2 * np.pi),
size=(num_data, 6))
obsExp = ObsExp(exp_name="Docking")
self.envs = obsExp.get_envs(self.trqueries)
self.next()
self.connect()
def train_evaluate_mdnmp_for_hitball(mdnmp, trqueries, trvmps, tdata, max_epochs=20000, sample_num=1,
isvel=True, env_file="hitball_exp_v1.xml", isdraw=False, num_test=100, learning_rate=0.002, EXP=Armar6HitBallExpV1):
weights = np.ones(shape=(np.shape(trvmps)[0], 1))
train_weights = np.copy(weights)
mdnmp.build_mdn(learning_rate=learning_rate)
mdnmp.init_train()
isSuccess, nlls, ents = mdnmp.train(trqueries, trvmps, train_weights, max_epochs=max_epochs, is_load=False, is_save=False)
mp = VMP(dim=2, kernel_num=10)
if num_test > np.shape(tdata)[0]:
num_test = np.shape(tdata)[0]-1
srate = 0
if isSuccess:
tqueries = tdata[:num_test, 0:2]
starts = tdata[:num_test, 2:4]
goals = tdata[:num_test, 4:6]
wout, _ = mdnmp.predict(tqueries, sample_num)
wout = wout / 100
if isvel:
srate = evaluate_hitball(wout, tqueries, starts, goals,
low_ctrl=TaskSpaceVelocityController,
high_ctrl=TaskSpacePositionVMPController(mp),
env_path=ENV_DIR+env_file, EXP=EXP, isdraw=isdraw)
else:
srate = evaluate_hitball(wout, tqueries, starts, goals,
low_ctrl=TaskSpaceImpedanceController,
high_ctrl=TaskSpacePositionVMPController(mp),
env_path=ENV_DIR+env_file, EXP=EXP, isdraw=isdraw)
return srate, nlls, ents
def train_evaluate_baseline_for_hitball(model_name,
trqueries,
trvmps,
tdata,
sample_num=1,
isvel=True,
env_file="hitball_exp_v1.xml",
isdraw=False,
num_test=100,
EXP=Armar6HitBallExpV1):
model = possible_models[model_name]
if model is not None:
model.fit(trqueries, trvmps)
mp = VMP(dim=2, kernel_num=10)
if num_test > np.shape(tdata)[0]:
num_test = np.shape(tdata)[0] - 1
tqueries = tdata[:num_test, 0:2]
tstarts = tdata[:num_test, 2:4]
tgoals = tdata[:num_test, 4:6]
wmeans = model.predict(tqueries)
if model is not None:
wouts = sample_baseline(wmeans, sample_num)
else:
wouts = np.random.uniform(low=trvmps.min(),
high=trvmps.max(),
size=(100, sample_num, np.shape(trvmps)[1]))
wouts = wouts / 100
if isvel:
srate = evaluate_hitball(wouts,
tqueries,
tstarts,
tgoals,
low_ctrl=TaskSpaceVelocityController,
high_ctrl=TaskSpacePositionVMPController(mp),
env_path=ENV_DIR + env_file,
isdraw=isdraw,
EXP=EXP)
else:
srate = evaluate_hitball(wouts,
tqueries,
tstarts,
tgoals,
low_ctrl=TaskSpaceImpedanceController,
high_ctrl=TaskSpacePositionVMPController(mp),
env_path=ENV_DIR + env_file,
isdraw=isdraw,
EXP=EXP)
return srate
class CollectDocking:
def __init__(self, figure, ax, num_data, bfname, traj_dir):
self.figure = figure
self.ax = ax
self.press = False
self.vmp = VMP(dim=2, kernel_num=10)
self.bfname = bfname
self.traj_dir = traj_dir
if not os.path.exists(traj_dir):
try:
os.makedirs(traj_dir)
except OSError:
raise
self.id = 0
self.num_data = num_data
self.trqueries = np.random.uniform(low=(0, 0, 0, 10, 10, 0),
high=(10, 10, 2 * np.pi, 20, 20,
2 * np.pi),
size=(num_data, 6))
obsExp = ObsExp(exp_name="Docking")
self.envs = obsExp.get_envs(self.trqueries)
self.next()
self.connect()
def connect(self):
'connect to all the events we need'
self.cidpress = self.figure.canvas.mpl_connect('button_press_event',
self.on_press)
self.cidrelease = self.figure.canvas.mpl_connect(
'button_release_event', self.on_release)
self.cidmotion = self.figure.canvas.mpl_connect(
'motion_notify_event', self.on_motion)
self.cidkey = self.figure.canvas.mpl_connect('key_press_event',
self.on_key)
def on_press(self, event):
x, y = event.xdata, event.ydata
self.trajpoints.append(np.array([x, y]))
self.press = True
def on_motion(self, event):
if not self.press: return
x, y = event.xdata, event.ydata
self.ax.plot(x, y, 'r.')
self.trajpoints.append(np.array([x, y]))
self.figure.canvas.draw()
def on_release(self, event):
'on release we reset the press data'
self.press = False
traj = np.stack(self.trajpoints)
timestamps = np.linspace(0, 1, np.shape(traj)[0])
timestamps = np.expand_dims(timestamps, axis=1)
traj = np.concatenate([timestamps, traj], axis=1)
self.vmp.train(traj)
newTraj = self.vmp.roll(traj[0, 1:], traj[-1, 1:])
self.ax.cla()
self.ax.set_xlim(-5, 25)
self.ax.set_ylim(-5, 25)
self.ax.set_aspect('equal')
self.ax.plot(traj[:, 1], traj[:, 2], 'k-.')
self.ax.plot(newTraj[:, 1], newTraj[:, 2], 'g-')
self.env.plot(self.ax)
weight = self.vmp.get_flatten_weights()
starts = traj[0, 1:]
goals = traj[-1, 1:]
with open(self.bfname + "_" + "queries.csv", "a") as f:
cquery = np.expand_dims(self.current_query, axis=0)
np.savetxt(f, cquery, delimiter=',', fmt='%.3f')
with open(self.bfname + "_" + "weights.csv", "a") as f:
weight = np.expand_dims(weight, axis=1)
np.savetxt(f, weight.transpose(), delimiter=',', fmt='%.5f')
with open(self.bfname + "_" + "starts.csv", "a") as f:
starts = np.expand_dims(starts, axis=0)
np.savetxt(f, starts, delimiter=',', fmt='%.3f')
with open(self.bfname + "_" + "goals.csv", "a") as f:
goals = np.expand_dims(goals, axis=0)
np.savetxt(f, goals, delimiter=',', fmt='%.3f')
traj_fname = self.traj_dir + '/traj_' + str(self.id)
np.savetxt(traj_fname, traj, delimiter=',', fmt='%.3f')
# if self.id < self.num_data:
# self.next()
# else:
# self.disconnect()
def on_key(self, event):
if event.key == 'z':
self.next()
def next(self):
self.ax.cla()
self.ax.set_xlim(-5, 25)
self.ax.set_ylim(-5, 25)
self.ax.set_aspect('equal')
self.env = self.envs[self.id]
self.current_query = self.trqueries[self.id, :]
self.env.plot(self.ax)
self.trajpoints = []
msg = 'collect ' + str(self.id) + '-th data'
self.figure.suptitle(msg)
plt.draw()
self.id = self.id + 1
def disconnect(self):
'disconnect all the stored connection ids'
self.figure.canvas.mpl_disconnect(self.cidpress)
self.figure.canvas.mpl_disconnect(self.cidrelease)
self.figure.canvas.mpl_disconnect(self.cidmotion)
self.viewer.render()
tcp_xpos = self.sim.data.get_site_xpos(
self.high_ctrl.low_ctrl.tcp_name)
tcp_xmat = self.sim.data.get_site_xmat(
self.high_ctrl.low_ctrl.tcp_name)
target_quat = np.zeros(4)
mujoco_py.functions.mju_mat2Quat(
target_quat, np.reshape(tcp_xmat, (-1, ), order='C'))
start = np.concatenate([tcp_xpos, target_quat], axis=0)
ball_pos = self.sim.data.get_body_xpos("curling_ball")
return start, ball_pos.copy()
if __name__ == "__main__":
vmp = VMP(dim=2, elementary_type='minjerk')
high_ctrl = TaskSpacePositionVMPController(vmp)
env = Armar6CurlingExp(env_path=env_path,
low_ctrl=TaskSpaceVelocityController,
high_ctrl=high_ctrl,
isdraw=True)
env.is_ball_pos_change = False
env.render = True
target_pos = np.zeros(shape=(2, 3))
target_pos[0, :] = init_ball_pos.copy()
target_pos[1, :] = init_ball_pos.copy()
target_pos[0, 1] = target_pos[0, 1] + 0.8
target_pos[1, 1] = target_pos[1, 1] + 0.8
target_pos[0, 0] = target_pos[0, 0] - 0.1
def run_baselines_for_hitball(MAX_EXPNUM=20,
nsamples=[1, 10, 30, 50],
model_names=['Uniform', 'SVR', 'GPR'],
isvel=False,
env_file="hitball_exp_v0.xml",
data_dir="hitball_mpdata_v0",
EXP=Armar6HitBallExpV0):
data_dir = os.environ['MPGEN_DIR'] + EXP_DIR + data_dir
queries = np.loadtxt(data_dir + '/hitball_queries.csv', delimiter=',')
vmps = np.loadtxt(data_dir + '/hitball_weights.csv', delimiter=',')
starts = np.loadtxt(data_dir + '/hitball_starts.csv', delimiter=',')
goals = np.loadtxt(data_dir + '/hitball_goals.csv', delimiter=',')
if np.shape(queries)[-1] == np.shape(goals)[0]:
queries = np.expand_dims(queries, axis=-1)
inputs = np.concatenate([queries, starts, goals], axis=1)
d_input = np.shape(queries)[-1]
# prepare model
_svr = SVR(gamma='scale', C=1.0, epsilon=0.1)
svr = MultiDimSkRegressor(_svr)
kernel = RBF(42, (1e-2, 1e2)) + WhiteKernel()
_gpr = GaussianProcessRegressor(kernel=kernel, random_state=0)
gpr = MultiDimSkRegressor(_gpr)
mp = VMP(dim=2, kernel_num=10)
models = {'SVR': svr, 'GPR': gpr, 'Uniform': None}
rstates = np.random.randint(0, 100, size=MAX_EXPNUM)
n_test = 100
srates = {}
allres = np.zeros(shape=(len(model_names), MAX_EXPNUM, len(nsamples)))
for modelId in range(len(model_names)):
model = models[model_names[modelId]]
csrates = np.zeros(shape=(MAX_EXPNUM, len(nsamples)))
for expId in range(MAX_EXPNUM):
trdata, tdata, trvmps, tvmps = train_test_split(
inputs, vmps, test_size=0.9, random_state=rstates[expId])
print("use {} data for training and {} data for testing".format(
np.shape(trdata)[0],
np.shape(tdata)[0]))
print("======== Exp: {} with {} ========".format(
expId, model_names[modelId]))
trqueries = trdata[:, 0:d_input]
tqueries = tdata[:n_test, 0:d_input]
tstarts = tdata[:n_test, d_input:d_input + 2]
tgoals = tdata[:n_test, d_input + 2:]
if model is not None:
model.fit(trqueries, trvmps)
wmeans = model.predict(tqueries)
for sampleId in range(len(nsamples)):
if model is not None:
wouts = sample_baseline(wmeans, nsamples[sampleId])
else:
wouts = np.random.uniform(low=vmps.min(),
high=vmps.max(),
size=(n_test, nsamples[sampleId],
np.shape(vmps)[1]))
if isvel:
srate = evaluate_hitball(
wouts,
tqueries,
tstarts,
tgoals,
low_ctrl=TaskSpaceVelocityController,
high_ctrl=TaskSpacePositionVMPController(mp),
env_path=ENV_DIR + env_file,
EXP=EXP)
else:
srate = evaluate_hitball(
wouts,
tqueries,
tstarts,
tgoals,
low_ctrl=TaskSpaceImpedanceController,
high_ctrl=TaskSpacePositionVMPController(mp),
env_path=ENV_DIR + env_file,
EXP=EXP)
csrates[expId, sampleId] = srate
allres[modelId, expId, sampleId] = srate
srates[model_names[modelId]] = np.mean(csrates, axis=0)
return srates, allres
def train_evaluate_gmgan_for_hitball(gmgan,
trqueries,
trvmps,
tdata,
use_entropy=False,
max_epochs=20000,
sup_max_epoch=0,
isvel=True,
env_file="hitball_exp_v1.xml",
sample_num=1,
isdraw=False,
num_test=100,
g_lrate=0.002,
d_lrate=0.002,
EXP=Armar6HitBallExpV1):
if use_entropy:
gmgan.entropy_ratio = 1
else:
gmgan.entropy_ratio = 0
gmgan.lratio['entropy'] = 10
gmgan.lratio['adv_cost'] = 0
gmgan.gen_sup_lrate = g_lrate
gmgan.gen_adv_lrate = g_lrate
gmgan.dis_lrate = d_lrate
gmgan.sup_max_epoch = sup_max_epoch
train_input = np.random.uniform(low=np.min(trqueries, axis=0),
high=np.max(trqueries, axis=0),
size=(10000, np.shape(trqueries)[1]))
gmgan.create_network()
gmgan.init_train()
gmgan.train(train_context=train_input,
real_context=trqueries,
real_response=trvmps,
max_epochs=max_epochs,
is_load=False,
is_save=False)
mp = VMP(dim=2, kernel_num=10)
if num_test > np.shape(tdata)[0]:
num_test = np.shape(tdata)[0] - 1
tqueries = tdata[:num_test, 0:2]
starts = tdata[:num_test, 2:4]
goals = tdata[:num_test, 4:6]
wout = gmgan.generate(tqueries, 10000, sample_num)
if isvel:
srate = evaluate_hitball(wout,
tqueries,
starts,
goals,
low_ctrl=TaskSpaceVelocityController,
high_ctrl=TaskSpacePositionVMPController(mp),
env_path=ENV_DIR + env_file,
isdraw=isdraw,
EXP=EXP)
else:
srate = evaluate_hitball(wout,
tqueries,
starts,
goals,
low_ctrl=TaskSpaceImpedanceController,
high_ctrl=TaskSpacePositionVMPController(mp),
env_path=ENV_DIR + env_file,
isdraw=isdraw,
EXP=EXP)
return srate
default="hitball_mpdata_v0")
parser.add_option("-p",
"--draw",
action="store_true",
dest="is_draw",
default=False)
parser.add_option("-v",
"--version",
dest="exp_version",
type="string",
default="v1")
(options, args) = parser.parse_args(sys.argv)
mp_dir = options.mp_dir
vmp = VMP(dim=2, kernel_num=10, use_outrange_kernel=False)
ostarts = np.loadtxt(mp_dir + '/hitball_starts.csv', delimiter=',')
ogoals = np.loadtxt(mp_dir + '/hitball_goals.csv', delimiter=',')
oqueries = np.loadtxt(mp_dir + '/hitball_queries.csv', delimiter=',')
oweights = np.loadtxt(mp_dir + '/hitball_weights.csv', delimiter=',')
if options.exp_version == "v1":
env_path = env_dir + "hitball_exp_v1.xml"
env = Armar6HitBallExpV1(low_ctrl=TaskSpaceVelocityController,
high_ctrl=TaskSpacePositionVMPController(vmp),
env_path=env_path,
isdraw=options.is_draw)
elif options.exp_version == "v0":
env_path = env_dir + "hitball_exp_v0.xml"
env = Armar6HitBallExpV0(low_ctrl=TaskSpaceImpedanceController,
goals = np.loadtxt('../data/modeCollapse_goals.csv', delimiter=',')
vmps = np.loadtxt('../data/modeCollapse_weights.csv', delimiter=',')
weights = np.ones(shape=(np.shape(goals)[0], 1))
# generate exp
num_test = 20
obsExp = ObsExp(exp_name='goAroundObs')
nmodel = obsExp.nmodel
start = obsExp.start
colors = obsExp.colors
obs = obsExp.obs
testgoals = obsExp.gen_test(num_test=num_test)
knum = np.shape(vmps)[1]
dim = np.shape(goals)[1]
vmp = VMP(2, kernel_num=int(knum / dim), use_outrange_kernel=False)
train_goals = np.copy(goals)
train_ws = np.copy(vmps)
train_ispos = np.copy(weights)
testgoals = np.array([[0.1, 1]])
MAX_EXP = 3
n_samples = 5
fig, axes = plt.subplots(nrows=MAX_EXP, ncols=2)
if MAX_EXP == 1:
axes = np.expand_dims(axes, axis=0)
colors = [(0, 0.43, 0.73), (0, 0.73, 0.43)]
use_entropy_cost = [False, True]
def run_mdnmp_for_hitball(nmodel=3, MAX_EXPNUM=20, use_entropy_cost=[False, True],
model_names=["Original MDN", "Entropy MDN"], nsamples=[1, 10, 30, 50, 70],
env_file="hitball_exp_v0.xml", data_dir="hitball_mpdata_v0",
isvel=False, EXP=Armar6HitBallExpV0):
# prepare data
data_dir = os.environ['MPGEN_DIR'] + EXP_DIR + data_dir
queries = np.loadtxt(data_dir + '/hitball_queries.csv', delimiter=',')
vmps = np.loadtxt(data_dir + '/hitball_weights.csv', delimiter=',')
starts = np.loadtxt(data_dir + '/hitball_starts.csv', delimiter=',')
goals = np.loadtxt(data_dir + '/hitball_goals.csv', delimiter=',')
if np.shape(queries)[-1] == np.shape(goals)[0]:
queries = np.expand_dims(queries, axis=-1)
inputs = np.concatenate([queries, starts, goals], axis=1)
# prepare model
nn_structure = {'d_feat': 20,
'feat_layers': [40],
'mean_layers': [60],
'scale_layers': [60],
'mixing_layers': [10]}
d_input = np.shape(queries)[-1]
d_output = np.shape(vmps)[1]
mp = VMP(dim=2, kernel_num=10)
mdnmp = MDNMP(n_comps=nmodel, d_input=d_input, d_output=d_output, nn_structure=nn_structure, var_init=VAR_INIT)
rstates = np.random.randint(0, 100, size=MAX_EXPNUM)
n_test = 100
srates = {}
allres = np.zeros(shape=(len(model_names), MAX_EXPNUM, len(nsamples)))
for modelId in range(len(model_names)):
if use_entropy_cost[modelId]:
mdnmp.lratio['entropy'] = 20
else:
mdnmp.lratio['entropy'] = 0
csrates = np.zeros(shape=(MAX_EXPNUM,len(nsamples)))
for expId in range(MAX_EXPNUM):
mdnmp.build_mdn(learning_rate=0.0001)
mdnmp.init_train()
trdata, tdata, trvmps, tvmps = train_test_split(inputs, vmps, test_size=0.9, random_state=rstates[expId])
print("use {} data for training and {} data for testing".format(np.shape(trdata)[0], np.shape(tdata)[0]))
print("======== Exp: {} with {} ========".format(expId, model_names[modelId]))
is_pos = np.ones(shape=(np.shape(trvmps)[0], 1))
trqueries = trdata[:,0:d_input]
mdnmp.train(trqueries, trvmps, is_pos, max_epochs=10000, is_load=False, is_save=False)
tqueries = tdata[:n_test, 0:d_input]
tstarts = tdata[:n_test, d_input:d_input+2]
tgoals = tdata[:n_test, d_input+2:]
for sampleId in range(len(nsamples)):
wout, _ = mdnmp.predict(tqueries, nsamples[sampleId])
if isvel:
srate = evaluate_hitball(wout, tqueries, tstarts, tgoals,
low_ctrl=TaskSpaceVelocityController,
high_ctrl=TaskSpacePositionVMPController(mp),
env_path=ENV_DIR + env_file, EXP=EXP)
else:
srate = evaluate_hitball(wout, tqueries, tstarts, tgoals,
low_ctrl=TaskSpaceImpedanceController,
high_ctrl=TaskSpacePositionVMPController(mp),
env_path=ENV_DIR + env_file, EXP=EXP)
csrates[expId, sampleId] = srate
allres[modelId, expId, sampleId] = srate
srates[model_names[modelId]] = np.mean(csrates, axis=0)
return srates, allres
from mp.vmp import VMP
from mp.vmp_tools import VMPDataTools
from optparse import OptionParser
parser = OptionParser()
parser.add_option("-r",
"--raw_dir",
dest="raw_dir",
type="string",
default="hitball_mpdata_v0")
parser.add_option("-d",
"--mp_dir",
dest="mp_dir",
type="string",
default="hitball_dataset_v0")
(options, args) = parser.parse_args(sys.argv)
vmp = VMP(dim=2, kernel_num=10)
vmp_data_processor = VMPDataTools(vmp, options.raw_dir, "hitball")
weights, starts, goals = vmp_data_processor.get_flatten_weights_file()
np.savetxt(options.mp_dir + '/hitball_weights.csv',
np.stack(weights),
delimiter=',')
np.savetxt(options.mp_dir + '/hitball_starts.csv',
np.stack(starts),
delimiter=',')
np.savetxt(options.mp_dir + '/hitball_goals.csv',
np.stack(goals),
delimiter=',')
def evaluate_docking_for_all_models(mpweights,
mfs,
testqueries,
teststarts,
testgoals,
knum=10,
ndraw=4):
ndata = np.shape(mpweights)[0]
vmp = VMP(2, kernel_num=knum)
obsExp = ObsExp(exp_name="Docking")
envs = obsExp.get_envs(testqueries)
fig, axes = plt.subplots(nrows=ndraw, ncols=ndraw)
colors = [(232 / 255, 82 / 255, 88 / 255, 1),
(1 / 255, 129 / 255, 1 / 255, 1),
(4 / 255, 129 / 255, 205 / 255, 1)
] #cmap(np.linspace(0,1,nmodel))
k = 0
for i in range(ndata):
ws = mpweights[i, :, :]
iscollision = False
for j in range(np.shape(ws)[1]):
start = teststarts[i, :]
goal = testgoals[i, :]
env = envs[i]
mf = mfs[i, :]
vmp.set_weights(ws[:, np.argmax(mf)])
traj = vmp.roll(start, goal)
if np.linalg.norm(traj[0, 1:] -
start) > 1 or np.linalg.norm(traj[-1, 1:] -
goal) > 1:
iscollision = True
continue
if np.linalg.norm(testqueries[i, 0:2] -
start) > 1 or np.linalg.norm(
testqueries[i, 3:5] - goal) > 1:
iscollision = True
continue
for j in range(np.shape(traj)[0]):
iscollision = env.isCollision([traj[j, 1], traj[j, 2]])
if iscollision:
break
if not iscollision and k < ndraw * ndraw:
ax = int(np.floor(k / ndraw))
ay = int(k % ndraw)
axes[ax, ay].clear()
axes[ax, ay].set_ylim([-5, 25])
axes[ax, ay].set_xlim([-5, 25])
axes[ax, ay].set_yticklabels([])
axes[ax, ay].set_xticklabels([])
axes[ax, ay].set_aspect('equal')
axes[ax, ay].tick_params(length=0)
env.plot(axes[ax, ay])
axes[ax, ay].plot(traj[0, 1], traj[0, 2], 'k.')
axes[ax, ay].plot(traj[-1, 1], traj[-1, 2], 'k.')
for mid in range(len(mf)):
vmp.set_weights(ws[:, mid])
traj = vmp.roll(start, goal)
if mid == np.argmax(mf):
axes[ax, ay].plot(traj[:, 1],
traj[:, 2],
'-',
color=colors[mid],
linewidth=2)
else:
axes[ax, ay].plot(traj[:, 1],
traj[:, 2],
'-.',
color=colors[mid],
linewidth=2)
k = k + 1
plt.subplots_adjust(hspace=0.05, wspace=0.05)
plt.draw()
plt.pause(0.001)
plt.savefig('./docking_img.png')
def evaluate_docking(mpweights,
testqueries,
teststarts,
testgoals,
knum=10,
mua=None,
isdraw=False,
ndraw=3,
onlySuccess=True):
# mpweights: N x S x dim, N: number of experiments, S: number of samples, dim: dimension of MP
# testqueries: N x qdim, N: number of experiments, qdim: dimension of queries
# teststarts: N x 2
# testgoals: N x 2
success_num = 0
ndata = np.shape(mpweights)[0]
vmp = VMP(2, kernel_num=knum)
obsExp = ObsExp(exp_name="Docking")
envs = obsExp.get_envs(testqueries)
k = 0
if isdraw:
if not onlySuccess:
ids = np.random.choice(ndata, ndraw * ndraw, replace=False)
else:
ids = np.linspace(0, ndata, ndata)
fig, axes = plt.subplots(nrows=ndraw, ncols=ndraw)
successId = []
colors = [(232 / 255, 82 / 255, 88 / 255, 0.5),
(1 / 255, 129 / 255, 1 / 255, 0.5),
(4 / 255, 129 / 255, 205 / 255, 0.5)]
for i in range(ndata):
w = mpweights[i, :]
for j in range(np.shape(w)[0]):
vmp.set_weights(w[j, :])
start = teststarts[i, :]
goal = testgoals[i, :]
env = envs[i]
traj = vmp.roll(start, goal)
iscollision = False
if np.linalg.norm(traj[0, 1:] -
start) > 1 or np.linalg.norm(traj[-1, 1:] -
goal) > 1:
iscollision = True
continue
for j in range(np.shape(traj)[0]):
iscollision = env.isCollision([traj[j, 1], traj[j, 2]])
if iscollision:
break
if not iscollision:
success_num = success_num + 1
successId.append(i)
break
if isdraw:
if k < ndraw * ndraw:
if not onlySuccess or not iscollision:
ax = int(np.floor(k / ndraw))
ay = int(k % ndraw)
axes[ax, ay].clear()
axes[ax, ay].set_ylim([-5, 25])
axes[ax, ay].set_xlim([-5, 25])
axes[ax, ay].set_yticklabels([])
axes[ax, ay].set_xticklabels([])
axes[ax, ay].set_aspect('equal')
axes[ax, ay].tick_params(length=0)
# axes[ax, ay].axhline(linewidth=2)
# axes[ax, ay].axvline(linewidth=2)
env.plot(axes[ax, ay])
axes[ax, ay].plot(start[0], start[1], 'r.')
axes[ax, ay].plot(goal[0], goal[1], 'b.')
axes[ax, ay].plot(traj[:, 1],
traj[:, 2],
'k-',
linewidth=2)
if mua is not None:
for mid in range(np.shape(mua)[2]):
vmp.set_weights(mua[i, :, mid])
traj = vmp.roll(start, goal)
axes[ax, ay].plot(traj[:, 1],
traj[:, 2],
'-.',
color=colors[mid],
linewidth=2)
k = k + 1
print('success_num: %1d, ndata: %1d, success_rate: %.3f' %
(success_num, ndata, success_num / ndata))
if isdraw:
plt.subplots_adjust(hspace=0.05, wspace=0.05)
plt.draw()
plt.pause(0.001)
plt.savefig('./docking.png', format='png', dpi=1200)
return success_num / ndata, successId
def evaluate_circular_obstacle_avoidance(mpweights,
testqueries,
teststarts,
testgoals,
knum=10,
isdraw=False,
ndraw=4,
onlySuccess=False):
# mpweights: N x dim, N: number of experiments, dim: dimension of MP
# testqueries: N x qdim, N: number of experiments, qdim: dimension of queries
# teststarts: N x 2
# testgoals: N x 2
success_num = 0
ndata = np.shape(mpweights)[0]
vmp = VMP(2, kernel_num=knum)
obsExp = ObsExp(exp_name="goAroundObsR3V2")
envs = obsExp.get_envs(testqueries)
k = 0
if isdraw:
if not onlySuccess:
ids = np.random.choice(ndata, ndraw * ndraw, replace=False)
else:
ids = np.linspace(0, ndata, ndata)
fig, axes = plt.subplots(nrows=ndraw, ncols=ndraw)
for i in range(ndata):
w = mpweights[i, :]
vmp.set_weights(w)
start = teststarts[i, :]
goal = testgoals[i, :]
env = envs[i]
traj = vmp.roll(start, goal)
iscollision = False
for j in range(np.shape(traj)[0]):
iscollision = env.isCollision([traj[j, 1], traj[j, 2]])
if iscollision:
break
if not iscollision:
success_num = success_num + 1
if isdraw:
if i in ids:
if not onlySuccess or not iscollision:
ax = int(np.floor(k / ndraw))
ay = int(k % ndraw)
axes[ax, ay].clear()
axes[ax, ay].set_ylim([-10, 10])
axes[ax, ay].set_xlim([-10, 10])
axes[ax, ay].set_yticklabels([])
axes[ax, ay].set_xticklabels([])
env.plot(axes[ax, ay])
k = k + 1
if iscollision:
axes[ax, ay].plot(traj[:, 1],
traj[:, 2],
'-.',
color='r')
else:
axes[ax, ay].plot(traj[:, 1],
traj[:, 2],
'-',
color='b')
axes[ax, ay].plot(start[0], start[1], 'ro')
axes[ax, ay].plot(goal[0], goal[1], 'bo')
print('success_num: {}, ndata: {}'.format(success_num, ndata))
if isdraw:
plt.show()
return success_num / ndata