forked from Telos4/RoboRally
general refactoring
This commit is contained in:
parent
7ded3bee79
commit
a0f701fe83
|
|
@ -3,6 +3,7 @@ import time
|
|||
|
||||
from casadi_opt import OpenLoopSolver
|
||||
|
||||
|
||||
class MPCController:
|
||||
def __init__(self, estimator):
|
||||
self.t = time.time()
|
||||
|
|
@ -30,19 +31,19 @@ class MPCController:
|
|||
elif event[0] == 'key':
|
||||
key = event[1]
|
||||
|
||||
if key == 84: # arrow up
|
||||
if key == 84: # arrow up
|
||||
self.controlling = True
|
||||
self.t = time.time()
|
||||
elif key == 82: # arrow down
|
||||
elif key == 82: # arrow down
|
||||
self.controlling = False
|
||||
robot.send_cmd()
|
||||
elif key == 48: # 0
|
||||
target_pos = np.array([0.0,0.0,0.0])
|
||||
elif key == 43: # +
|
||||
elif key == 48: # 0
|
||||
target_pos = np.array([0.0, 0.0, 0.0])
|
||||
elif key == 43: # +
|
||||
self.control_scaling += 0.1
|
||||
self.control_scaling = min(self.control_scaling, 1.0)
|
||||
print("control scaling = ", self.control_scaling)
|
||||
elif key == 45: # -
|
||||
elif key == 45: # -
|
||||
self.control_scaling -= 0.1
|
||||
self.control_scaling = max(self.control_scaling, 0.1)
|
||||
print("control scaling = ", self.control_scaling)
|
||||
|
|
@ -51,7 +52,7 @@ class MPCController:
|
|||
self.controlling = False
|
||||
robot.send_cmd()
|
||||
return
|
||||
elif key == 27: # escape
|
||||
elif key == 27: # escape
|
||||
print("quit!")
|
||||
self.controlling = False
|
||||
robot.send_cmd()
|
||||
|
|
@ -61,27 +62,26 @@ class MPCController:
|
|||
|
||||
if x_pred is not None:
|
||||
error_pos = np.linalg.norm(x_pred[0:2] - target_pos[0:2])
|
||||
angles_unwrapped = np.unwrap([x_pred[2], target_pos[2]]) # unwrap angle to avoid jump in data
|
||||
angles_unwrapped = np.unwrap([x_pred[2], target_pos[2]]) # unwrap angle to avoid jump in data
|
||||
error_ang = np.abs(angles_unwrapped[0] - angles_unwrapped[1])
|
||||
#print("error pos = ", error_pos)
|
||||
#print("error_pos = {}, error_ang = {}".format(error_pos, error_ang))
|
||||
# print("error pos = ", error_pos)
|
||||
# print("error_pos = {}, error_ang = {}".format(error_pos, error_ang))
|
||||
|
||||
#if error_pos > 0.075 or error_ang > 0.35:
|
||||
# if error_pos > 0.075 or error_ang > 0.35:
|
||||
if error_pos > 0.05 or error_ang > 0.1:
|
||||
# solve mpc open loop problem
|
||||
res = self.ols.solve(x_pred, target_pos)
|
||||
|
||||
#us1 = res[0]
|
||||
#us2 = res[1]
|
||||
# us1 = res[0]
|
||||
# us2 = res[1]
|
||||
us1 = res[0] * self.control_scaling
|
||||
us2 = res[1] * self.control_scaling
|
||||
#print("u = {}", (us1, us2))
|
||||
# print("u = {}", (us1, us2))
|
||||
|
||||
tmpc_end = time.time()
|
||||
#print("---------------- mpc solution took {} seconds".format(tmpc_end - tmpc_start))
|
||||
# print("---------------- mpc solution took {} seconds".format(tmpc_end - tmpc_start))
|
||||
dt_mpc = time.time() - self.t
|
||||
if dt_mpc < self.dt: # wait until next control can be applied
|
||||
#print("sleeping for {} seconds...".format(self.dt - dt_mpc))
|
||||
if dt_mpc < self.dt: # wait until next control can be applied
|
||||
# print("sleeping for {} seconds...".format(self.dt - dt_mpc))
|
||||
time.sleep(self.dt - dt_mpc)
|
||||
else:
|
||||
us1 = [0] * self.mstep
|
||||
|
|
@ -89,13 +89,13 @@ class MPCController:
|
|||
near_target += 1
|
||||
|
||||
# send controls to the robot
|
||||
for i in range(0, self.mstep): # option to use multistep mpc if len(range) > 1
|
||||
for i in range(0, self.mstep): # option to use multistep mpc if len(range) > 1
|
||||
u1 = us1[i]
|
||||
u2 = us2[i]
|
||||
robot.send_cmd(u1, u2)
|
||||
if i < self.mstep:
|
||||
time.sleep(self.dt)
|
||||
self.t = time.time() # save time the most recent control was applied
|
||||
self.t = time.time() # save time the most recent control was applied
|
||||
else:
|
||||
print("robot not detected yet!")
|
||||
|
||||
|
|
@ -116,31 +116,31 @@ class MPCController:
|
|||
elif event[0] == 'key':
|
||||
key = event[1]
|
||||
|
||||
if key == 32: # arrow up
|
||||
if key == 32: # arrow up
|
||||
self.controlling = not self.controlling
|
||||
if not self.controlling:
|
||||
print("disable control")
|
||||
robot.send_cmd() # stop robot
|
||||
robot.send_cmd() # stop robot
|
||||
else:
|
||||
print("enable control")
|
||||
self.t = time.time()
|
||||
elif key == 48: # 0
|
||||
target_pos = np.array([0.0, 0.0, 0.0]) # TODO: use center of board for target pos
|
||||
elif key == 43: # +
|
||||
elif key == 48: # 0
|
||||
target_pos = np.array([0.0, 0.0, 0.0]) # TODO: use center of board for target pos
|
||||
elif key == 43: # +
|
||||
self.control_scaling += 0.1
|
||||
self.control_scaling = min(self.control_scaling, 1.0)
|
||||
print("control scaling = ", self.control_scaling)
|
||||
elif key == 45: # -
|
||||
elif key == 45: # -
|
||||
self.control_scaling -= 0.1
|
||||
self.control_scaling = max(self.control_scaling, 0.1)
|
||||
print("control scaling = ", self.control_scaling)
|
||||
elif key == 9: # TAB
|
||||
elif key == 9: # TAB
|
||||
# switch controlled robot
|
||||
robot.send_cmd() # stop current robot
|
||||
controlled_robot_number = (controlled_robot_number + 1) % len(robots)
|
||||
robot = robots[controlled_robot_number]
|
||||
print(f"controlled robot: {robot.id}")
|
||||
elif key == 113 or key == 27: # q or ESCAPE
|
||||
elif key == 113 or key == 27: # q or ESCAPE
|
||||
print("quit!")
|
||||
self.controlling = False
|
||||
robot.send_cmd()
|
||||
|
|
@ -150,7 +150,7 @@ class MPCController:
|
|||
# measure state
|
||||
x_pred = self.get_measurement(robot.id)
|
||||
|
||||
#print(np.linalg.norm(x_pred-target_pos))
|
||||
# print(np.linalg.norm(x_pred-target_pos))
|
||||
|
||||
# solve mpc open loop problem
|
||||
res = self.ols.solve(x_pred, target_pos)
|
||||
|
|
@ -172,4 +172,4 @@ class MPCController:
|
|||
self.t = time.time() # save time the most recent control was applied
|
||||
|
||||
def get_measurement(self, robot_id):
|
||||
return self.estimator.get_robot_state_estimate(robot_id)
|
||||
return self.estimator.get_robot_state_estimate(robot_id)
|
||||
|
|
|
|||
Loading…
Reference in New Issue
Block a user