def __init__(self,
pb,
obj_states,
robot_fsms,
robot_id,
max_linear_v=MAX_LINEAR_V):
self.pb = pb
self.handlers = {
"PICKUP": self.pickup,
"MOVE": self.move,
"VISUALSERVO": self.visual_servo,
"RETRIEVE": self.retrieve
}
self.handler = self.move
self.robot = robot_id
self.current_state = "NONE"
self.destination = None
self.target_obj = None
self.obj_states = obj_states
self.robot_fsms = robot_fsms
self.basket = set()
self.dest_dist = 0
self.dest_timeout = 0
self.esc_timeout = self.max_esc_timeout
self.servo_timeout = 0
self.control = RobotControl(pb,
self.robot_fsms,
max_linear_velocity=max_linear_v)
self.arm_fsm = ManipulatorStateMachine(self)
def cycle_robot(pb, fsm: RobotStateMachine, controller: RobotControl = None):
if controller is None:
controller = fsm.control
while True:
manipulator_state = controller.get_manipulator_state(fsm.robot)
robot_state = controller.get_robot_state(fsm.robot)
fsm.run_once((manipulator_state, robot_state))
step(pb, int(SIM_FREQUENCY / CONTROL_FREQUENCY))
if fsm.current_state == "NONE":
break
def cycle_robots(pb,
fsms: Dict[int, RobotStateMachine],
controller: RobotControl = None):
while True:
for fsm in fsms.values():
if controller is None:
controller = fsm.control
manipulator_state = controller.get_manipulator_state(fsm.robot)
robot_state = controller.get_robot_state(fsm.robot)
fsm.run_once((manipulator_state, robot_state))
step(pb, int(SIM_FREQUENCY / CONTROL_FREQUENCY))
if all(fsm.current_state == "NONE" for fsm in fsms.values()):
break
def test1(pb, objects, object_states, robots, robot_fsms):
logging.info('Running Single Robot/Single Object Test...')
controller = RobotControl(pb)
obj = objects[0]
logging.debug('Object ID: %s', obj)
robot = robots[0]
logging.debug('Robot ID: %s', robot)
robot_fsm: fsm.RobotStateMachine = robot_fsms[robot]
robot_fsm.set_target(obj)
obj_pos = controller.get_object_state(obj)
logging.debug('Object Position: (%d, %d)', obj_pos[0], obj_pos[1])
logging.debug("Executing Simulation...")
while object_states[obj] != "RETRIEVED":
manipulator_state = controller.get_manipulator_state(robot)
robot_state = controller.get_robot_state(robot)
robot_fsm.run_once((manipulator_state, robot_state))
# logging.debug('Arm FSM is in mode %s', robot_fsm.arm_fsm.current_state) logging.debug('Yaw: {:.2}
# pi'.format(pb.getEulerFromQuaternion(pb.getBasePositionAndOrientation(robot)[1])[2]/np.pi))
for _ in range(int(240 / CONTROL_FREQUENCY)):
pb.stepSimulation()
time.sleep(1. / 240.)
for _ in range(480):
pb.stepSimulation()
time.sleep(1. / 240.)
logging.debug("Returning to Main...")
return
def velocity_test(pb, objects, object_states, robots, robot_fsms):
controller = RobotControl(pb)
pb.resetDebugVisualizerCamera(2, 0, -89, [0, 0, 0])
robot = lib.load_robots(pb, [[0, 0]])[0]
for _ in range(10000):
controller.velocity_control(robot, linear_velocity=3, rotational_velocity=1.0)
for _ in range(int(lib.SIM_FREQUENCY/lib.CONTROL_FREQUENCY)):
pb.stepSimulation()
time.sleep(1. / lib.SIM_FREQUENCY)
logging.debug('{} is moving at {:.2}m/s, {:.2} rad/s'.format(lib.NAMES[robot-1],
np.linalg.norm(controller.get_robot_state(robot)[1][0:2]),
controller.get_robot_state(robot)[1][2]))
logging.debug('Returning to main...')
return
def ucb1(pb, objects, object_states, robots, robot_fsms):
logging.info('Running Single Robot UCB1...')
mu = [3, 2]
sig = [[0.4, 0], [0, 0.4]]
var = 0.1
field = np.zeros((M, M))
visits = np.zeros((M, M))
reward = np.zeros((M, M))
T = N
delta = 1
object_locations = np.random.multivariate_normal(mu, sig, N)
for obj in lib.load_objects(pb, object_locations):
objects.append(obj)
object_states[obj] = "ON_GROUND"
controller = RobotControl(pb)
robot = robots[0]
robot_fsm: fsm.RobotStateMachine = robot_fsms[robot]
logging.debug("Executing Simulation...")
# INITIALIZE AGENT
for i in range(M):
for j in (range(M - 1, -1, -1) if i & 1 else range(M)):
visits[i][j] += 1
robot_fsm.set_destination(lib.get_cell_coordinates(i, j))
lib.cycle_robot(pb, robot_fsm)
lib.step(pb, 100)
reward[i][j] = controller.measure(robot, robot_fsm.obj_states, noise="GAUSSIAN", sigma=var)
# RUN UCB1
for i in range(1, T+1):
exp_mean = np.divide(reward, visits)
Q = exp_mean + np.divide(math.sqrt(2 * math.log(i)), np.sqrt(visits))
target = np.unravel_index(np.argmax(Q), Q.shape)
robot_fsm.set_destination(lib.get_cell_coordinates(target[0], target[1]))
lib.cycle_robot(pb, robot_fsm)
lib.step(pb, 100)
measurement = controller.measure(robot, robot_fsm.obj_states, noise="GAUSSIAN", sigma=var)
reward[target] += measurement
# PICKUP OBJECT
for obj in objects:
if object_states[obj] == "ON_GROUND" and in_cell(target, controller.get_object_state(obj)):
robot_fsm.set_target(obj)
break
lib.cycle_robot(pb, robot_fsm)
visits[target] += 1
logging.info("Time step: {:<2} | Target: {}".format(i, target))
logging.info(visits)
lib.step(pb, 100)
# FINAL OUTPUT
logging.info("Simulation Complete!")
visits -= 1
logging.info(visits)
logging.info(field - visits)
logging.debug("Returning to Main...")
return
def function_test_multi(pb, objects, object_states, robots, robot_fsms):
logging.info('Running Multi-Robot Functions Test...')
controller = RobotControl(pb, robot_fsms)
robot_coords = [(1, 1), (1, -1), (-1, -1), (-1, 1)]
for robot in lib.load_robots(pb, robot_coords):
robots.append(robot)
robot_fsms[robot] = RobotStateMachine(
pb,
object_states,
robot_fsms,
robot,
max_linear_v=utils.control.MAX_LINEAR_V)
logging.debug('Loaded robots...')
coords = [(-3.8, -3.8), (-3.8, -3.6), (-3.8, -3.4), (-3.6, -3.8),
(-3.4, -3.8), (-3.8, 3.8), (-3.8, 3.6), (-3.8, 3.4), (-3.6, 3.8),
(-3.4, 3.8), (3.8, 3.8), (3.8, 3.6), (3.8, 3.4), (3.6, 3.8),
(3.4, 3.8), (3.8, -3.8), (3.8, -3.6), (3.8, -3.4), (3.6, -3.8),
(3.4, -3.8)]
for obj in lib.load_objects(pb, coords):
objects.append(obj)
object_states[obj] = "ON_GROUND"
logging.debug('Loaded objects...')
logging.debug("Executing Simulation...")
cells = [(4, 4), (4, -4), (-4, -4), (-4, 4)]
targets = [(3.5, 3.5), (3.5, -3.5), (-3.5, -3.5), (-3.5, 3.5)]
# ASSIGN TARGETS
for i in range(len(cells)):
robot_fsms[robots[i]].set_destination((targets[i]))
logging.debug("Moving Robot {} to Cell {}...".format(
robots[i], cells[i]))
# MEASURE CELLS
lib.cycle_robots(pb, robot_fsms)
for robot in robots:
robot_fsm = robot_fsms[robot]
m = controller.measure(robot,
robot_fsm.obj_states,
r=RADIUS,
noise="GAUSSIAN")
actual = controller.measure(robot, robot_fsm.obj_states, r=RADIUS)
logging.debug("Robot {} measured {} objects (Actual = {}).".format(
robot, m, actual))
robot_fsm.set_destination(robot_coords[robot - 1])
logging.debug(
"Robot {} returning to starting location...".format(robot))
# RETURN TO START
lib.cycle_robots(pb, robot_fsms)
# RETRIEVE OBJECTS
targets = [7, 12, 17, 22]
for i in range(len(robots)):
robot_fsms[robots[i]].set_target(targets[i])
logging.debug("Robot {} retrieving object {}...".format(
robots[i], targets[i]))
lib.cycle_robots(pb, robot_fsms)
# RETURN TO START
for robot in robots:
robot_fsms[robot].set_destination(robot_coords[robot - 1])
logging.debug(
"Robot {} returning to starting location...".format(robot))
lib.cycle_robots(pb, robot_fsms)
lib.step(pb, 100)
logging.debug("Returning to Main...")
return
def function_test(pb, objects, object_states, robots, robot_fsms):
logging.info('Running Single Robot Functions Test...')
controller = RobotControl(pb)
robot = robots[0]
logging.debug('Robot ID: %s', robot)
robot_fsm: RobotStateMachine = robot_fsms[robot]
logging.debug("Executing Simulation...")
cells = [(4, 4), (4, -4), (-4, -4), (-4, 4)]
targets = [(3.5, 3.5), (3.5, -3.5), (-3.5, -3.5), (-3.5, 3.5)]
coords = [(-3.8, -3.8), (-3.8, -3.6), (-3.8, -3.4), (-3.6, -3.8),
(-3.4, -3.8), (-3.8, 3.8), (-3.8, 3.6), (-3.8, 3.4), (-3.6, 3.8),
(-3.4, 3.8), (3.8, 3.8), (3.8, 3.6), (3.8, 3.4), (3.6, 3.8),
(3.4, 3.8), (3.8, -3.8), (3.8, -3.6), (3.8, -3.4), (3.6, -3.8),
(3.4, -3.8)]
for obj in lib.load_objects(pb, coords):
objects.append(obj)
object_states[obj] = "ON_GROUND"
# MEASURE CELLS
for i in range(len(cells)):
robot_fsm.set_destination(targets[i])
logging.debug("Moving to Cell {}...".format(cells[i]))
lib.cycle_robot(pb, robot_fsm)
lib.step(pb, 100)
m = controller.measure(robot,
robot_fsm.obj_states,
r=RADIUS,
noise="GAUSSIAN")
actual = controller.measure(robot, robot_fsm.obj_states, r=RADIUS)
logging.debug("Robot measured {} objects (Actual = {}).".format(
m, actual))
lib.step(pb, 100)
robot_fsm.set_destination((0, 0))
logging.debug("Returning to Origin...")
lib.cycle_robot(pb, robot_fsm)
# RETRIEVE ONE OBJECT FROM EACH MEASURED CELL
targets = [4, 9, 14, 19]
for target in targets:
robot_fsm.set_target(target)
logging.debug("Retrieving object at {}...".format(
controller.get_object_state(target)))
lib.cycle_robot(pb, robot_fsm)
# Only needed if utils.control.MAX_VOLUME is not 1
robot_fsm.set_destination((0, 0))
logging.debug("Returning to Origin...")
lib.cycle_robot(pb, robot_fsm)
lib.step(pb, 100)
logging.debug("Returning to Main...")
return
def ucb1_multi(pb, objects: List[int], object_states: Dict[int, str], robots: List[int],
robot_fsms: Dict[int, fsm.RobotStateMachine]):
logging.info('Running Multi Agent UCB1...')
# SIMULATION VARIABLES
mu = [3, 2] # Center of object pile
sig = [[0.4, 0], [0, 0.4]] # Spread of objects
var = 0.1 # Measurement error
field = np.zeros((M, M)) # Locations of objects
visits = np.zeros((K, M, M)) # Visit history of the grid for each robot
reward = np.zeros((K, M, M)) # Sum of rewards by location for each robot
exp_mean = np.zeros((K, M, M)) # Expected mean reward by location for each robot
# T = N # Number of time steps
# delta = 1 # Reward decreases by delta each visit
xi = 2 # Constant Xi > 1
gamma = 1 # Max message length
msgTx = {} # Dict {agent: list of messages to send - tuple [agent, time, arm, reward]}
msgRx = {} # Dict {agent: list of messages to received - tuple [agent, time, arm, reward]}
regret = []
controller = RobotControl(pb, robot_fsms)
# INITIALIZE COMMUNICATION NETWORK
logging.debug("Initializing communication network...")
for agent in range(K):
msgTx[agent] = []
msgRx[agent] = []
regret.append([0])
# If graph[i][j] = 1 then there is an edge between agents i and j (graph must be symmetric)
# graph = np.ones((K, K))
degree = 9
graph = np.identity(K)
if degree % 2 == 0:
count = int(degree/2)
for j in range(count):
# Make vertex for every (i + 1) steps away
for i in range(K):
neighbor = int(i + j + 1)
if neighbor > (K - 1):
neighbor = neighbor - K
graph[i][neighbor] = 1
graph[neighbor][i] = 1
else:
count = int((degree - 1)/2 + 1)
for j in range(count):
# Make vertex for every n/2 - i steps away
for i in range(K):
neighbor = int(i + K/2 - j)
if neighbor > (K - 1):
neighbor = neighbor - K
graph[i][neighbor] = 1
graph[neighbor][i] = 1
logging.info("Simulation Graph (DEGREE = {}, GAMMA = {}): \n{}".format(degree, gamma, graph))
# LOAD OBJECTS AND ROBOTS
logging.debug("Loading {} agents and {} objects...".format(K, N))
coords = [(1, 7), (-2, 7), (-2, 4), (-2, 1), (-2, -2), (1, -2), (4, -2), (7, -2), (7, 1), (7, 4)]
for robot in lib.load_robots(pb, coords[0:K]):
robots.append(robot)
robot_fsms[robot] = fsm.RobotStateMachine(pb, object_states, robot_fsms, robot)
object_locations = np.random.multivariate_normal(mu, sig, N)
for i, j in object_locations:
x = math.floor(i) if i < 5 else 4
y = math.floor(j) if j < 5 else 4
field[x][y] += 1
for obj in lib.load_objects(pb, object_locations):
objects.append(obj)
object_states[obj] = "ON_GROUND"
lib.step(pb, 100)
logging.debug("Executing Simulation...")
# bot = robot_fsms[robots[0]]
# fsms = bot.control.robot_fsms
# logging.debug("{} is aware of fsms for {}".format(lib.NAMES[robots[0]-1],
# [lib.NAMES[bot_id - 1] for bot_id in fsms]))
# INITIALIZE AGENT
logging.debug("Initializing agents...")
T = 0
for agent in range(K):
robot = robots[agent]
# Explore cells
for x in range(M):
for y in (range(M-1, -1, -1) if x & 1 else range(M)):
robot_fsms[robot].set_destination(lib.get_cell_coordinates(x, y))
lib.cycle_robot(pb, robot_fsms[robot])
lib.step(pb, 10)
logging.debug("{} measured cell ({}, {})".format(lib.NAMES[agent], x, y))
visits[agent][x][y] += 1
reward[agent][x][y] += controller.measure(robot, object_states, noise="GAUSSIAN", sigma=var)
# Return to start
robot_fsms[robot].set_destination(coords[agent])
lib.cycle_robot(pb, robot_fsms[robot])
# Calculate expected mean
exp_mean[agent] = np.divide(reward[agent], visits[agent])
# RUN UCB1
T += 1
while np.max(field) > 0:
count = 0
for obj in object_states.values():
if obj in ("ON_GROUND", "ASSIGNED"):
count += 1
logging.debug("TIMESTEP T = {} ({} objects remain)".format(T, count))
idxs = []
for agent in range(K):
robot = robots[agent]
# Select arm with highest expected Q
Q = exp_mean[agent] + var * np.divide(math.sqrt(2*(xi + 1)*math.log(T)), np.sqrt(visits[agent]))
target = np.unravel_index(np.argmax(Q), Q.shape)
# Make measurement
robot_fsms[robot].set_destination(lib.get_cell_coordinates(target[0], target[1]))
lib.cycle_robot(pb, robot_fsms[robot])
lib.step(pb, 10)
measurement = controller.measure(robot, object_states, noise="GAUSSIAN", sigma=var)
idxs.append(target)
logging.debug("{} measured {} at cell {}".format(lib.NAMES[agent], measurement, target))
# Calculate regret
max_reward = field[np.unravel_index(np.argmax(field), field.shape)] # based off optimal target
actual_reward = field[target]
regret[agent].append(max_reward - actual_reward)
# Message neighbors
msg = (T, agent, target, measurement)
tx = msgTx[agent]
if len(tx) == gamma:
tx.pop(0)
tx.append(msg)
msgTx[agent] = tx
# Return
robot_fsms[robot].set_destination(coords[agent])
lib.cycle_robot(pb, robot_fsms[robot])
logging.debug("{} returned to their start position {}".format(lib.NAMES[agent], coords[agent]))
for agent in range(K):
robot = robots[agent]
# Go to target
Q = exp_mean[agent] + var * np.divide(math.sqrt(2 * (xi + 1) * math.log(T)), np.sqrt(visits[agent]))
target = np.unravel_index(np.argmax(Q), Q.shape)
robot_fsms[robot].set_destination(lib.get_cell_coordinates(target[0], target[1]))
lib.cycle_robot(pb, robot_fsms[robot])
# Pickup object
flag = False
for obj in objects:
if object_states[obj] == "ON_GROUND" and in_cell(target, controller.get_object_state(obj)):
robot_fsms[robot].set_target(obj)
flag = True
break
lib.cycle_robot(pb, robot_fsms[robot])
logging.debug("{} {} an object at {}".format(lib.NAMES[agent], "picked up" if flag else "did not find",
target))
# Decrement field
if field[target] > 0:
field[target] -= 1
# Return to start location
robot_fsms[robot].set_destination(coords[agent])
lib.cycle_robot(pb, robot_fsms[robot])
logging.debug("{} returned to their start location at {}".format(lib.NAMES[agent], coords[agent]))
# Exchange messages and adjust expected mean
for agent in range(K):
new_visits = np.zeros((M, M))
# Check new messages, skipping repeats
for neighbor in range(K):
if graph[agent][neighbor] == 1:
received = msgRx[agent]
sent = msgTx[neighbor]
for i in range(len(sent)):
msg = sent[i]
if received.count(msg) == 0:
received.append(msg)
cell = msg[2]
visits[agent][cell] += 1
new_visits[cell] += 1
reward[agent][cell] += msg[3]
msgRx[agent] = received
# Calculate expected mean
reward[agent] -= np.multiply(new_visits, visits[agent])
exp_mean[agent] = np.divide(reward[agent], visits[agent])
logging.debug("FIELD AT END OF TIMESTEP:\n{}".format(field))
T += 1
# FINAL OUTPUT
logging.info("Simulation Complete!")
cumulative_regret = np.cumsum(np.array(regret), axis=1)
total_cumulative_regret = np.sum(cumulative_regret, axis=0)
logging.info("Cumulative Regret:\n{}".format(cumulative_regret))
plt.plot(np.arange(T), total_cumulative_regret)
plt.show()
with open("./outputs/Multi-Agent UCB d{}g{}.txt".format(degree, gamma), "a") as f:
f.write("Multi-Agent UCB (Degree = {}, Message Passing {})\n\n".format(degree, "OFF" if gamma == 1 else "ON"))
f.write("Cumulative Regret by Agent:\n")
np.savetxt(f, cumulative_regret, fmt="%.2f", delimiter=", ")
f.write("\n\nSystem Cumulative Regret:")
np.savetxt(f, total_cumulative_regret, fmt="%.2f", delimiter=", ")
f.write("\n\nTimesteps: {}".format(T))
logging.debug("Returning to Main...")
return
def ray_test(pb, objects, object_states, robots, robot_fsms):
controller = RobotControl(pb, robot_fsms, max_linear_velocity=5)
# p.configureDebugVisualizer(p.COV_ENABLE_GUI, 0)
# p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,0)
pb.resetDebugVisualizerCamera(2, 0, -89, [0, 0, 0])
useGui = True
x_, y_ = 0, 0
robots = lib.load_robots(pb, [[x_, y_]])
for _ in range(100):
pb.stepSimulation()
time.sleep(1. / 240.)
# robots = lib.load_robots(pb, [[3, 3], [-5, -2]])
# objects = lib.load_objects(pb, [[0.7, 0.56], [0.5, 0.68], [0.45, 0.24]])
# logging.debug("Objects have {} joints.".format(pb.getNumJoints(objects[0])))
rayFrom = []
rayTo = []
rayIds = []
numRays = 51
rayLen = 5
rayHitColor = [1, 0, 0]
rayMissColor = [0, 1, 0]
replaceLines = True
# for k in range(numRays):
# rayFrom.append([0, 0, 0.2])
# rayTo.append([
# rayLen * math.sin(2. * math.pi * float(k) / numRays),
# rayLen * math.cos(2. * math.pi * float(k) / numRays), 0.064
# ])
# if replaceLines:
# rayIds.append(p.addUserDebugLine(rayFrom[k], rayTo[k], rayMissColor))
# else:
# rayIds.append(-1)
length = 3.0
width = 3.0
sideLength = length
height = 0.015
factor = 1.5
(x, y, yaw), _ = controller.get_robot_state(robots[-1])
rayFrom.append([x * np.cos(yaw), y * np.sin(yaw), height])
rayTo.append([
rayFrom[0][0] + length * np.cos(yaw),
rayFrom[0][1] + length * np.sin(yaw), height
])
if replaceLines:
rayIds.append(
p.addUserDebugLine(rayFrom[0],
rayTo[0],
rayMissColor,
lineWidth=width,
parentObjectUniqueId=robots[-1]))
else:
rayIds.append(-1)
for i in np.linspace(0, 2 * np.pi, 80):
rayFrom.append(rayFrom[-1])
rayTo.append([
rayFrom[-1][0] + length * np.cos(yaw + i),
rayFrom[-1][1] + length * np.sin(yaw + i), height
])
if replaceLines:
rayIds.append(
p.addUserDebugLine(rayFrom[-1],
rayTo[-1],
rayMissColor,
lineWidth=width))
else:
rayIds.append(-1)
# for i in np.linspace(np.pi/4, 2*np.pi - np.pi/4, 7):
# rayFrom.append(rayFrom[-1])
# rayTo.append([rayFrom[-1][0] + length * np.cos(yaw+i), rayFrom[-1][1] + length * np.sin(yaw+i), height])
#
# if replaceLines:
# rayIds.append(p.addUserDebugLine(rayFrom[0], rayTo[0], rayMissColor))
# else:
# rayIds.append(-1)
# for i in np.linspace(0.02, 0.11, 6):
# dx = i/length * (rayFrom[0][1]-rayTo[0][1])
# dy = i/length * (rayFrom[0][0]-rayTo[0][0])
# rayFrom.append([rayFrom[0][0] + dx, rayFrom[0][1] + dy, height])
# # rayTo.append([rayTo[0][0] + dx, rayTo[0][1] + dy, height])
# rayTo.append([rayFrom[-1][0] + length * np.cos(yaw-(factor*i)), rayFrom[-1][1] + length * np.sin(yaw-(factor*i)), height])
#
# if replaceLines:
# rayIds.append(p.addUserDebugLine(rayFrom[0], rayTo[0], rayMissColor, lineWidth=width))
# else:
# rayIds.append(-1)
#
# for i in np.linspace(-0.02, -0.11, 6):
# dx = i / length * (rayFrom[0][1] - rayTo[0][1])
# dy = i / length * (rayFrom[0][0] - rayTo[0][0])
# rayFrom.append([rayFrom[0][0] + dx, rayFrom[0][1] + dy, height])
# # rayTo.append([rayTo[0][0] + dx, rayTo[0][1] + dy, height])
# rayTo.append([rayFrom[-1][0] + length * np.cos(yaw-(factor*i)), rayFrom[-1][1] + length * np.sin(yaw-(factor*i)), height])
#
# if replaceLines:
# rayIds.append(p.addUserDebugLine(rayFrom[0], rayTo[0], rayMissColor, lineWidth=width))
# else:
# rayIds.append(-1)
# for i in np.linspace(0.47*np.pi, np.pi/20, 15, endpoint=False):
# rayFrom.append(rayFrom[-1])
# rayTo.append([rayFrom[-1][0] + sideLength * np.cos(yaw+i), rayFrom[-1][1] + sideLength * np.sin(yaw+i), height])
#
# if replaceLines:
# rayIds.append(p.addUserDebugLine(rayFrom[0], rayTo[0], rayMissColor, lineWidth=width))
# else:
# rayIds.append(-1)
#
# for i in np.linspace(-0.47*np.pi, -np.pi/20, 15, endpoint=False):
# rayFrom.append(rayFrom[6])
# rayTo.append([rayFrom[6][0] + sideLength * np.cos(yaw+i), rayFrom[6][1] + sideLength * np.sin(yaw+i), height])
#
# if replaceLines:
# rayIds.append(p.addUserDebugLine(rayFrom[0], rayTo[0], rayMissColor, lineWidth=width))
# else:
# rayIds.append(-1)
numSteps = 327680
for i in range(numSteps):
p.stepSimulation()
for j in range(8):
results = p.rayTestBatch(rayFrom, rayTo, j + 1)
# for i in range (10):
# p.removeAllUserDebugItems()
if (useGui):
if (not replaceLines):
p.removeAllUserDebugItems()
for i in range(numRays):
hitObjectUid = results[i][0]
if (hitObjectUid < 0):
hitPosition = [0, 0, 0]
p.addUserDebugLine(rayFrom[i],
rayTo[i],
rayMissColor,
replaceItemUniqueId=rayIds[i],
lineWidth=width)
else:
hitPosition = results[i][3]
p.addUserDebugLine(rayFrom[i],
rayTo[i],
rayMissColor,
replaceItemUniqueId=rayIds[i],
lineWidth=width)
def measure_corners(pb, objects, object_states, robots, robot_fsms):
logging.info('Running Single Robot Measurement Test...')
controller = RobotControl(pb)
cells = [(4, 4), (4, -4), (-4, -4), (-4, 4)]
targets = [(3.5, 3.5), (3.5, -3.5), (-3.5, -3.5), (-3.5, 3.5)]
coords = [(-3.8, -3.8), (-3.8, -3.6), (-3.8, -3.4), (-3.6, -3.8),
(-3.4, -3.8), (-3.8, 3.8), (-3.8, 3.6), (-3.8, 3.4), (-3.6, 3.8),
(-3.4, 3.8), (3.8, 3.8), (3.8, 3.6), (3.8, 3.4), (3.6, 3.8),
(3.4, 3.8), (3.8, -3.8), (3.8, -3.6), (3.8, -3.4), (3.6, -3.8),
(3.4, -3.8)]
for obj in lib.load_objects(pb, coords):
objects.append(obj)
object_states[obj] = "ON_GROUND"
robot = robots[0]
logging.debug('Robot ID: %s', robot)
robot_fsm: fsm.RobotStateMachine = robot_fsms[robot]
logging.debug("Executing Simulation...")
for i in range(len(cells)):
robot_fsm.set_destination(targets[i])
logging.debug("Moving to Cell {}...".format(cells[i]))
while True:
manipulator_state = controller.get_manipulator_state(robot)
robot_state = controller.get_robot_state(robot)
robot_fsm.run_once((manipulator_state, robot_state))
for _ in range(int(240 / CONTROL_FREQUENCY)):
pb.stepSimulation()
time.sleep(1. / 240.)
if robot_fsm.current_state == "NONE":
for _ in range(100):
pb.stepSimulation()
time.sleep(1. / 240.)
m = controller.measure(robot,
robot_fsm.obj_states,
r=RADIUS,
noise="GAUSSIAN")
actual = controller.measure(robot,
robot_fsm.obj_states,
r=RADIUS)
logging.debug(
"Robot measured {} objects (Actual = {}).".format(
m, actual))
for _ in range(100):
pb.stepSimulation()
time.sleep(1. / 240.)
robot_fsm.set_destination((0, 0))
logging.debug("Returning to Origin...".format(cells[i]))
while True:
manipulator_state = controller.get_manipulator_state(robot)
robot_state = controller.get_robot_state(robot)
robot_fsm.run_once((manipulator_state, robot_state))
for _ in range(int(240 / CONTROL_FREQUENCY)):
pb.stepSimulation()
time.sleep(1. / 240.)
if robot_fsm.current_state == "NONE":
break
break
for _ in range(100):
pb.stepSimulation()
time.sleep(1. / 240.)
logging.debug("Returning to Main...")
return
class RobotStateMachine:
max_dest_timeout = 1000
max_esc_timeout = 80
max_servo_timeout = 500
collection_sites = [((0, 0), 5)]
threshold_distance = 0.5
def __init__(self,
pb,
obj_states,
robot_fsms,
robot_id,
max_linear_v=MAX_LINEAR_V):
self.pb = pb
self.handlers = {
"PICKUP": self.pickup,
"MOVE": self.move,
"VISUALSERVO": self.visual_servo,
"RETRIEVE": self.retrieve
}
self.handler = self.move
self.robot = robot_id
self.current_state = "NONE"
self.destination = None
self.target_obj = None
self.obj_states = obj_states
self.robot_fsms = robot_fsms
self.basket = set()
self.dest_dist = 0
self.dest_timeout = 0
self.esc_timeout = self.max_esc_timeout
self.servo_timeout = 0
self.control = RobotControl(pb,
self.robot_fsms,
max_linear_velocity=max_linear_v)
self.arm_fsm = ManipulatorStateMachine(self)
def pickup(self, state):
manipulator_state = state[0]
result = self.arm_fsm.run_once(manipulator_state)
if result is not "INPROCESS":
try:
self.obj_states[self.target_obj] = "RECOVERED"
except KeyError:
self.current_state = "NONE"
return "NONE"
self.basket.add(self.target_obj)
self.target_obj = None
if result is "DONE":
self.current_state = "MOVE"
return "MOVE"
elif result is "RETRIEVE":
self.current_state = "RETRIEVE"
return "RETRIEVE"
self.current_state = "PICKUP"
return "PICKUP"
def move(self, state):
pose, vel = state[1]
if self.destination is not None:
dist = np.linalg.norm(
(self.destination[0] - pose[0], self.destination[1] - pose[1]))
# increase the timeout counter when there is no improvement in reaching the destination
if self.dest_dist <= dist:
self.dest_timeout += 1
if self.dest_timeout >= self.max_dest_timeout:
self.control.velocity_control(self.robot, 0, 0)
self.set_destination(None)
self.current_state = "NONE"
return "NONE"
self.dest_dist = dist
if self.dest_dist > DISTANCE_THRESHOLD:
self.control.smart_pose_control(self.robot, self.destination)
self.current_state = "MOVE"
return "MOVE"
else:
self.control.velocity_control(self.robot, 0, 0)
self.set_destination(None)
if self.target_obj is not None:
self.servo_timeout = 0
self.current_state = "VISUALSERVO"
return "VISUALSERVO"
self.current_state = "NONE"
return "NONE"
def visual_servo(self, state):
pose, vel = state[1]
try:
if self.obj_states[self.target_obj] in ("RECOVERED", "RETRIEVED"):
self.target_obj = None
self.current_state = "MOVE"
return "MOVE"
obj_pos = self.control.get_object_state(self.target_obj)
except KeyError:
self.target_obj = None
self.current_state = "MOVE"
return "MOVE"
dist, orn, _, _ = self.control.visual_servoing(self.robot, obj_pos,
pose)
if self.dest_dist <= dist:
self.servo_timeout += 1
if self.servo_timeout >= self.max_servo_timeout:
self.control.velocity_control(self.robot, 0, 0)
self.set_destination(None)
self.target_obj = None
self.servo_timeout = 0
self.current_state = "NONE"
return "NONE"
self.dest_dist = dist
if dist <= 0.05 and orn < 10 * np.pi / 180:
self.control.velocity_control(self.robot, 0, 0)
self.arm_fsm.reinitialize()
self.arm_fsm.object = self.target_obj
self.current_state = "PICKUP"
return "PICKUP"
else:
self.current_state = "VISUALSERVO"
return "VISUALSERVO"
def retrieve(self, state):
pose, vel = state[1]
return_site = [0, 0]
return_dist = np.linalg.norm(
(return_site[0] - pose[0], return_site[1] - pose[1]))
if return_dist > DISTANCE_THRESHOLD:
self.control.smart_pose_control(self.robot, return_site)
self.current_state = "RETRIEVE"
return "RETRIEVE"
else:
self.control.velocity_control(self.robot, 0, 0)
self.empty_basket()
self.set_destination(None)
# TODO: Verify that this should just read 'self.current_state = "NONE"' followed by 'return "NONE"'
self.current_state = "MOVE"
return "MOVE"
def set_destination(self, destination):
self.destination = destination
self.dest_timeout = 0
def set_target(self, obj):
self.target_obj = obj
self.obj_states[obj] = "ASSIGNED"
self.servo_timeout = 0
self.set_destination(self.control.get_object_state(obj))
def empty_basket(self):
while self.basket:
obj = self.basket.pop()
self.obj_states[obj] = "RETRIEVED"
self.pb.removeBody(obj)
self.arm_fsm.current_volume = 0
def run_once(self, state):
new_fsm_state = self.handler(state)
if new_fsm_state is not "NONE":
self.handler = self.handlers[new_fsm_state]
def breakdown(self):
self.control.velocity_control(self.robot, 0, 0)
return
def visit_cells(pb, objects, object_states, robots, robot_fsms):
logging.info('Running Single Robot Movement Test...')
controller = RobotControl(pb)
robot = robots[0]
logging.debug('Robot ID: %s', robot)
robot_fsm: fsm.RobotStateMachine = robot_fsms[robot]
logging.debug("Executing Simulation...")
# for i in range(-5, 6):
# r = range(-5, 6) if i & 1 else range(5, -6, -1)
# for j in r:
# robot_fsm.set_destination((i, j))
# logging.debug("Moving to Location ({}, {})...".format(i, j))
#
# while True:
# manipulator_state = controller.get_manipulator_state(robot)
# robot_state = controller.get_robot_state(robot)
#
# robot_fsm.run_once((manipulator_state, robot_state))
#
# for _ in range(int(240 / CONTROL_FREQUENCY)):
# pb.stepSimulation()
# time.sleep(1. / 240.)
#
# if robot_fsm.current_state == "NONE":
# break
targets = [(0, 2), (3, 2), (-3, -3), (0, 0), (2, -4), (1, 3), (-2, 2),
(0, 0)]
for target in targets:
robot_fsm.set_destination(target)
logging.debug("Moving to Location ({}, {})...".format(
target[0], target[1]))
while True:
manipulator_state = controller.get_manipulator_state(robot)
robot_state = controller.get_robot_state(robot)
robot_fsm.run_once((manipulator_state, robot_state))
for _ in range(int(240 / CONTROL_FREQUENCY)):
pb.stepSimulation()
time.sleep(1. / 240.)
if robot_fsm.current_state == "NONE":
for _ in range(240):
pb.stepSimulation()
time.sleep(1. / 240.)
break
# d = (-2, 2)
# robot_fsm.set_destination(d)
# logging.debug("Moving to Location {}...".format(d))
while True:
manipulator_state = controller.get_manipulator_state(robot)
robot_state = controller.get_robot_state(robot)
robot_fsm.run_once((manipulator_state, robot_state))
for _ in range(int(240 / CONTROL_FREQUENCY)):
pb.stepSimulation()
time.sleep(1. / 240.)
# logging.debug("FSM State: {}".format(robot_fsm.current_state))
# if robot_fsm.destination is None:
# break
if robot_fsm.current_state == "NONE":
break
for _ in range(480):
pb.stepSimulation()
time.sleep(1. / 240.)
logging.debug("Returning to Main...")
return