def __init__(self, agent_name, model,
weights=None, t_horizon=10, num_basis=5,
capacity=100000, batch_size=20):
self._agent_name = agent_name
self._model = model
self._t_horizon = t_horizon
self._replay_buffer = ReplayBuffer(capacity)
self._batch_size = batch_size
self._basis = Basis(self._model.explr_space, num_basis=num_basis)
self._lamk = np.exp(-0.8*np.linalg.norm(self._basis.k, axis=1))
self._barr = Barrier(self._model.explr_space)
self._targ_dist = TargetDist(basis=self._basis)
self._u = [0.0*np.zeros(self._model.action_space.shape[0])
for _ in range(t_horizon)]
# TODO: implement more weights
if weights is None:
weights = {'R' : np.eye(self._model.action_space.shape[0])}
self._Rinv = np.linalg.inv(weights['R'])
self._phik = None
self._ck_mean = None
self._ck_msg = Ck()
self._ck_msg.name = self._agent_name
self._ck_dict = {}
self.pred_path = []
# set up the eos stuff last
rospy.Subscriber('ck_link', Ck, self._ck_link_callback)
self._ck_pub = rospy.Publisher('ck_link', Ck, queue_size=1)
def __init__(self):
# basic config
self.pose = np.array([0.1, 0.1, 0.])
self.obsv = []
# ts = message_filters.ApproximateTimeSynchronizer([self.odom_sub, self.scan_sub], 10, 0.01)
# ts.registerCallback(self.callback)
self.bearings = np.linspace(0, 2 * pi, 360)
self.start_time = time.time()
# ergodic control config
self.size = 4.0
self.explr_space = Box(np.array([0.0, 0.0]),
np.array([self.size, self.size]),
dtype=np.float64)
self.basis = Basis(explr_space=self.explr_space, num_basis=10)
# self.t_dist = TargetDist()
self.t_dist = TargetDist(means=[[1.0, 1.0], [3.0, 3.0]],
cov=0.1,
size=self.size)
self.phik = convert_phi2phik(self.basis, self.t_dist.grid_vals,
self.t_dist.grid, self.size)
self.weights = {'R': np.diag([10, 1])}
self.model = TurtlebotDyn(size=self.size, dt=0.1)
self.erg_ctrl = RTErgodicControl(self.model,
self.t_dist,
weights=self.weights,
horizon=80,
num_basis=10,
batch_size=500)
self.obstacles = np.array([[1., 2.], [2., 1.], [2., 2.], [3., 1.],
[1., 3.], [2., 3.], [3., 2.]])
self.erg_ctrl.barr.update_obstacles(self.obstacles)
self.erg_ctrl.phik = self.phik
self.log = {'traj': [], 'ctrls': [], 'ctrl_seq': [], 'count': 0}
# self.odom_sub = message_filters.Subscriber('/odom', Odometry)#, self.odom_callback)
# self.scan_sub = message_filters.Subscriber('/scan', LaserScan)#, self.scan_callback)
self.odom_sub = rospy.Subscriber('/odom', Odometry, self.odom_callback)
self.scan_sub = rospy.Subscriber('/scan', LaserScan,
self.scan_callback)
self.ctrl_sub = rospy.Subscriber('/ctrl_flag', Bool,
self.ctrl_callback)
self.obsv_pub = rospy.Publisher('/landmarks', Marker, queue_size=1)
self.ctrl_pub = rospy.Publisher('/cmd_vel', Twist, queue_size=1)
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 __init__(self):
# basic config
self.pose = np.array([0.1, 0.1, 0.])
self.obsv = []
self.bearings = np.linspace(0, 2*pi, 360)
self.start_time = time.time()
# ergodic control config
self.size = 4.0
self.explr_space = Box(np.array([0.0,0.0]), np.array([self.size,self.size]), dtype=np.float64)
self.basis = Basis(explr_space=self.explr_space, num_basis=10)
# self.t_dist = TargetDist()
self.t_dist = TargetDist(means=[[1.0,1.0],[3.0,3.0]], cov=0.1, size=self.size)
self.phik = convert_phi2phik(self.basis, self.t_dist.grid_vals, self.t_dist.grid, self.size)
self.weights = {'R': np.diag([10, 1])}
self.model = TurtlebotDyn(size=self.size, dt=0.1)
self.erg_ctrl = RTErgodicControl(self.model, self.t_dist, weights=self.weights, horizon=80, num_basis=10, batch_size=500)
self.obstacles = np.array([[1.,2.], [2.,1.], [2.,2.], [3.,1.], [1.,3.], [2.,3.], [3.,2.]])
self.erg_ctrl.barr.update_obstacles(self.obstacles)
self.erg_ctrl.phik = self.phik
self.log = {'traj':[], 'ctrls':[], 'ctrl_seq':[], 'count':0}
self.odom_sub = rospy.Subscriber('/odom', Odometry, self.odom_callback)
self.scan_sub = rospy.Subscriber('/scan', LaserScan, self.scan_callback)
self.ctrl_sub = rospy.Subscriber('/ctrl_flag', Bool, self.ctrl_callback)
self.obsv_pub = rospy.Publisher('/landmarks', Marker, queue_size=1)
self.ctrl_pub = rospy.Publisher('/cmd_vel', Twist, queue_size=1)
self.path_pub = rospy.Publisher('/test_path', Path, queue_size=1)
self.map_pub = rospy.Publisher('/landmarks_map', Marker, queue_size=1)
self.path_msg = Path()
self.odom_header = None
#######
# for test only
self.old_obsv = []
self.curr_obsv = []
self.lm_table = []
def __init__(self):
# basic config
self.pose = np.array([0.1, 0.1, 0.])
self.obsv = []
self.bearings = np.linspace(0, 2 * pi, 360)
self.start_time = time.time()
# ergodic control config
self.size = 4.0
self.explr_space = Box(np.array([0.0, 0.0]),
np.array([self.size, self.size]),
dtype=np.float64)
self.basis = Basis(explr_space=self.explr_space, num_basis=10)
# self.t_dist = TargetDist()
self.t_dist = TargetDist(means=[[1.0, 1.0], [3.0, 3.0]],
cov=0.1,
size=self.size)
self.weights = {'R': np.diag([10, 1])}
self.model = TurtlebotDyn(size=self.size, dt=0.1)
self.erg_ctrl = RTErgodicControl(self.model,
self.t_dist,
weights=self.weights,
horizon=80,
num_basis=10,
batch_size=500)
self.obstacles = np.array([[1., 2.], [2., 1.], [2., 2.], [3., 1.],
[1., 3.], [2., 3.], [3., 2.]])
self.erg_ctrl.barr.update_obstacles(self.obstacles)
self.phik = convert_phi2phik(self.basis, self.t_dist.grid_vals,
self.t_dist.grid, self.size)
self.erg_ctrl.phik = self.phik
self.log = {'traj': [], 'ctrls': [], 'ctrl_seq': [], 'count': 0}
self.odom_sub = rospy.Subscriber('/odom', Odometry, self.odom_callback)
self.scan_sub = rospy.Subscriber('/scan', LaserScan,
self.scan_callback)
self.ctrl_sub = rospy.Subscriber('/ctrl_flag', Bool,
self.ctrl_callback)
self.obsv_pub = rospy.Publisher('/landmarks', Marker, queue_size=1)
self.ctrl_pub = rospy.Publisher('/cmd_vel', Twist, queue_size=1)
self.path_pub = rospy.Publisher('/test_path', Path, queue_size=1)
self.map_pub = rospy.Publisher('/landmarks_map', Marker, queue_size=1)
self.ekf_pub = rospy.Publisher('/ekf_odom', Odometry, queue_size=1)
self.path_msg = Path()
self.odom_header = None
#######
# for test only
self.old_obsv = []
self.curr_obsv = []
self.lm_table = []
cube_list = Marker()
cube_list.header.frame_id = 'odom'
cube_list.header.stamp = rospy.Time.now()
cube_list.ns = 'landmark_map'
cube_list.action = Marker.ADD
cube_list.pose.orientation.w = 1.0
cube_list.id = 0
cube_list.type = Marker.CUBE_LIST
cube_list.scale.x = 0.05
cube_list.scale.y = 0.05
cube_list.scale.z = 0.5
cube_list.color.b = 1.0
cube_list.color.a = 1.0
self.cube_list = cube_list
self.real_landmarks = np.array([
[2.34, 2.13], # id: 0
[0.60, 0.60], # id: 1
[0.60, 3.40], # id: 2
[3.40, 0.60], # id: 3
[3.40, 3.40]
]) # id: 4
self.init_pose = np.zeros(3)
self.raw_odom_traj = []
self.lm_id = []
#######
# ekf
self.ekf_mean = np.array([0.0, 0.0, 0.0])
self.ekf_cov = np.diag([1e-09 for _ in range(3)])
self.ekf_R = np.diag([0.01, 0.01, 0.01])
self.ekf_Q = np.diag([0.03, 0.03])
self.init_flag = False
# define the Barrier object
barrier = Barrier(explr_space)
# test cost function
print(barrier.cost(explr_space.sample() - 1.0))
# test derivative of cost function wrt to pose
print(barrier.dx(explr_space.sample() - 1.0))
###########################################
# target distribution test
from target_dist import TargetDist
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
import sys
sys.path.append('../rt_erg_lib')
import numpy as np
import matplotlib.pyplot as plt
from matplotlib import animation
from tqdm import tqdm
from single_integrator import SingleIntegrator
from ergodic_control import RTErgodicControl
from target_dist import TargetDist
from utils import convert_phi2phik, convert_ck2dist, convert_traj2ck, convert_phik2phi
env = SingleIntegrator()
model = SingleIntegrator()
t_dist = TargetDist(num_nodes=2)
weights = {'R': np.diag([5 for _ in range(2)])}
erg_ctrl = RTErgodicControl(model,
t_dist,
horizon=15,
num_basis=10,
batch_size=-1,
weights=weights)
erg_ctrl.phik = convert_phi2phik(erg_ctrl.basis, t_dist.grid_vals, t_dist.grid)
tf = 200
state = env.reset()
log = {'trajectory': [state.copy()]}
for t in tqdm(range(tf)):
ctrl = erg_ctrl(state, turtle_mode=False)
# define the Barrier object
barrier = Barrier(explr_space)
# test cost function
print(barrier.cost(explr_space.sample() - size))
# test derivative of cost function wrt to pose
print(barrier.dx(explr_space.sample() - size))
###########################################
# target distribution test
from target_dist import TargetDist
from utils import convert_phi2phik, convert_phik2phi
import matplotlib.pyplot as plt
# define a target distribution object
t_dist = TargetDist(
) #TargetDist(size=size, means=[[1.0,1.0],[3.0,3.0]], cov=0.1)
# plot first fig, original target dist
fig1 = plt.figure()
ax1_1 = fig1.add_subplot(121)
ax1_1.set_aspect('equal')
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, size)
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')
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)