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)