def __init__(self,
cost_model,
dynamics_model,
init_controls_model,
waypoints,
dt,
threshold=0.1,
debug=False,
lookahead=10,
max_rollout=5,
num_iter=20,
trust_region=0.1,
deterministic=True):
ControlLoop.__init__(self,
waypoints=waypoints,
dt=dt,
threshold=threshold,
debug=debug)
self.lookahead = lookahead
self.max_rollout = min(max_rollout, lookahead)
self.num_iter = num_iter
self.trust_region = trust_region
self.deterministic = deterministic
# Quadratic cost returns number given state, action
self.cost_model = cost_model
# Model of physics. Returns next state given time, [state, action] (in batches, so there's an extra dim we don't use)
self.dynamics_model = dynamics_model
# Policy. Returns action given time, state (not necessarily deterministic)
self.controls_model = init_controls_model
# Keep track of t since iLQR with lookahead was computed. If we pass max_rollout, recompute iLQR at the current state.
self.t_since_ilqr = -1
def set_waypoint(self, waypoint):
ControlLoop.set_waypoint(self, waypoint)
######################################
# Reset PID Loop #
######################################
'''
def set_waypoint(self, waypoint):
ControlLoop.set_waypoint(self, waypoint)
# Pad waypoint [x,y] with a zero to make it a state vector [x,y,theta]
self.waypoint_as_state = tf.cast(
tf.expand_dims(tf.pad(waypoint, tf.constant([[0, 1]])), axis=0),
tf.float32)
# Reset t since iLQR to -1, so that iLQR will be recomputed the next call to calc_action.
self.t_since_ilqr = -1
def __init__(self,
waypoints,
dt,
threshold=0.1,
debug=False): # Feel free to add more arguments
ControlLoop.__init__(self,
waypoints=waypoints,
dt=dt,
threshold=threshold,
debug=debug)
def set_waypoint(self, waypoint):
ControlLoop.set_waypoint(self, waypoint)
######################################
# Reset PID Loop #
######################################
'''
Reset PID terms each time a waypoint is set
If not reset, derivative term will spike
'''
self.last_error1 = 0
self.last_error2 = 0
self.total_error1 = 0
self.total_error2 = 0
def __init__(self,
waypoints,
dt,
threshold=0.1,
debug=False): # Feel free to add more arguments
ControlLoop.__init__(self,
waypoints=waypoints,
dt=dt,
threshold=threshold,
debug=debug)
######################################
# Initialize PID constants #
######################################
self.last_error1 = 0
self.last_error2 = 0
self.total_error1 = 0
self.total_error2 = 0
self.angle = 0
import time
from queue import Queue
from control_loop import ControlLoop
cqi = Queue()
cqo = Queue()
uri = "http://192.168.10.237:52261/"
c = ControlLoop(uri, cqi, cqo)
c.start()
time.sleep(2)
cqi.put(['play', []])
while True:
print(cqo.get())
c.join()