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):
# 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
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
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):
# plot the reconstructed phi
ax1_2 = fig1.add_subplot(122)
ax1_2.set_aspect('equal')
ax1_2.contourf(*xy, phi.reshape(50, 50), levels=20)
plt.show()
plt.close()
############################################
# test ergodic controller
from turtlebot_dyn import TurtlebotDyn
from ergodic_control import RTErgodicControl
from utils import convert_ck2dist
from time import time
# configure simulation
env = TurtlebotDyn(size=size)
model = TurtlebotDyn(size=size)
weights = {'R': np.diag([10, 1])}
erg_ctrl = RTErgodicControl(model,
t_dist,
weights=weights,
horizon=80,
num_basis=5,
batch_size=500)
erg_ctrl.phik = convert_phi2phik(erg_ctrl.basis, t_dist.grid_vals, t_dist.grid)
tf = 1200
# ros configuration
import rospy
from nav_msgs.msg import Odometry
from geometry_msgs.msg import Twist