# 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 :)')
traj = np.array(log['traj'])
ctrls = np.array(log['ctrls'])
ctrl_seq = np.array(log['ctrl_seq'])
# randomly plot ctrl sequences
fig3 = plt.figure()
num_tests = 5
for i in range(num_tests):
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
from numpy import sin, cos, pi, arcsin, arctan2
rospy.init_node('ergodic_controller')
pub = rospy.Publisher('/cmd_vel', Twist, queue_size=1)
rate = rospy.Rate(10)
# start simulation
log = {'traj': [], 'ctrls': [], 'ctrl_seq': [], 'count': 0}
state = env.reset(np.array([0.1, 0.1, 0.0]))
def callback(msg):
# read odometry
rx = msg.pose.pose.position.x
ry = msg.pose.pose.position.y
q = msg.pose.pose.orientation
rth = arctan2(2 * q.x * q.y - 2 * q.z * q.w, 1 - 2 * q.y**2 - 2 * q.z**2)
rth = 2 * pi - rth % (2 * pi)
state = np.array([rx, ry, rth]) #+ np.array([0.1, 0.1, 0.0])
# compute control signal
start_time = time()
ctrl = erg_ctrl(state.copy())
ctrl_lin = ctrl[0]
ctrl_ang = ctrl[1]