def __init__(self, basis, num_nodes=2, num_pts=50):
self.basis = basis
self.num_pts = num_pts
grid = np.meshgrid(*[np.linspace(0, 1, num_pts) for _ in range(2)])
self.grid = np.c_[grid[0].ravel(), grid[1].ravel()]
# Initialize with Uniform Exploration
self.grid_vals = self.init_uniform_grid(self.grid)
self._phik = convert_phi2phik(self.basis, self.grid_vals, self.grid)
self.has_update = False
# Update for Environmental Stimuli (EEs & DDs)
self.dd_means = []
self.ee_means = []
self.dd_vars = []
self.ee_vars = []
self._ee = False
self._dd = False
rospy.Subscriber('/ee_loc', Pose, self.ee_callback)
rospy.Subscriber('/dd_loc', Pose, self.dd_callback)
# Update for Tanvas Inputs
self._tanvas = False
rospy.Subscriber('/input', input_array, self.tanvas_callback)
def tanvas_callback(self, data):
if data.datalen != 0:
self._tanvas = True
print('received tanvas input')
tan_x = data.xinput
tan_y = data.yinput
grid_lenx = 50
grid_leny = 50
tan_arr = np.ones((grid_lenx, grid_leny)) * .05 #*.0001
for i in range(data.datalen):
tan_arr[tan_x[i], tan_y[i]] = 1.0
tan_arr = np.transpose(tan_arr)
tan_arr = tan_arr.ravel()
if np.max(tan_arr) > 0:
temp = tan_arr
target_dist = temp
target_dist /= np.sum(target_dist)
self.tanvas_dist = target_dist
grid = np.meshgrid(
*[np.linspace(0, 1, grid_lenx),
np.linspace(0, 1, grid_leny)])
self.grid = np.c_[grid[0].ravel(), grid[1].ravel()]
# self._phik = convert_phi2phik(self.basis,target_dist, self.grid)
ee_val = np.zeros(self.grid.shape[0])
ee_scale = np.ones(self.grid.shape[0])
if self._ee:
for m, v in zip(self.ee_means, self.ee_vars):
innerds = np.sum((self.grid - m)**2 / v, 1)
ee_val += np.exp(
-innerds / 2.0) # / np.sqrt((2*np.pi)**2 * np.prod(v))
ee_val /= np.sum(ee_val)
ee_scale = ee_val
dd_val = np.zeros(self.grid.shape[0])
dd_scale = np.ones(self.grid.shape[0])
if self._dd:
for m, v in zip(self.dd_means, self.dd_vars):
innerds = np.sum((self.grid - m)**2 / v, 1)
dd_val += np.exp(
-innerds / 2.0) # / np.sqrt((2*np.pi)**2 * np.prod(v))
# Invert DD distribution
dd_val -= np.max(dd_val)
dd_val = np.abs(dd_val) #+1e-5
dd_val /= np.sum(dd_val)
dd_scale = dd_val
val = (target_dist + ee_val + dd_val) * dd_scale
# normalizes the distribution
val /= np.sum(val)
self.grid_vals = val
self._phik = convert_phi2phik(self.basis, self.grid_vals,
self.grid)
self.has_update = True
def run(self):
while not rospy.is_shutdown():
# If necessary, update target distribution:
if self.t_dist.has_update == True:
self.controller.phik = convert_phi2phik(
self.controller.basis, self.t_dist.grid_vals)
self.t_dist.has_update = False
comm_link = True
for _ck in self.ck_list:
if _ck is None:
comm_link = False
# Update ck in controller
if comm_link:
ctrl = self.controller(self.state, self.ck_list,
self.agent_num)
else:
ctrl = self.controller(self.state)
# Publish Current Ck
ck_msg = Ck_msg()
ck_msg.agent_num = self.agent_num
ck_msg.ck_array = self.controller.ck.copy()
self.ck_pub.publish(ck_msg)
state = self.step(ctrl)
self.broadcast.sendTransform(
(state[0], state[1], 1.0),
(0, 0, 0, 1), # quaternion
rospy.Time.now(),
self.agent_name,
"world")
pnt = Point()
pnt.x = state[0]
pnt.y = state[1]
pnt.z = 1.0
if len(self.marker.points) < self.__max_points:
self.marker.points.append(pnt)
else:
self.marker.points[:-1] = self.marker.points[1:]
self.marker.points[-1] = pnt
self.marker.pose.position.x = 0 * state[0]
self.marker.pose.position.y = 0 * state[1]
self.marker.pose.position.z = 0 * 1.0
self.marker_pub.publish(self.marker)
self.rate.sleep()
def __init__(self, agent_num=0):
SingleIntegrator.__init__(self)
self.agent_num = agent_num
self.agent_name = 'agent{}'.format(agent_num)
self.model = SingleIntegrator()
self.t_dist = TargetDist()
self.controller = RTErgodicControl(self.model,
self.t_dist,
horizon=15,
num_basis=10,
batch_size=100)
self.controller.phik = convert_phi2phik(self.controller.basis,
self.t_dist.grid_vals,
self.t_dist.grid)
self.reset() # need to override this
def dd_callback(self, data):
print("updating dd location dist")
self.dd_means.append(np.array([data.position.x, data.position.y]))
self.dd_vars.append(np.array([0.1, 0.1]))
self._dd = True
ee_val = np.zeros(self.grid.shape[0])
ee_scale = np.ones(self.grid.shape[0])
if self._ee:
for m, v in zip(self.ee_means, self.ee_vars):
innerds = np.sum((self.grid - m)**2 / v, 1)
ee_val += np.exp(-innerds /
2.0) # / np.sqrt((2*np.pi)**2 * np.prod(v))
ee_val /= np.sum(ee_val)
ee_scale = ee_val
dd_val = np.zeros(self.grid.shape[0])
dd_scale = np.ones(self.grid.shape[0])
for m, v in zip(self.dd_means, self.dd_vars):
innerds = np.sum((self.grid - m)**2 / v, 1)
dd_val += np.exp(-innerds /
2.0) # / np.sqrt((2*np.pi)**2 * np.prod(v))
dd_scale = dd_val
# Invert DD distribution
dd_val -= np.max(dd_val)
dd_val = np.abs(dd_val) #+1e-5
dd_val /= np.sum(dd_val)
if self._tanvas:
val = (self.tanvas_dist + ee_val + dd_val) * dd_scale
else:
val = (ee_val + dd_val) * dd_scale
# normalizes the distribution
val /= np.sum(val)
self.grid_vals = val
self._phik = convert_phi2phik(self.basis, self.grid_vals, self.grid)
from utils import convert_phi2phik, convert_phik2phi
import matplotlib.pyplot as plt
# define a target distribution object
t_dist = TargetDist()
# plot first fig, original target dist
fig1 = plt.figure()
ax1_1 = fig1.add_subplot(121)
ax1_1.set_aspect('equal')
ax1_1.set_xlim(0, 1)
ax1_1.set_ylim(0, 1)
xy, vals = t_dist.get_grid_spec()
ax1_1.contourf(*xy, vals, levels=20)
# compute phik from target distribution phi
# phik is determined once phi is determined
phik = convert_phi2phik(basis, t_dist.grid_vals, t_dist.grid)
print(phik.shape) # square of num_basis
# convert phik back to phi
phi = convert_phik2phi(basis, phik, t_dist.grid)
# plot the reconstructed phi
ax1_2 = fig1.add_subplot(122)
ax1_2.set_aspect('equal')
ax1_2.set_xlim(0, 1)
ax1_2.set_ylim(0, 1)
ax1_2.contourf(*xy, phi.reshape(50, 50))
# plt.pause(0.1)
plt.close()
############################################
# test ergodic controller
from turtlebot_dyn import TurtlebotDyn
def __init__(self, agent_num=0, tot_agents=1):
DoubleIntegrator.__init__(self)
rospy.init_node('agent{}'.format(agent_num))
self.rate = rospy.Rate(30)
self.agent_num = agent_num
self.tot_agents = tot_agents
self.agent_name = 'agent{}'.format(agent_num)
self.model = DoubleIntegrator()
self.t_dist = TargetDist(
num_nodes=3) #TODO: remove example target distribution
self.controller = RTErgodicControl(self.model,
self.t_dist,
horizon=15,
num_basis=5,
batch_size=200,
capacity=500) #, batch_size=-1)
# setting the phik on the ergodic controller
self.controller.phik = convert_phi2phik(self.controller.basis,
self.t_dist.grid_vals,
self.t_dist.grid)
self.tdist_sub = rospy.Subscriber('/target_distribution', Target_dist,
self.update_tdist)
self.reset() # reset the agent
self.broadcast = tf.TransformBroadcaster()
self.broadcast.sendTransform(
(self.state[0], self.state[1], 1.0),
(0, 0, 0, 1), # quaternion
rospy.Time.now(),
self.agent_name,
"world")
self.__max_points = 1000
self.marker_pub = rospy.Publisher(
'/agent{}/marker_pose'.format(agent_num), Marker, queue_size=1)
self.marker = Marker()
self.marker.id = agent_num
self.marker.type = Marker.LINE_STRIP
self.marker.header.frame_id = "world"
self.marker.scale.x = .01
self.marker.scale.y = .01
self.marker.scale.z = .01
color = [0.0, 0.0, 0.0]
color[agent_num] = 1.0
self.marker.color.a = .7
self.marker.color.r = color[0]
self.marker.color.g = color[1]
self.marker.color.b = color[2]
# Publisher for Ck Sharing
self.ck_pub = rospy.Publisher('agent{}/ck'.format(agent_num),
Ck_msg,
queue_size=1)
# Ck Subscriber for all agent ck's
self.ck_list = [None] * tot_agents
for i in range(tot_agents):
rospy.Subscriber('agent{}/ck'.format(i), Ck_msg, self.update_ck)
