def plot_table_guess(query,
demo,
obj_weights,
abs_weights,
var_obj_weights,
var_abs_weights,
title=""):
global count
count += 1
plt.subplot(6, 6, count)
#plot placement of objects
for i in range(len(query)):
plt.plot(query[i, 0],
query[i, 1],
'o',
label="object " + str(i + 1),
markersize=10)
#plot pi_map
query_rbf = autils.RbfComplexReward(query, obj_weights, abs_weights)
pi_map, _ = query_rbf.estimate_best_placement()
plt.plot(pi_map[0], pi_map[1], 'kv', markersize=10, label="MAP")
var_rbf = autils.RbfComplexReward(query, var_obj_weights, var_abs_weights)
pi_var, _ = var_rbf.estimate_best_placement()
plt.plot(pi_var[0], pi_var[1], 's', markersize=10, label="$\alpha$-VaR")
plt.plot(demo[0], demo[1], '*', markersize=10, label="demo")
#plt.legend()
plt.xticks([])
plt.yticks([])
plt.axis([0, 1, 0, 1])
plt.title(title)
def plot_table_guess_demo(query,
demo,
obj_weights,
abs_weights,
var_obj_weights,
var_abs_weights,
title=""):
plt.subplot(2, 3, 1)
#plot placement of objects
for i in range(len(query)):
plt.plot(query[i, 0],
query[i, 1],
'o',
label="object " + str(i + 1),
markersize=10)
#plot pi_map
query_rbf = autils.RbfComplexReward(query, obj_weights, abs_weights)
pi_map, _ = query_rbf.estimate_best_placement()
var_rbf = autils.RbfComplexReward(query, var_obj_weights, var_abs_weights)
pi_var, _ = var_rbf.estimate_best_placement()
#plt.plot([pi_map[0], demo[0]],[pi_map[1], demo[1]],'r:')
plt.plot([pi_map[0], pi_var[0]], [pi_map[1], pi_var[1]],
':',
color='purple')
plt.plot(pi_map[0],
pi_map[1],
'kv',
markersize=10,
label="MAP Prediction",
fillstyle='none',
markeredgewidth=2)
plt.plot(pi_var[0],
pi_var[1],
's',
markersize=10,
label="VaR Prediction",
fillstyle='none',
markeredgewidth=2)
plt.plot(demo[0], demo[0], '*', markersize=10, label="demo")
#plt.legend()
plt.xticks([])
plt.yticks([])
plt.axis('square')
plt.axis([0, 1, 0, 1])
plt.legend(bbox_to_anchor=(1.1, 0))
plt.title(title)
def calculate_policy_loss(config, hyp_params, map_params):
#calculate reward for optimal placement under hyp_reward
hyp_obj_weights, hyp_abs_weights = hyp_params
hyp_reward_fn = active_utils.RbfComplexReward(config, hyp_obj_weights, hyp_abs_weights)
#get optimal placement under the hypothesis reward function and new configuration
hyp_placement, hyp_return = hyp_reward_fn.estimate_best_placement()
#calculate reward for map placement under hyp_reward
map_obj_weights, map_abs_weights = map_params
map_reward_fn = active_utils.RbfComplexReward(config, map_obj_weights, map_abs_weights)
#get optimal placement under map reward function and new configuration
map_placement, _ = map_reward_fn.estimate_best_placement()
map_return = hyp_reward_fn.get_reward(map_placement)
return hyp_return - map_return
def find_max_placement_var_config(num_objs, birl, num_configs=10, alpha = 0.95, random_queries=False):
max_var = 0
query_config = None
query_rbf = None
if random_queries:
#query_config = generate_random_configuration(num_objs)
query_config = generate_one_tweak_configuration(num_objs,birl)
else:
#generate random config and evaluate VaR
for i in range(num_configs):
print "-------trying config", i
#new_config = generate_random_configuration(num_objs)
new_config = generate_one_tweak_configuration(num_objs, birl)
config_rbf = active_utils.RbfComplexReward(new_config, birl.get_map_params()[0], birl.get_map_params()[1])
#active_utils.visualize_reward(config_rbf, "config with pi")
#plt.show()
#print "new config", new_config
var, var_rbf = calculate_placement_var(alpha, new_config, birl)
#print "var", var
if var > max_var:
#print "best so far"
max_var = var
query_config = new_config
query_rbf = var_rbf
return query_config, max_var, query_rbf
def log_likelihood(self, hyp_obj_weights, hyp_abs_weights, granularity=10):
#print hyp_obj_weights
#print hyp_abs_weights
log_sum = 0.0
for centers, placement in self.demonstrations:
#to compute the optimal placement under the demo config
#make new rbf with hypothesis weights and widths
hyp_rbf = active_utils.RbfComplexReward(centers, hyp_obj_weights,
hyp_abs_weights)
#active_utils.visualize_reward(hyp_rbf, title="birl hyp")
placement_reward = hyp_rbf.get_reward(placement)
xspace = np.linspace(0, 1, granularity)
yspace = np.linspace(0, 1, granularity)
pairs = [(xi, yi) for xi in xspace for yi in yspace]
#for p in pairs:
# plt.plot(p[0],p[1],'x')
#print pairs
Z_exponents = []
for pair in pairs:
Z_exponents.append(self.beta * hyp_rbf.get_reward(pair))
#print Z_exponents
log_sum += self.beta * placement_reward - scipy.misc.logsumexp(
Z_exponents)
#print "likelihood:", np.exp(self.beta * placement_reward - scipy.misc.logsumexp(Z_exponents))
#plt.show()
return log_sum
def calculate_placement_loss(config, hyp_params, map_params):
#calculate reward for optimal placement under hyp_reward
hyp_obj_weights, hyp_abs_weights = hyp_params
hyp_reward_fn = active_utils.RbfComplexReward(config, hyp_obj_weights, hyp_abs_weights)
#active_utils.visualize_reward(hyp_reward_fn, "hypothesis reward")
#get optimal placement under the hypothesis reward function and new configuration
hyp_placement, _ = hyp_reward_fn.estimate_best_placement()
#calculate reward for map placement under hyp_reward
map_obj_weights, map_abs_weights = map_params
map_reward_fn = active_utils.RbfComplexReward(config, map_obj_weights, map_abs_weights)
#active_utils.visualize_reward(map_reward_fn, "map reward")
#get optimal placement under map reward function and new configuration
map_placement, _ = map_reward_fn.estimate_best_placement()
#print "placement loss", np.linalg.norm(hyp_placement - map_placement)
#plt.show()
return np.linalg.norm(hyp_placement - map_placement)
def plot_table_heat_map(query, obj_weights, abs_weights, title=""):
global count
count += 1
plt.subplot(6, 6, count)
zero_obj_weights = np.zeros(obj_weights.shape)
table_rbf = autils.RbfComplexReward(query, zero_obj_weights, abs_weights)
n = 50
x = np.linspace(0., 1., n)
y = np.linspace(1., 0., n)
X, Y = np.meshgrid(x, y)
Z = table_rbf.rbf_heat(X, Y)
plt.pcolormesh(X, Y, Z, cmap='hot', vmin=-.2, vmax=.35)
plt.colorbar()
#plt.imshow(heatmap, cmap='hot')
#plt.colorbar()
plt.xticks([])
plt.yticks([])
plt.axis([0, 1, 0, 1])
plt.title(title)
def plot_object_heat_map(query, obj_weights, abs_weights, title=""):
global count
count += 1
plt.subplot(6, 6, count)
zero_abs_weights = np.zeros(abs_weights.shape)
obj_rbf = autils.RbfComplexReward(query, obj_weights, zero_abs_weights)
n = 50
x = np.linspace(0., 1., n)
y = np.linspace(1., 0., n)
X, Y = np.meshgrid(x, y)
Z = obj_rbf.rbf_heat(X, Y)
plt.pcolormesh(X, Y, Z, cmap='hot', vmin=-0.2, vmax=0.35)
plt.colorbar()
for c in obj_rbf.obj_centers:
plt.plot(c[0], c[1], 'o', markersize=10)
#plt.imshow(heatmap, cmap='hot')
#plt.colorbar()
plt.xticks([])
plt.yticks([])
plt.axis([0, 1, 0, 1])
plt.title(title)
def construct_world(objects=None):
# # # # # #
#marker0
#
#
#
#
global frames_client,v,c,b,p, goal_pose
printed = False
while not (0 in markers):
if not printed:
rospy.loginfo("Waiting for marker 0 ...")
printed = True
rospy.loginfo("Constructing working environment ...")
width = 0.9
height = 0.5
rospy.loginfo("workspace size:"+str(width)+" " +str(height))
marker0_transform = tfBuffer.lookup_transform('linear_actuator_link','ar_marker_0',rospy.Time(0), rospy.Duration(1.0))
ps = geometry_msgs.msg.PoseStamped()
ps.header.stamp = rospy.Time.now()
pt = tf2_geometry_msgs.do_transform_pose(ps, marker0_transform)
pt.header.stamp = rospy.Time.now()
pt.header.frame_id = 'linear_actuator_link'
tf_frames_request = BroadcastObjectFramesRequest()
tf_frames_request.parent_frames = ["linear_actuator_link"]
tf_frames_request.child_frames = ["table"]
pose = geometry_msgs.msg.Pose()
pose.position.x = pt.pose.position.x
pose.position.y = pt.pose.position.y
pose.position.z = pt.pose.position.z + 0.05
pose.orientation.w = 1
tf_frames_request.poses = [pose]
tf_res = frames_client(tf_frames_request)
if tf_res is False:
rospy.logerr("Update failed")
return False
#raw_input('Workspace constructed, press Enter to continue extracting features...')
rospy.loginfo('Detecting Features ...')
if not objects:
objects = []
if v.get(): objects.append("vase")
if r.get(): objects.append("red_bowl")
if g.get(): objects.append("green_plate")
if b.get(): objects.append("blue_cup")
if o.get(): objects.append("orange_cup")
print objects
object_poses = {}
for obj in objects:
plant_transform = tfBuffer.lookup_transform('table',obj,rospy.Time(0), rospy.Duration(1.0))
ps = geometry_msgs.msg.PoseStamped()
ps.header.stamp = rospy.Time.now()
pt = tf2_geometry_msgs.do_transform_pose(ps, plant_transform)
pt.header.stamp = rospy.Time.now()
obj_x = pt.pose.position.x
obj_y = pt.pose.position.y
rospy.loginfo('---> '+obj+' position: '+str(obj_x)+", "+str(obj_y))
object_poses[obj] = (-obj_x,-obj_y)
centers = np.array(object_poses.values())
#get leanred weights for MAP Reward function
filename = "./flowers_seed0_randomFalse_demo10.txt"
_, _, obj_weights, abs_weights, _, _ = get_query_data(filename)
map_reward = autils.RbfComplexReward(centers, obj_weights, abs_weights)
pi_map, _ = map_reward.estimate_best_placement()
print(pi_map)
hlds = []
plt.xlim(0,width)
plt.ylim(height,0)
for obj in object_poses:
hlds.append(plt.scatter(object_poses[obj][1],object_poses[obj][0]))
plt.annotate(obj, (object_poses[obj][1],object_poses[obj][0]))
hlds.append(plt.scatter(pi_map[1],pi_map[0]))
plt.annotate('placement', (pi_map[1],pi_map[0]))
plt.show()
return True
import numpy as np import matplotlib.pyplot as plt import active_utils as autils from plot_flower_queries import get_query_data #object centers centers = np.array([[0.25, 0.75], [0.75, 0.75], [0.25, 0.25], [0.75, 0.25]]) #get leanred weights for MAP Reward function filename = "./data/flowers_seed0_randomFalse_demo10.txt" _, _, obj_weights, abs_weights, _, _ = get_query_data(filename) map_reward = autils.RbfComplexReward(centers, obj_weights, abs_weights) pi_map, _ = map_reward.estimate_best_placement() print(pi_map)
