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 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