def set_conveyor_speed(self, conveyor_property_list, conveyor_speed_list):
# We iterate through the conveyor name list
for i in range(len(conveyor_property_list)):
# We set the conveyor speed (m/s)
sim.simxSetFloatSignal(self.client_id, conveyor_property_list[i],
conveyor_speed_list[i],
sim.simx_opmode_oneshot)
def reset(self):
sim.simxSetFloatSignal(self.clientID, 'renderEnv', 0,
sim.simx_opmode_oneshot)
sim.simxGetPingTime(self.clientID)
self.resetSim()
self.prepareSim()
obs = self._get_obs()
return obs
def publish(self):
"""
send msg to vrep
msg : the frequency of leg
"""
vrep.simxSetFloatSignal(self.clientID, self.LSignalName,
self.LCycleFreq, vrep.simx_opmode_oneshot)
vrep.simxSetFloatSignal(self.clientID, self.RSignalName,
self.RCycleFreq, vrep.simx_opmode_oneshot)
def set_conveyor_distance_and_speed(self, conveyor_property_list,
conveyor_distance_list,
conveyor_speed_list):
# We set the conveyor belts to move at the speeds specified
self.set_conveyor_speed(conveyor_property_list, conveyor_speed_list)
# We iterate through the conveyor name list
for i in range(len(conveyor_property_list)):
# We set the conveyor speed (m/s)
sim.simxSetFloatSignal(self.client_id, conveyor_property_list[i],
conveyor_speed_list[i],
sim.simx_opmode_oneshot)
# We know that time to run the conveyor(s) = distance(m)/speed(m/s)
time_to_finish_run = self.get_simulation_time(
) + conveyor_distance_list[i] / conveyor_speed_list[i]
# We update the conveyor velocity dictionary
self.conveyor_belt_velocity_timers[
conveyor_property_list[i]] = time_to_finish_run
# We check if a conveyor thread is not running
if self.current_thread is None:
# We run a parallel thread that monitors conveyor belt movements
self.current_thread = self.conveyor_timer_pool.submit(
self.conveyor_thread_handler)
def _set_action(self, action):
actionX = action[0]
actionY = action[1]
actionZ = action[2]
# print('[DATA] X:{:.4f}, Y:{:.4f}, Z:{:.4f}'.format(actionX,actionY,actionZ))
sim.simxSetFloatSignal(self.clientID, 'actionX', actionX,
sim.simx_opmode_oneshot)
sim.simxSetFloatSignal(self.clientID, 'actionY', actionY,
sim.simx_opmode_oneshot)
sim.simxSetFloatSignal(self.clientID, 'actionZ', actionZ,
sim.simx_opmode_oneshot)
sim.simxGetPingTime(self.clientID)
def doTracking(self):
self.resetSim()
self.prepareSim()
pathIsDone = sim.simxGetFloatSignal(self.clientID, 'movePathDone',
sim.simx_opmode_streaming)[1]
posX = []
posY = []
posZ = []
tactile1 = []
reward = []
while pathIsDone == 0:
X, Y, Z = self.getKinectXYZ(False)
# forces = self.getForce(False)
# diffForce = (forces[0][0] + ((forces[1][0] + forces[2][0]) / 2))/2
# print('[DATA] diff force: {:.4f}'.format(diffForce))
actionX = X # + 0.005*diffForce
actionY = Y
actionZ = Z
# print('[DATA] X:{:.4f}, Y:{:.4f}, Z:{:.4f}'.format(actionX,actionY,actionZ))
sim.simxSetFloatSignal(self.clientID, 'actionX', actionX,
sim.simx_opmode_oneshot)
sim.simxSetFloatSignal(self.clientID, 'actionY', actionY,
sim.simx_opmode_oneshot)
sim.simxSetFloatSignal(self.clientID, 'actionZ', actionZ,
sim.simx_opmode_oneshot)
sim.simxGetPingTime(self.clientID)
sim.simxSynchronousTrigger(self.clientID)
forces = self.getForceMagnitude(False)
posX.append(X)
posY.append(Y)
posZ.append(Z)
tactile1.append(forces[0])
reward.append(self._getReward())
sim.simxGetPingTime(self.clientID)
pathIsDone = sim.simxGetFloatSignal(self.clientID, 'movePathDone',
sim.simx_opmode_buffer)[1]
# time.sleep(0.5)
# print('[INFO] tracking is done.')
# print('[DATA] accumulated reward: {}'.format(np.sum(np.array(reward))))
sim.simxStopSimulation(self.clientID, sim.simx_opmode_blocking)
def prepareSim(self):
self.initializeFunctions()
isTracking = sim.simxGetFloatSignal(self.clientID, 'isTracking',
sim.simx_opmode_streaming)[1]
while isTracking != 1:
sim.simxSynchronousTrigger(self.clientID)
isTracking = sim.simxGetFloatSignal(self.clientID, 'isTracking',
sim.simx_opmode_buffer)[1]
sim.simxGetPingTime(self.clientID)
X, Y, Z = self.getKinectXYZ(True)
actionX = X
actionY = Y
actionZ = Z
# print(actionX,actionY,actionZ)
# print('[DATA] X:{:.4f}, Y:{:.4f}, Z:{:.4f}'.format(actionX,actionY,actionZ))
sim.simxSetFloatSignal(self.clientID, 'actionX', actionX,
sim.simx_opmode_oneshot)
sim.simxSetFloatSignal(self.clientID, 'actionY', actionY,
sim.simx_opmode_oneshot)
sim.simxSetFloatSignal(self.clientID, 'actionZ', actionZ,
sim.simx_opmode_oneshot)
sim.simxGetPingTime(self.clientID)
def set_signal(self, signal, value):
return sim.simxSetFloatSignal(self.client_id, signal, value,
sim.simx_opmode_oneshot)
def size_sort():
result, stack = sim.simxGetFloatSignal(clientID, "stack",
sim.simx_opmode_streaming)
result, height = sim.simxGetFloatSignal(clientID, "height",
sim.simx_opmode_streaming)
result, size = sim.simxGetFloatSignal(clientID, "size",
sim.simx_opmode_streaming)
size_list = []
#wait for stack to be detected
while (stack == 0):
result, stack = sim.simxGetFloatSignal(clientID, "stack",
sim.simx_opmode_buffer)
#set the robot as busy
sim.simxSetFloatSignal(clientID, 'ss_robotBusy', 1,
sim.simx_opmode_oneshot)
while (stack > 0):
#get top book height
result, height = sim.simxGetFloatSignal(clientID, "height",
sim.simx_opmode_buffer)
#set book position depending on stack length
book_position = invk(ss_robot, -1.2952, 0.6111, height)
#move book to ss table
moveJoints(ss_robot, book_position)
sim.simxSetFloatSignal(clientID, 'ss_succ', 1,
sim.simx_opmode_oneshot) # pick up the book
time.sleep(1)
moveJoints(ss_robot, ss_table)
sim.simxSetFloatSignal(clientID, 'ss_succ', 0,
sim.simx_opmode_oneshot) # drop the book off
time.sleep(2)
#get book size
result, size = sim.simxGetFloatSignal(clientID, "size",
sim.simx_opmode_buffer)
size_list.append([len(size_list), size, height])
#separate stack
sim.simxSetFloatSignal(clientID, 'ss_succ', 1,
sim.simx_opmode_oneshot) # pick up the book
book_position = invk(ss_robot, -0.425,
0.6025 - (len(size_list) - 1) * 0.16, 0.17)
moveJoints(ss_robot, book_position) #move to appropriate position
sim.simxSetFloatSignal(clientID, 'ss_succ', 0,
sim.simx_opmode_oneshot) # drop the book off
time.sleep(1.1)
result, stack = sim.simxGetFloatSignal(clientID, "stack",
sim.simx_opmode_buffer)
#loop to get book thickness
for i in range(len(size_list) - 1):
size_list[i][2] = size_list[i][2] - size_list[i + 1][2]
size_list[len(size_list) - 1][2] = size_list[len(size_list) - 1][2] - 0.15
#sort the size list
size_list = sorted(size_list, key=lambda size: size[1], reverse=True)
#largest books
largest_book = size_list[0]
#remove largest book from size list
size_list.pop(0)
#intial stack height
height = largest_book[2] + 0.15
#stack the books depending on size
for i in range(len(size_list)):
intial_position = invk(ss_robot, -0.425,
0.6025 - (size_list[i][0]) * 0.15, 0.14)
moveJoints(ss_robot, intial_position)
sim.simxSetFloatSignal(clientID, 'ss_succ', 1,
sim.simx_opmode_oneshot) # pick up the book
time.sleep(1)
#define a mid position to stop the robot from knocking over other robots
mid_position = getJointAngles(ss_robot)
mid_position[1] = 0
moveJoints(ss_robot, mid_position)
#finding height so the robot knows where to put the robot
height = height + size_list[i][2]
#get the largest book's position
target_position = invk(ss_robot, -0.425,
0.6025 - (largest_book[0]) * 0.15, height)
#move to the smaller book on top of the bigger book
moveJoints(ss_robot, target_position)
time.sleep(1.5)
sim.simxSetFloatSignal(clientID, 'ss_succ', 0,
sim.simx_opmode_oneshot) # drop the book off
time.sleep(1)
time.sleep(1)
#set the robot as free
sim.simxSetFloatSignal(clientID, 'ss_robotBusy', 0,
sim.simx_opmode_oneshot)
time.sleep(3) #wait for book stack to reach the end
return size_list
def color_sort():
color_list = []
result, cs_bookline = sim.simxGetFloatSignal(clientID, "cs_bookline",
sim.simx_opmode_streaming)
result, book = sim.simxGetFloatSignal(clientID, "book",
sim.simx_opmode_streaming)
#move to intial position
moveJoints(cs_robot, position0)
#set the robot as busy
sim.simxSetFloatSignal(clientID, 'cs_robotBusy', 1,
sim.simx_opmode_oneshot)
while (cs_bookline == 0):
#wait for book to be detected
while (book == 0):
result, book = sim.simxGetFloatSignal(clientID, "book",
sim.simx_opmode_buffer)
#move book to cs table
moveJoints(cs_robot, cs_left_conveyor)
sim.simxSetFloatSignal(clientID, 'cs_succ', 1,
sim.simx_opmode_oneshot) # pick up the book
time.sleep(0.5)
moveJoints(cs_robot, cs_table)
sim.simxSetFloatSignal(clientID, 'cs_succ', 0,
sim.simx_opmode_oneshot) # drop the book off
#check book color
result, res, image = sim.simxGetVisionSensorImage(
clientID, cs_vis, 0, sim.simx_opmode_blocking)
book_color = [image[0], image[1], image[2]] #RGB color value
book_order = len(color_list) #assumes color is new initially
rng = 10 #range to determine wheter to colors are the same or not
#loop that check RGB values of every color list with book_color to determine is the same as a color in color_list
for color in enumerate(color_list):
if (abs(color[1][0] - book_color[0]) <
rng) and (abs(color[1][1] - book_color[1]) < rng) and (
abs(color[1][2] - book_color[2]) < rng):
book_order = color[0]
#if color is new append it to the color_list
if (book_order == len(color_list)):
color_list.append(book_color)
#set the book position depending on its order
book_position = invk(cs_robot, -1.2952, -0.68 + 0.154 * (book_order),
0.25)
#move and place the book in the proper position
sim.simxSetFloatSignal(clientID, 'cs_succ', 1,
sim.simx_opmode_oneshot) # pick up the book\
time.sleep(0.5)
moveJoints(cs_robot, book_position)
time.sleep(1.1)
sim.simxSetFloatSignal(clientID, 'cs_succ', 0,
sim.simx_opmode_oneshot) # drop the book off
result, cs_bookline = sim.simxGetFloatSignal(clientID, "cs_bookline",
sim.simx_opmode_buffer)
time.sleep(1)
#set the robot as free
sim.simxSetFloatSignal(clientID, 'cs_robotBusy', 0,
sim.simx_opmode_oneshot)
moveJoints(cs_robot, position0)
return color_list
def render(self, mode='human'):
sim.simxSetFloatSignal(self.clientID, 'renderEnv', 1,
sim.simx_opmode_oneshot)
sim.simxGetPingTime(self.clientID)