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
plt.close()
############################################
# test ergodic controller
from turtlebot_dyn import TurtlebotDyn
from integrator_se2 import IntegratorSE2
from ergodic_control import RTErgodicControl
from utils import convert_ck2dist
# configure simulation
env = TurtlebotDyn()
model = TurtlebotDyn()
weights = {'R': np.diag([10, 1])}
erg_ctrl = RTErgodicControl(model,
t_dist,
weights=weights,
horizon=80,
num_basis=5,
batch_size=-1)
erg_ctrl.phik = convert_phi2phik(erg_ctrl.basis, t_dist.grid_vals, t_dist.grid)
tf = 1200
# start simulation
log = {'traj': [], 'ctrls': [], 'ctrl_seq': []}
state = env.reset()
for t in range(tf):
log['traj'].append(state.copy())
ctrl, ctrl_seq = erg_ctrl(state, seq=True)
log['ctrls'].append(ctrl.copy())
log['ctrl_seq'].append(ctrl_seq.copy())
state = env.step(ctrl)
print('simulation finished :)')
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)