def streamVisionSensor(visionSensorName, clientID, pause=0.0001):
# enable the synchronous mode on the client:
vrep.simxSynchronous(clientID, 1)
# start the simulation:
vrep.simxStartSimulation(clientID, vrep.simx_opmode_blocking)
# enable streaming of the iteration counter:
res, iteration1 = vrep.simxGetIntegerSignal(clientID, "iteration",
vrep.simx_opmode_streaming)
print("iteration1: ", iteration1)
#Get the handle of the vision sensor
res1, visionSensorHandle = vrep.simxGetObjectHandle(
clientID, visionSensorName, vrep.simx_opmode_oneshot_wait)
#Get the image
res2, resolution, image = vrep.simxGetVisionSensorImage(
clientID, visionSensorHandle, 0, vrep.simx_opmode_streaming)
#Initialiazation of the figure
time.sleep(0.5)
res, resolution, image = vrep.simxGetVisionSensorImage(
clientID, visionSensorHandle, 0, vrep.simx_opmode_buffer)
im = I.new("RGB", (resolution[0], resolution[1]), "white")
# Init figure
plt.ion()
fig = plt.figure(1)
plotimg = plt.imshow(im, origin='lower')
#Let some time to Vrep in order to let him send the first image, otherwise the loop will start with an empty image and will crash
time.sleep(1)
while (vrep.simxGetConnectionId(clientID) != -1):
if iteration1 < 30:
vrep.simxSynchronousTrigger(clientID)
res, iteration1 = vrep.simxGetIntegerSignal(
clientID, "iteration", vrep.simx_opmode_buffer)
if res != vrep.simx_return_ok: iteration1 = -1
iteration2 = iteration1
while iteration2 == iteration1: # wait until the iteration counter has changed
res, iteration2 = vrep.simxGetIntegerSignal(
clientID, "iteration", vrep.simx_opmode_buffer)
if res != vrep.simx_return_ok:
iteration2 = -1
print(iteration2)
#Get the image of the vision sensor
res, resolution, image = vrep.simxGetVisionSensorImage(
clientID, visionSensorHandle, 0, vrep.simx_opmode_buffer)
#Transform the image so it can be displayed
img = np.array(image, dtype=np.uint8).reshape(
[resolution[1], resolution[0], 3])
#Update the image
fig.canvas.set_window_title(visionSensorName + '_' +
str(iteration2))
plotimg.set_data(img)
plt.draw()
plt.pause(pause)
# vrep.simxSynchronousTrigger(clientID)
print('End of Simulation')
def operate_gripper(x):
# Set x to 0 to open, 1 to close the gripper
if x == 0:
returnCode_1, signalValue = vrep.simxGetIntegerSignal(clientID, "_gripperClose", vrep.simx_opmode_streaming)
returnCode_2 = vrep.simxSetIntegerSignal(clientID, "_gripperClose", 0, vrep.simx_opmode_oneshot)
returnCode_3 = vrep.simxSetJointTargetVelocity(clientID, phantom_gripper_close_joint, 2, vrep.simx_opmode_oneshot)
elif x == 1:
returnCode_1, signalValue = vrep.simxGetIntegerSignal(clientID, "_gripperClose", vrep.simx_opmode_streaming)
returnCode_2 = vrep.simxSetIntegerSignal(clientID, "_gripperClose", 1, vrep.simx_opmode_oneshot)
returnCode_3 = vrep.simxSetJointTargetVelocity(clientID, phantom_gripper_close_joint, -2, vrep.simx_opmode_oneshot)
def grasp(self, grasp_pose, surface):
# Set up grasping position and orientation
emptyBuff = self.dummybyte
panda_id = self.cid
# Set grasp position and orientation
matrix = [
grasp_pose[0][0], grasp_pose[1][0], grasp_pose[2][0], surface[0],
grasp_pose[0][1], grasp_pose[1][1], grasp_pose[2][1], surface[1],
grasp_pose[0][2], grasp_pose[1][2], grasp_pose[2][2], surface[2]
]
vrep.simxCallScriptFunction(panda_id, 'grasp',
vrep.sim_scripttype_childscript,
'setmatrix', [], matrix, [], emptyBuff,
vrep.simx_opmode_blocking)
time.sleep(1.0)
# --------------------------------------------------------------------------------------------------------------
# Execute movements
vrep.simxCallScriptFunction(panda_id, 'Sphere',
vrep.sim_scripttype_childscript, 'grasp',
[], [], [], emptyBuff,
vrep.simx_opmode_blocking)
running = True
while running:
res, signal = vrep.simxGetIntegerSignal(
panda_id, "finish", vrep.simx_opmode_oneshot_wait)
if signal == 18:
running = False
else:
running = True
def __init__(self, id):
super(ModeMA, self).__init__(id)
# constant shared with lua
self.n_agent = vrep.simxGetIntegerSignal(self.id_, "agents",
vrep.simx_opmode_blocking)[1]
self.s_act_ = []
self.s_obs_ = []
for i in range(self.n_agent):
self.s_obs_.append(
self._get_Space("state", "Agent" + str(i + 1) + "_"))
self.s_act_.append(
self._get_Space("action", "Agent" + str(i + 1) + "_"))
# variables will be received
self.state = [None for _ in range(self.n_agent)]
self.reward = np.zeros(self.n_agent)
self.done = False
# variables will be sended
self.action = [
np.zeros_like(self.s_act_[i].high) for i in range(self.n_agent)
]
# enable streaming
for i in range(self.n_agent):
self.state[i], self.reward[i] = self._get_StateReward(
prefix="Agent" + str(i + 1) + "_", init=True)
self.done = self._check_Done(init=True)
def get_image_from_vrep(clientID, cam_handle):
iter = 60
for i in range(iter): # repeat iter number of times
time.sleep(1)
# Check if there is an object under the camera:
_, signal = vrep.simxGetIntegerSignal(clientID, "takeImage",
vrep.simx_opmode_blocking)
if int(signal) == 1:
print("got image")
#There is an object under the camera
err, resolution, image = vrep.simxGetVisionSensorImage(
clientID, cam_handle, 0, vrep.simx_opmode_blocking)
img = np.array(image, dtype=np.uint8)
print(img.shape, "shape")
img.resize([resolution[1], resolution[0], 3])
if err == vrep.simx_return_ok:
im = Image.fromarray(img)
im.save("current_image.png")
elif err == vrep.simx_return_novalue_flag:
print("no image yet")
pass
else:
print(err)
return image
def simulation_paused(self):
"""Returns True if the simulation is paused but not stopped"""
res, state = remote_api.simxGetIntegerSignal(self.api_id, 'state',
remote_api.simx_opmode_buffer)
if res == 0:
return state == 0
return False
def run_threaded_drop(self, frame_work2obj):
"""Gets an initial object pose by 'dropping' it WRT frame_work2obj.
This function launches a threaded script that sets an object position,
enables the object to be dynamically simulated, and allows it to fall
to the ground and come to a resting pose. This is usually the first
step when collecting grasps.
Note that we also set the gripper to be non-collidable / respondable,
so it doesn't interfere with the drop process
"""
self._clear_signals()
self.set_gripper_properties(visible=True,
dynamic=True,
collidable=True)
frame = self._format_matrix(frame_work2obj)
# Launch the threaded script
vrep.simxSetStringSignal(self.clientID, 'run_drop_object',
vrep.simxPackFloats(frame),
vrep.simx_opmode_oneshot)
r = -1
while r != vrep.simx_return_ok:
r, success = vrep.simxGetIntegerSignal(
self.clientID, 'object_resting', vrep.simx_opmode_oneshot_wait)
if success == 0:
raise Exception('Error dropping object! Return code: ', r)
def GetSuctionPad():
result, suck = vrep.simxGetIntegerSignal(clientID, 'suc_IO',
vrep.simx_opmode_blocking)
if result != vrep.simx_return_ok:
raise Exception('could not get suction variable')
#print('Getsuction',suck)
return suck
def simulation_paused(self):
"""Returns True if the simulation is paused but not stopped"""
res, state = remote_api.simxGetIntegerSignal(
self.api_id, 'state', remote_api.simx_opmode_buffer)
if res == 0:
return state == 0
return False
def getEmotion(self, rob):
ok, val = vrep.simxGetIntegerSignal(self.getClientID(),
rob.getName() + ":Emotion",
vrep.simx_opmode_streaming)
if ok == vrep.simx_return_ok:
return val
else:
return 0 # neutral emotion
def detectCollisionSignal(clientID):
collision_str = "collision_signal"
detector = vrep.simxGetIntegerSignal(clientID, collision_str,
vrep.simx_opmode_oneshot_wait)
if detector[1] == 1:
return 1
else:
return 0
def __get(self):
vrep.simxGetPingTime(self.__ID)
self.state = np.array(
vrep.simxUnpackFloats(
vrep.simxGetStringSignal(self.__ID, "states",
vrep.simx_opmode_buffer)[1]))
self.reward = vrep.simxGetFloatSignal(self.__ID, "reward",
vrep.simx_opmode_buffer)[1]
self.done = bool(
vrep.simxGetIntegerSignal(self.__ID, "done",
vrep.simx_opmode_buffer)[1])
def path_stop(self):
hit = 0
res, hit = vrep.simxGetIntegerSignal(self.clientID, 'path_stop',
vrep.simx_opmode_blocking)
if res == vrep.simx_return_ok:
print(
"path stop ", hit
) # display the reply from V-REP (in this case, the handle of the created dummy)
else:
print('Remote function call failed: path_stop')
return hit
def check_collision(self):
if self.id != -1:
mode = 0
if self.sim_first_call:
mode = vrepConst.simx_opmode_streaming
else:
mode = vrepConst.simx_opmode_buffer
res = vrep.simxGetIntegerSignal(self.id, 'collision_detection',
mode)
time.sleep(0.03)
self.sim_first_call = False
return res[1]
def detectCollisionSignal(clientID):
detector = 0
collision_str = "collision_signal"
detector = vrep.simxGetIntegerSignal(clientID, collision_str,
vrep.simx_opmode_oneshot_wait)
start = time.time()
while (time.time() < start + 1):
pass
if detector[1] == 1:
# print ("\nHit")
return 1
else:
# print ("\nMiss")
return 0
def is_hit(self):
hit = 0
res, hit = vrep.simxGetIntegerSignal(self.clientID, 'hit',
vrep.simx_opmode_blocking)
if res == vrep.simx_return_ok:
pass
#print ("hit ", hit) # display the reply from V-REP (in this case, the handle of the created dummy)
else:
print('Remote function call failed: is_hit')
if (hit == 1):
vrep.simxSetIntegerSignal(self.clientID, 'hit', 0,
vrep.simx_opmode_blocking)
return hit
def step(self, action):
assert vrep.simxGetConnectionId(self.clientID) != -1
# TODO SEND THE ACTION
vrep.simxSynchronousTrigger(self.clientID)
res, iteration1 = vrep.simxGetIntegerSignal(self.clientID, "iteration",
vrep.simx_opmode_buffer)
if res != vrep.simx_return_ok: iteration1 = -1
iteration2 = iteration1
while iteration2 == iteration1: # wait until the iteration counter has changed
res, iteration2 = vrep.simxGetIntegerSignal(
self.clientID, "iteration", vrep.simx_opmode_buffer)
if res != vrep.simx_return_ok: iteration2 = -1
logging.debug(f"iteration2: {iteration2}")
#Get the image of the vision sensor
res, resolution, image = vrep.simxGetVisionSensorImage(
self.clientID, self.visionSensorHandle, 0, vrep.simx_opmode_buffer)
#Transform the image so it can be displayed
self.img = np.array(image, dtype=np.uint8).reshape(
[resolution[1], resolution[0], 3])
done = True if iteration2 > self.episode_length else False
reward, info = 0, {} # TODO
return self.img, reward, done, info # TODO .copy() ?
def stop_simulation(self):
if not self.sim_running:
raise RuntimeError('Simulation is not running.')
self.RAPI_rc(vrep.simxStopSimulation(self.cID, vrep.simx_opmode_blocking))
# Checking if the server really stopped
try:
while True:
self.RAPI_rc(vrep.simxGetIntegerSignal(self.cID,'sig_debug',vrep.simx_opmode_blocking))
e = vrep.simxGetInMessageInfo(self.cID,vrep.simx_headeroffset_server_state)
still_running = e[1] & 1
if not still_running:
break
except: pass
self.sim_running = False
def __init__(self, id):
super(ModeMO, self).__init__(id)
# constant shared with lua
self.n_task = vrep.simxGetIntegerSignal(self.id_, "tasks",
vrep.simx_opmode_blocking)[1]
self.s_obs_ = self._get_Space("state")
self.s_act_ = self._get_Space("action")
# variables will be received
self.state = np.zeros(len(self.s_obs_.high))
self.reward = 0.0
self.done = False
# variables will be sended
self.action = np.zeros(len(self.s_act_.high) + self.n_task)
# enable streaming
self.state, self.reward = self._get_StateReward(init=True)
self.done = self._check_Done(init=True)
def reset(self):
# Set up grasping position and orientation
emptyBuff = self.dummybyte
panda_id = self.cid
# --------------------------------------------------------------------------------------------------------------
# Execute movements
res, retInts, retFloats, retStrings, retBuffer = vrep.simxCallScriptFunction(
panda_id, 'Sphere', vrep.sim_scripttype_childscript, 'reset', [],
[], [], emptyBuff, vrep.simx_opmode_blocking)
running = True
while running:
res, signal = vrep.simxGetIntegerSignal(
panda_id, "finish", vrep.simx_opmode_oneshot_wait)
if signal == 18:
running = False
else:
running = True
def push(self, push_pose):
emptyBuff = self.dummybyte
panda_id = self.cid
# ----------------------------------------------------------------------------------------------------------------------
# Push Pose Generation
res, target1 = vrep.simxGetObjectHandle(panda_id, 'lift0',
vrep.simx_opmode_oneshot_wait)
res, target2 = vrep.simxGetObjectHandle(panda_id, 'grasp',
vrep.simx_opmode_oneshot_wait)
res, target3 = vrep.simxGetObjectHandle(panda_id, 'lift',
vrep.simx_opmode_oneshot_wait)
angles = push_pose[1]
# Set pushing point and orientation
res1 = vrep.simxSetObjectPosition(
panda_id, target1, -1,
[push_pose[0][0], push_pose[0][1], push_pose[0][2] + 0.25],
vrep.simx_opmode_oneshot)
res2 = vrep.simxSetObjectOrientation(panda_id, target1, -1, angles,
vrep.simx_opmode_oneshot)
# Set landing position
res1 = vrep.simxSetObjectPosition(panda_id, target2, -1, push_pose[2],
vrep.simx_opmode_oneshot)
res2 = vrep.simxSetObjectOrientation(panda_id, target2, -1, angles,
vrep.simx_opmode_oneshot)
# Set pushing direction
res3 = vrep.simxSetObjectPosition(panda_id, target3, -1, push_pose[3],
vrep.simx_opmode_oneshot)
res4 = vrep.simxSetObjectOrientation(panda_id, target3, -1, angles,
vrep.simx_opmode_oneshot)
time.sleep(10)
# Execute movements
res, retInts, retFloats, retStrings, retBuffer = vrep.simxCallScriptFunction(
panda_id, 'Sphere', vrep.sim_scripttype_childscript, 'push', [],
[], [], emptyBuff, vrep.simx_opmode_blocking)
print('pushing signal sent')
running = True
while running:
res, signal = vrep.simxGetIntegerSignal(
panda_id, "finish", vrep.simx_opmode_oneshot_wait)
if signal == 18:
running = False
else:
running = True
def reset(self):
# stop the simulation
vrep.simxStopSimulation(self.clientID, vrep.simx_opmode_blocking)
vrep.simxSynchronous(self.clientID, 1)
# start the simulation
vrep.simxStartSimulation(self.clientID, vrep.simx_opmode_blocking)
# enable streaming of the iteration counter:
res, iteration1 = vrep.simxGetIntegerSignal(self.clientID, "iteration",
vrep.simx_opmode_streaming)
logging.debug(f"iteration1: {iteration1}")
#Get the handle of the vision sensor
_, self.visionSensorHandle = vrep.simxGetObjectHandle(
self.clientID, self.camera_name, vrep.simx_opmode_oneshot_wait)
#Get the image
_, resolution, image = vrep.simxGetVisionSensorImage(
self.clientID, self.visionSensorHandle, 0,
vrep.simx_opmode_streaming)
if self.figure is not None: plt.close() # try?
self.figure = None
return self.step(0)
def _get_gama_range(self, side):
gama_range = []
if (side == 'top'):
gama_range = range(0, 90) + range(270, 360)
elif (side == 'bottom'):
gama_range = range(90, 270)
elif (side == 'left'):
gama_range = range(0, 180)
elif (side == 'right'):
gama_range = range(180, 360)
else:
res, angle = vrep.simxGetIntegerSignal(self.clientID, 'gama',
vrep.simx_opmode_blocking)
if res == vrep.simx_return_ok:
pass
#print ("gama ", angle) # display the reply from V-REP (in this case, the handle of the created dummy)
else:
print('Remote function call failed: get gama')
gama_range = range(angle - 30, angle + 30)
return gama_range
def grasp(self, grasp_pose, surface):
# Set up grasping position and orientation
emptyBuff = self.dummybyte
panda_id = self.cid
# Get target and lift handles
res, target1 = vrep.simxGetObjectHandle(panda_id, 'grasp',
vrep.simx_opmode_oneshot_wait)
res, target2 = vrep.simxGetObjectHandle(panda_id, 'lift',
vrep.simx_opmode_oneshot_wait)
# Set grasp position and orientation
tmp_angles = mat2euler(grasp_pose)
angles = [-tmp_angles[0], -tmp_angles[1], -tmp_angles[2]]
res1 = vrep.simxSetObjectPosition(panda_id, target1, -1, surface,
vrep.simx_opmode_oneshot)
res2 = vrep.simxSetObjectOrientation(panda_id, target1, -1, angles,
vrep.simx_opmode_oneshot)
# Set lift position and orientation
res3 = vrep.simxSetObjectPosition(
panda_id, target2, -1, [surface[0], surface[1], surface[2] + 0.1],
vrep.simx_opmode_oneshot)
res4 = vrep.simxSetObjectOrientation(panda_id, target2, -1, angles,
vrep.simx_opmode_oneshot)
time.sleep(1.0)
# --------------------------------------------------------------------------------------------------------------
# Execute movements
res, retInts, retFloats, retStrings, retBuffer = vrep.simxCallScriptFunction(
panda_id, 'Sphere', vrep.sim_scripttype_childscript, 'grasp', [],
[], [], emptyBuff, vrep.simx_opmode_blocking)
running = True
while running:
res, signal = vrep.simxGetIntegerSignal(
panda_id, "finish", vrep.simx_opmode_oneshot_wait)
if signal == 18:
running = False
else:
running = True
def __init__(self, config={}, port_num=19997):
print ('Program started')
vrep.simxFinish(-1) # just in case, close all opened connections
self.clientID = vrep.simxStart('127.0.0.1', port_num, True,
True, 5000, 5) # Connect to V-REP
if self.clientID != -1:
print ('Connected to remote API server')
vrep.simxStartSimulation(
self.clientID, vrep.simx_opmode_oneshot_wait)
vrep.simxSynchronous(self.clientID, True)
# Now try to retrieve data in a blocking fashion (i.e. a service
# call):
res, objs = vrep.simxGetObjects(
self.clientID, vrep.sim_handle_all, vrep.simx_opmode_blocking)
if res == vrep.simx_return_ok:
print ('Number of objects in the scene: ', len(objs))
else:
print ('Remote API function call returned with error code: ', res)
print("connected through port number: {}".format(port_num))
# used to connect multiple clients in synchronous mode http://www.coppeliarobotics.com/helpFiles/en/remoteApiModusOperandi.htm
return_code, iteration = vrep.simxGetIntegerSignal(self.clientID, "iteration", vrep.simx_opmode_streaming)
time.sleep(2)
def _reset(self):
if self._firstSim:
self._connect()
vrep.simxLoadScene(self.clientID, self._scene, 0,
vrep.simx_opmode_blocking)
else:
vrep.simxStopSimulation(self.clientID, vrep.simx_opmode_oneshot)
number_of_iteration_not_stopped = 0
while True:
vrep.simxGetIntegerSignal(self.clientID, 'asdf',
vrep.simx_opmode_blocking)
e = vrep.simxGetInMessageInfo(
self.clientID, vrep.simx_headeroffset_server_state)
not_stopped = e[1] & 1
if not not_stopped:
print("STOPPED")
break
else:
if number_of_iteration_not_stopped % 10 == 0:
print(number_of_iteration_not_stopped, ': not stopped')
number_of_iteration_not_stopped += 1
if number_of_iteration_not_stopped > 100:
self._firstSim = True
self._connect()
vrep.simxLoadScene(self.clientID, self._scene, 0,
vrep.simx_opmode_blocking)
errorCode, handles, intData, floatData, stringData = vrep.simxGetObjectGroupData(
self.clientID, vrep.sim_appobj_object_type, 0,
vrep.simx_opmode_blocking)
self._handles_dict = dict(zip(stringData, handles))
tmp = []
self.new_trajectory = {key: list(tmp) for key in self._order}
for el in self._order:
vrep.simxSetJointTargetPosition(self.clientID,
self._handles_dict[el], 0,
vrep.simx_opmode_oneshot)
vrep.simxGetJointPosition(self.clientID, self._handles_dict[el],
vrep.simx_opmode_streaming)
#
vrep.simxStartSimulation(self.clientID, vrep.simx_opmode_oneshot)
for i in range(self._range):
if self._firstSim:
print("FirStSim iteration: ", i)
for el in self._order:
vrep.simxSynchronousTrigger(self.clientID)
vrep.simxSetJointTargetPosition(
self.clientID, self._handles_dict[el],
self._rad(self._walking_data[el][i]),
vrep.simx_opmode_streaming)
else:
print("Reset iteration: ", i)
for el in self._order:
vrep.simxSynchronousTrigger(self.clientID)
vrep.simxSetJointTargetPosition(
self.clientID, self._handles_dict[el],
self._rad(self._walking_data[el][i]),
vrep.simx_opmode_streaming)
if i == (self._range - 2):
lastposlist = []
for jo in self._order:
errorCode, lastpostmp = vrep.simxGetJointPosition(
self.clientID, self._handles_dict[jo],
vrep.simx_opmode_streaming)
lastposlist.append(lastpostmp)
rightSenValues, leftSenValues = _GetSensorValues(
self.clientID, self._rightSen, self._leftSen, self._handles_dict)
# print("RightSensors: ", rightSenValues)
# print("LeftSensors: ", leftSenValues)
rightSenSum = np.sum(np.asarray(rightSenValues))
leftSenSum = np.sum(np.asarray(leftSenValues))
zmpX, zmpY = _GetZmp(self.clientID, self._handles_dict, self._floor,
rightSenValues, leftSenValues, rightSenSum,
leftSenSum)
# print("ZMPX: ", zmpX)
# print("ZMPy: ", zmpY)
errorCode, sphere = vrep.simxGetObjectPosition(
self.clientID, self._handles_dict['Sphere'],
self._handles_dict[self._floor], vrep.simx_opmode_streaming)
self._sphere_height = sphere[2]
# print("Sphere height: ", self._sphere_height)
CurrentPos, CurrentVel = _GetJointValues(self.clientID,
self._handles_dict,
self._order, lastposlist)
# print("Position: ", CurrentPos)
# print("Velocity: ", CurrentVel)
self._state.Reset()
self._state.SetPosVel(CurrentPos, CurrentVel)
if self._firstSim:
self._firstSim = False
self._range = 15
self._reset()
self._done = False
self._quit = False
return self._state.GetState()
def single_object_evaluation():
model = load_model('/tmp/cnn3d_train/grasping_epoch150_train_cnn3d_original.h5')
experiment_number = 0
vrep.simxFinish(-1)
# Connect to V-REP on port 19997
clientID = vrep.simxStart('127.0.0.1', 19997, True, True, 5000, 5)
if clientID != -1:
print('Connected to simulation.')
while 1:
vrep.simxStopSimulation(clientID, vrep.simx_opmode_oneshot_wait)
time.sleep(5.0)
vrep.simxStartSimulation(clientID, vrep.simx_opmode_oneshot_wait)
emptyBuff = bytearray()
# Generate object pcd file from V-REP
# object_name, object_handle = add_one_object(clientID)
res, object_handle, retFloats, retStrings, retBuffer = vrep.simxCallScriptFunction(clientID, 'Sphere',
vrep.sim_scripttype_childscript,
'addRandObject', [], [], [],
emptyBuff,
vrep.simx_opmode_blocking)
object_handle = object_handle[0]
time.sleep(1.5)
res, object_pos = vrep.simxGetObjectPosition(clientID,object_handle,-1,vrep.simx_opmode_oneshot_wait)
pointcloud = setup_sim_camera(clientID)
# Object segmentation
pcd = remove_clipping(pointcloud)
# v = pptk.viewer(pcd)
# Save pcd file
np.savetxt('data.pcd', pcd, delimiter=' ')
insertHeader('data.pcd')
# Generate grasp poses
poses = GraspPoseGeneration('data.pcd')
poses.generate_candidates()
print('Number of valid grasps: ', poses.n_samples)
# use learned 3dcnn to pick the best grasp
if poses.n_samples != 0:
value, index = best_of(model,poses,pcd)
else:
continue
if value < 0.9:
continue
# --------------------------------------------------------------------------------------------------------------
# execute
# Get target and lift handles
res, target1 = vrep.simxGetObjectHandle(clientID, 'grasp', vrep.simx_opmode_oneshot_wait)
res, target2 = vrep.simxGetObjectHandle(clientID, 'lift', vrep.simx_opmode_oneshot_wait)
# Set grasp position and orientation
tmp_angles = rotationMatrixToEulerAngles(poses.rotm[index])
angles = [-tmp_angles[0],-tmp_angles[1],-tmp_angles[2]]
res1 = vrep.simxSetObjectPosition(clientID, target1, -1, poses.surface[index], vrep.simx_opmode_oneshot)
res2 = vrep.simxSetObjectOrientation(clientID, target1, -1, angles, vrep.simx_opmode_oneshot)
# Set lift position and orientation
res3 = vrep.simxSetObjectPosition(clientID, target2, -1,
[poses.surface[index][0], poses.surface[index][1], poses.surface[index][2] + 0.1],
vrep.simx_opmode_oneshot)
res4 = vrep.simxSetObjectOrientation(clientID, target2, -1, angles, vrep.simx_opmode_oneshot)
time.sleep(1.0)
print('Original Position is: ', object_pos)
# Execute movements
res, retInts, retFloats, retStrings, retBuffer = vrep.simxCallScriptFunction(clientID, 'Sphere',
vrep.sim_scripttype_childscript,
'go', [], [], [],
emptyBuff,
vrep.simx_opmode_blocking)
running = True
while running:
res, signal = vrep.simxGetIntegerSignal(clientID, "finish", vrep.simx_opmode_oneshot_wait)
if signal == 18:
running = False
else:
running = True
print('recording data ...')
# Recording data
res,current_pos = vrep.simxGetObjectPosition(clientID,object_handle,-1,vrep.simx_opmode_oneshot_wait)
print('Current Position is: ', current_pos)
if current_pos[2]>object_pos[2]+0.03:
test_res = 1
else:
test_res = 0
print(test_res)
vg = voxel_write(index, poses, pcd)
experiment_result = 'data/experiment_number_' + str(experiment_number)
np.save(experiment_result, vg)
experiment_number = experiment_number + 1
f = open('label.txt', "a+")
f.write(str(test_res))
f.close()
print('test completed, starting next iteration ...')
else:
print('Failed to connect to simulation (V-REP remote API server). Exiting.')
exit()
def push():
x_p = [[-0.125,0,0.05],[0.125,0,0.05]]
x_n = [[0.125,0,0.05],[-0.125,0,0.05]]
y_p = [[0,-0.125,0.05],[0,0.125,0.05]]
y_n = [[0,0.125,0.05],[0,-0.125,0.05]]
xy_p = [[-0.0884,-0.0884,0.05],[0.0884,0.0884,0.05]]
xy_n = [[0.0884,0.0884,0.05],[-0.0884,-0.0884,0.05]]
xp_yn = [[-0.0884,0.0884,0.05],[0.0884,-0.0884,0.05]]
xn_yp = [[0.0884,-0.0884,0.05],[-0.0884,0.0884,0.05]]
directions = [x_p,x_n,y_p,y_n,xy_p,xp_yn,xn_yp,xy_n]
# add object
object_name, object_handle = add_three_objects(clientID)
time.sleep(5.0)
num_obj = 3
object_pos = []
object_angle = []
for obj_i in range(num_obj):
res, pos = vrep.simxGetObjectPosition(clientID,object_handle[obj_i],-1,vrep.simx_opmode_oneshot_wait)
res, orientation = vrep.simxGetObjectOrientation(clientID,object_handle[obj_i],-1,vrep.simx_opmode_oneshot_wait)
object_pos.append(pos)
object_angle.append(orientation)
# Generate object pcd file from V-REP
sim_ret, cam_handle = vrep.simxGetObjectHandle(clientID, 'kinect_depth', vrep.simx_opmode_blocking)
emptyBuff = bytearray()
res, retInts, retFloats, retStrings, retBuffer = vrep.simxCallScriptFunction(clientID, 'kinect',
vrep.sim_scripttype_childscript,
'absposition', [], [], [], emptyBuff,
vrep.simx_opmode_blocking)
R = np.asarray([[retFloats[0], retFloats[1], retFloats[2], retFloats[3]],
[retFloats[4], retFloats[5], retFloats[6], retFloats[7]],
[retFloats[8], retFloats[9], retFloats[10], retFloats[11]]])
# print('camera pose is: ',R)
result, state, data = vrep.simxReadVisionSensor(clientID, cam_handle, vrep.simx_opmode_blocking)
data = data[1]
pointcloud = []
for i in range(2, len(data), 4):
p = [data[i], data[i + 1], data[i + 2], 1]
pointcloud.append(np.matmul(R, p))
# Object segmentation
pcl = remove_clipping(pointcloud)
# Save pcd file
np.savetxt('data.pcd', pcl, delimiter=' ')
insertHeader('data.pcd')
nb_clutters = segmentation('data.pcd')
label = [0,0,0,0,0,0,0,0]
if nb_clutters == num_obj:
continue
print('Number of objects: %d' % nb_clutters)
vg = transform(pcl)
input_shape = vg.reshape((1,1,32,32,32))
p = model.predict(input_shape, verbose=False)
p = p[0]
print('Predicted 8-dir success rate: ', p)
best_dir = np.argmax(p)
direction = directions[best_dir]
print('The best direction is %d' % best_dir)
f = open('predictions.txt', "a+")
f.write(str(best_dir))
f.close()
tries = 1
for direction in directions:
res, target1 = vrep.simxGetObjectHandle(clientID, 'grasp', vrep.simx_opmode_oneshot_wait)
res, target2 = vrep.simxGetObjectHandle(clientID, 'lift', vrep.simx_opmode_oneshot_wait)
res, target3 = vrep.simxGetObjectHandle(clientID, 'lift0', vrep.simx_opmode_oneshot_wait)
angles = [-3.14, 0, 0]
# Set landing position
res1 = vrep.simxSetObjectPosition(clientID, target1, -1, direction[0], vrep.simx_opmode_oneshot)
res2 = vrep.simxSetObjectOrientation(clientID, target1, -1, angles, vrep.simx_opmode_oneshot)
# Set pushing direction
res3 = vrep.simxSetObjectPosition(clientID, target2, -1, direction[1], vrep.simx_opmode_oneshot)
res4 = vrep.simxSetObjectOrientation(clientID, target2, -1, angles, vrep.simx_opmode_oneshot)
# Set wait position
res5 = vrep.simxSetObjectPosition(clientID, target3, -1, [direction[1][0],direction[1][1],direction[1][2]+0.15], vrep.simx_opmode_oneshot)
res6 = vrep.simxSetObjectOrientation(clientID, target3, -1, angles, vrep.simx_opmode_oneshot)
# Execute movements
res, retInts, retFloats, retStrings, retBuffer = vrep.simxCallScriptFunction(clientID, 'Sphere',
vrep.sim_scripttype_childscript,
'go', [], [], [],
emptyBuff,
vrep.simx_opmode_blocking)
print('execution signal sent')
running = True
while running:
res, signal = vrep.simxGetIntegerSignal(clientID, 'finish', vrep.simx_opmode_oneshot_wait)
if signal == tries:
running = False
else:
running = True
print('recording data ...')
# Recording data
time.sleep(1.0)
# After pushing
result, state, new_data = vrep.simxReadVisionSensor(clientID, cam_handle, vrep.simx_opmode_blocking)
new_data = new_data[1]
new_pointcloud = []
for i in range(2, len(data), 4):
p = [new_data[i], new_data[i + 1], new_data[i + 2], 1]
new_pointcloud.append(np.matmul(R, p))
# Object segmentation
new_pcl = remove_clipping(new_pointcloud)
np.savetxt('data_new.pcd', new_pcl, delimiter=' ')
insertHeader('data_new.pcd')
# v = pptk.viewer(new_pcl) # Visualize pcd if needed
nb_clutters_new = segmentation('data_new.pcd')
print('Number of objects: %d' % nb_clutters_new)
if nb_clutters_new>nb_clutters:
dir_index = directions.index(direction)
print('Tried direction:', direction)
print('Number %d in directions list' % dir_index)
label[dir_index]=1
print('Updated label:', label)
else:
print('Pushing not meaningful ...')
for j in range(num_obj):
vrep.simxSetObjectPosition(clientID, object_handle[j], -1, object_pos[j], vrep.simx_opmode_oneshot_wait)
vrep.simxSetObjectOrientation(clientID, object_handle[j], -1, object_angle[j], vrep.simx_opmode_oneshot_wait)
time.sleep(0.5)
tries = tries+1
print(label)
def integer_signal(cid, handle, signal_name):
err, val = vrep.simxGetIntegerSignal(cid, signal_name, vrep_mode)
return [val]
def __init__(self, scene="rollbalance", is_render=True, is_boot=True):
# import V-REP
if "linux" in sys.platform:
self.VREP_DIR = os.path.expanduser("~") + "/V-REP_PRO_EDU/"
vrepExe = "vrep.sh"
elif "darwin" in sys.platform:
self.VREP_DIR = "/Applications/V-REP_PRO_EDU/"
vrepExe = "vrep.app/Contents/MacOS/vrep"
else:
print(sys.platform)
sys.stderr.write("I don't know how to use vrep in Windows...\n")
sys.exit(-1)
sys.path.append(self.VREP_DIR +
"programming/remoteApiBindings/python/python/")
try:
global vrep
import vrep
except:
print(
'--------------------------------------------------------------'
)
print(
'"vrep.py" could not be imported. This means very probably that'
)
print(
'either "vrep.py" or the remoteApi library could not be found.'
)
print('Make sure both are in the same folder as this file,')
print('or appropriately adjust the file "vrep.py"')
print(
'--------------------------------------------------------------'
)
sys.exit(-1)
# start V-REP
self.IS_BOOT = is_boot
if self.IS_BOOT:
vrepArgs = [
self.VREP_DIR + vrepExe,
os.path.abspath(os.path.dirname(__file__)) + "/scenes/" +
scene + ".ttt"
]
if not is_render:
vrepArgs.extend(["-h"])
vrepArgs.extend(["&"])
self.vrepProcess = subprocess.Popen(vrepArgs,
stdout=open(os.devnull, 'w'),
stderr=subprocess.STDOUT,
preexec_fn=os.setsid)
print("Enviornment was opened: {}, {}".format(
vrepArgs[0], vrepArgs[1]))
# connect to V-REP
ipAddress = "127.0.0.1"
portNum = 19997
self.__ID = vrep.simxStart(ipAddress, portNum, True, True, 5000, 1)
while self.__ID == -1:
self.__ID = vrep.simxStart(ipAddress, portNum, True, True, 5000, 1)
# start to set constants
vrep.simxSynchronous(self.__ID, True)
vrep.simxStartSimulation(self.__ID, vrep.simx_opmode_blocking)
vrep.simxSynchronousTrigger(self.__ID)
if is_render:
vrep.simxSetBooleanParameter(self.__ID,
vrep.sim_boolparam_display_enabled,
False, vrep.simx_opmode_oneshot)
# constant shared with lua
self.DT = vrep.simxGetFloatSignal(self.__ID, "dt",
vrep.simx_opmode_blocking)[1]
max_state = np.array(
vrep.simxUnpackFloats(
vrep.simxGetStringSignal(self.__ID, "max_state",
vrep.simx_opmode_blocking)[1]))
max_action = np.array(
vrep.simxUnpackFloats(
vrep.simxGetStringSignal(self.__ID, "max_action",
vrep.simx_opmode_blocking)[1]))
min_state = np.array(
vrep.simxUnpackFloats(
vrep.simxGetStringSignal(self.__ID, "min_state",
vrep.simx_opmode_blocking)[1]))
min_action = np.array(
vrep.simxUnpackFloats(
vrep.simxGetStringSignal(self.__ID, "min_action",
vrep.simx_opmode_blocking)[1]))
# limits of respective states and action
self.observation_space = spaces.Box(min_state, max_state)
self.action_space = spaces.Box(min_action, max_action)
# variables will be received
self.state = np.zeros(len(max_state))
self.reward = 0.0
self.done = False
# variables will be sended
self.action = np.zeros(len(max_action))
# enable streaming
self.state = np.array(
vrep.simxUnpackFloats(
vrep.simxGetStringSignal(self.__ID, "states",
vrep.simx_opmode_streaming)[1]))
self.reward = vrep.simxGetFloatSignal(self.__ID, "reward",
vrep.simx_opmode_streaming)[1]
self.done = bool(
vrep.simxGetIntegerSignal(self.__ID, "done",
vrep.simx_opmode_streaming)[1])
# stop simulation
self.__stop()
self.IS_RECORD = False
if clientID!=-1:
print ('Connected to remote API server')
# setup a useless signal
vrep.simxSetIntegerSignal(
clientID,'asdf',1,vrep.simx_opmode_blocking)
for j in range(5):
print('---------------------simulation',j)
# IMPORTANT
# you should poll the server state to make sure
# the simulation completely stops before starting a new one
while True:
# poll the useless signal (to receive a message from server)
vrep.simxGetIntegerSignal(
clientID,'asdf',vrep.simx_opmode_blocking)
# check server state (within the received message)
e = vrep.simxGetInMessageInfo(clientID,
vrep.simx_headeroffset_server_state)
# check bit0
not_stopped = e[1] & 1
if not not_stopped:
break
else:
print('not_stopped')
# IMPORTANT
# you should always call simxSynchronous()
def integer_signal(cid, handle, signal_name):
err, val = vrep.simxGetIntegerSignal(cid, signal_name, vrep_mode)
return [val]
def train(load_model_path, save_model_path, number_training_steps,
data_path, image_shape, ratio=1, batch_size=2,
save_online_batch=False, save_online_batch_path=None):
# Initialize connection
connection = VrepConnection()
connection.synchronous_mode()
connection.start()
# Load data
data_file = h5py.File(data_path,"r")
x = h5py_to_array(data_file['images'][:1000], image_shape)
y = data_file['joint_vel'][:1000]
datapoints = x.shape[0]
# Initialize ratios
online_batchsize = int(np.floor(1.0 * batch_size/(ratio + 1)))
data_images_batchsize = int(batch_size - online_batchsize)
current_online_batch = 0
x_batch = np.empty((batch_size,) + image_shape)
y_batch = np.empty((batch_size, 6))
jointpos_array = np.empty((batch_size, 6))
nextpos_array = np.empty((batch_size, 6))
print "Batch size: ", batch_size
print "Online batch size: ", online_batchsize
print "Dataset batch size: ", data_images_batchsize
# Load keras model
model = load_model(load_model_path)
# Use client id from connection
clientID = connection.clientID
# Get joint handles
jhList = [-1, -1, -1, -1, -1, -1]
for i in range(6):
err, jh = vrep.simxGetObjectHandle(clientID, "Mico_joint" + str(i + 1), vrep.simx_opmode_blocking)
jhList[i] = jh
# Initialize joint position
jointpos = np.zeros(6)
for i in range(6):
err, jp = vrep.simxGetJointPosition(clientID, jhList[i], vrep.simx_opmode_streaming)
jointpos[i] = jp
# Initialize vision sensor
res, v1 = vrep.simxGetObjectHandle(clientID, "vs1", vrep.simx_opmode_oneshot_wait)
err, resolution, image = vrep.simxGetVisionSensorImage(clientID, v1, 0, vrep.simx_opmode_streaming)
vrep.simxGetPingTime(clientID)
err, resolution, image = vrep.simxGetVisionSensorImage(clientID, v1, 0, vrep.simx_opmode_buffer)
# Initialize distance handle
err, distanceHandle = vrep.simxGetDistanceHandle(clientID, "tipToTarget", vrep.simx_opmode_blocking)
err, distance_to_target = vrep.simxReadDistance(clientID, distanceHandle, vrep.simx_opmode_streaming)
# Initialize online batch hdf5 file
if save_online_batch:
f = h5py.File(save_online_batch_path, "w")
dset1 = f.create_dataset("images", (batch_size,) + image_shape, dtype=np.float32)
dset2 = f.create_dataset("joint_pos", (batch_size, 6), dtype=np.float32)
dset3 = f.create_dataset("next_pos", (batch_size, 6), dtype=np.float32)
dset4 = f.create_dataset("distance", (batch_size, 1), dtype=np.float32)
dset5 = f.create_dataset("joint_vel", (batch_size, 6), dtype=np.float32)
# Step while IK movement has not begun
returnCode, signalValue = vrep.simxGetIntegerSignal(clientID, "ikstart", vrep.simx_opmode_streaming)
while (signalValue == 0):
vrep.simxSynchronousTrigger(clientID)
vrep.simxGetPingTime(clientID)
returnCode, signalValue = vrep.simxGetIntegerSignal(clientID, "ikstart", vrep.simx_opmode_streaming)
# Iterate over number of steps to train model online
path_empty_counter = 0
step_counter = 0
while step_counter < number_training_steps:
# 1. Obtain image from vision sensor and next path position from Lua script
err, resolution, image = vrep.simxGetVisionSensorImage(clientID, v1, 0, vrep.simx_opmode_buffer)
img = np.resize(image, (1,) + image_shape) # resize into proper shape for input to neural network
img = img.astype(np.uint8)
img = img.astype(np.float32)
img /= 255
inputInts = []
inputFloats = []
inputStrings = []
inputBuffer = bytearray()
err, _, next_pos, _, _ = vrep.simxCallScriptFunction(clientID, 'Mico',
vrep.sim_scripttype_childscript,
'getNextPathPos', inputInts,
inputFloats, inputStrings,
inputBuffer,
vrep.simx_opmode_blocking)
# 2. Pass into neural network to get joint velocities
jointvel = model.predict(img, batch_size=1)[0] # output is a 2D array of 1X6, access the first variable to get vector
stepsize = 1
jointvel *= stepsize
# 3. Apply joint velocities to arm in V-REP
for j in range(6):
err, jp = vrep.simxGetJointPosition(clientID, jhList[j], vrep.simx_opmode_buffer)
jointpos[j] = jp
err = vrep.simxSetJointPosition(clientID, jhList[j], jointpos[j] + jointvel[j], vrep.simx_opmode_oneshot)
err, distance_to_target = vrep.simxReadDistance(clientID, distanceHandle, vrep.simx_opmode_buffer)
# Check if next pos is valid before fitting
if len(next_pos) != 6:
path_empty_counter += 1
continue
ik_jointvel = joint_difference(jointpos, next_pos)
ik_jointvel = ik_jointvel / np.sum(np.absolute(ik_jointvel)) * 0.5 * distance_to_target
ik_jointvel = np.resize(ik_jointvel, (1, 6))
x_batch[current_online_batch] = img[0]
y_batch[current_online_batch] = ik_jointvel[0]
jointpos_array[current_online_batch] = jointpos
nextpos_array[current_online_batch] = next_pos
dset4[current_online_batch] = distance_to_target
current_online_batch += 1
if current_online_batch == online_batchsize:
# Step counter
print "Training step: ", step_counter
step_counter += 1
# Random sample from dataset
if ratio > 0:
indices = random.sample(range(int(datapoints)), int(data_images_batchsize))
indices = sorted(indices)
x_batch[online_batchsize:] = x[indices]
y_batch[online_batchsize:] = y[indices]
dset4[online_batchsize:] = data_file["distance"][indices]
dset2[online_batchsize:] = data_file["joint_pos"][indices]
# 4. Fit model
model.fit(x_batch, y_batch,
batch_size=batch_size,
epochs=1)
# Reset counter
current_online_batch = 0
# Save to online batch dataset
dset1[:] = x_batch
dset5[:] = y_batch
dset2[:online_batchsize] = jointpos_array[:online_batchsize]
dset3[:] = nextpos_array
# Print statements
# print "Predicted joint velocities: ", jointvel, "Abs sum: ", np.sum(np.absolute(jointvel))
# print "IK Joint velocity: ", ik_jointvel, "Abs sum: ", np.sum(np.absolute(ik_jointvel))
# print "Distance to cube: ", distanceToCube
# trigger next step and wait for communication time
vrep.simxSynchronousTrigger(clientID)
vrep.simxGetPingTime(clientID)
# stop the simulation:
vrep.simxStopSimulation(clientID,vrep.simx_opmode_blocking)
# Now close the connection to V-REP:
vrep.simxFinish(clientID)
# save updated model and delete model
model.save(save_model_path)
del model
# Close file
if save_online_batch:
f.close()
# Error check
print "No of times path is empty:", path_empty_counter
def getEmotion(self, rob):
ok, val = vrep.simxGetIntegerSignal(self.getClientID(), rob.getName()+":Emotion", vrep.simx_opmode_streaming)
if ok==vrep.simx_return_ok:
return val
else:
return 0 # neutral emotion
