def __init__(self, clientID, name): self.clientID = clientID self.name = name self.noDetectionDist = 0.5 self.maxDetectionDist = 0.2 self.detect = [0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0] self.braitenbergL = [-0.2,-0.4,-0.6,-0.8,-1,-1.2,-1.4,-1.6, 0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0] self.braitenbergR = [-1.6,-1.4,-1.2,-1,-0.8,-0.6,-0.4,-0.2, 0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0] self.v0 = 2 errorCode, self.left_motor_handle = vrep.simxGetObjectHandle(clientID, 'Pioneer_p3dx_leftMotor'+name, vrep.simx_opmode_oneshot_wait) errorCode, self.right_motor_handle = vrep.simxGetObjectHandle(clientID, 'Pioneer_p3dx_rightMotor'+name, vrep.simx_opmode_oneshot_wait) self.ultra_sensors = [] for i in range(1,16): errorCode, sensor_handle = vrep.simxGetObjectHandle(clientID, 'Pioneer_p3dx_ultrasonicSensor'+str(i)+name, vrep.simx_opmode_oneshot_wait) if not errorCode: self.ultra_sensors.append(sensor_handle) returnCode, detectionState, \ detectedPoint, detectedObjectHandle, \ detectedSurfaceNormalVector = \ vrep.simxReadProximitySensor(clientID, self.ultra_sensors[i-1], vrep.simx_opmode_streaming) else: print 'Failed to connect to ultrasonic sensor '+str(i)
def __init__(self): self.ip = '127.0.0.1' self.port = 19997 self.scene = './simu.ttt' self.gain = 2 self.initial_position = [3,3,to_rad(45)] self.r = 0.096 # wheel radius self.R = 0.267 # demi-distance entre les r print('New pioneer simulation started') vrep.simxFinish(-1) self.client_id = vrep.simxStart(self.ip, self.port, True, True, 5000, 5) if self.client_id!=-1: print ('Connected to remote API server on %s:%s' % (self.ip, self.port)) res = vrep.simxLoadScene(self.client_id, self.scene, 1, vrep.simx_opmode_oneshot_wait) res, self.pioneer = vrep.simxGetObjectHandle(self.client_id, 'Pioneer_p3dx', vrep.simx_opmode_oneshot_wait) res, self.left_motor = vrep.simxGetObjectHandle(self.client_id, 'Pioneer_p3dx_leftMotor', vrep.simx_opmode_oneshot_wait) res, self.right_motor = vrep.simxGetObjectHandle(self.client_id, 'Pioneer_p3dx_rightMotor', vrep.simx_opmode_oneshot_wait) self.set_position(self.initial_position) vrep.simxStartSimulation(self.client_id, vrep.simx_opmode_oneshot_wait) else: print('Unable to connect to %s:%s' % (self.ip, self.port))
def __init__(self, clientID): self.clientID = clientID err,leftMotorHandle=vrep.simxGetObjectHandle(clientID,"remoteApiControlledBubbleRobLeftMotor",vrep.simx_opmode_oneshot_wait) if err==vrep.simx_error_noerror: self.leftMotorHandle = leftMotorHandle print('Get handle for left motor: {}'.format(leftMotorHandle)) else: print('Error by getting handle for left motor: {}'.format(err)) err,rightMotorHandle=vrep.simxGetObjectHandle(clientID,"remoteApiControlledBubbleRobRightMotor",vrep.simx_opmode_oneshot_wait) if err==vrep.simx_error_noerror: self.rightMotorHandle = rightMotorHandle print('Get handle for right motor: {}'.format(rightMotorHandle)) else: print('Error by getting handle for right motor: {}'.format(err)) err,visionSensorHandle=vrep.simxGetObjectHandle(clientID,"Vision_sensor",vrep.simx_opmode_oneshot_wait) if err==vrep.simx_error_noerror: self.visionSensorHandle = visionSensorHandle print('Get handle for vision sensor: {}'.format(visionSensorHandle)) else: print('Error by getting handle for vision sensor: {}'.format(err)) err,bubbleRobHandle=vrep.simxGetObjectHandle(clientID,"remoteApiControlledBubbleRob",vrep.simx_opmode_oneshot_wait) #getHandle if err==vrep.simx_error_noerror: self.bubbleRobHandle = bubbleRobHandle print('Get handle for bubbleRob: {}'.format(bubbleRobHandle)) else: print('Error by getting handle for bubbleRob: {}'.format(err)) err,self.bubbleRobStartPosition = vrep.simxGetObjectPosition(clientID, bubbleRobHandle, -1, vrep.simx_opmode_oneshot_wait) #getStartPosition
def restart(self,earlyStop = False): if (self.cid != None): vrep.simxStopSimulation(self.cid, self.mode()) vrep.simxSynchronousTrigger(self.cid) vrep.simxFinish(-1) # just in case, close all opened connections time.sleep(1) self.connect() time.sleep(1) vrep.simxLoadScene(self.cid, '/home/elias/[email protected]/_Winter2015/CSC494/Scenes/Base_Quad.ttt', 0, self.mode()) if earlyStop: vrep.simxStopSimulation(self.cid, self.mode()) vrep.simxSynchronousTrigger(self.cid) vrep.simxFinish(-1) # just in case, close all opened connections return vrep.simxStartSimulation(self.cid, self.mode()) self.runtime = 0 err, self.copter = vrep.simxGetObjectHandle(self.cid, "Quadricopter_base", vrep.simx_opmode_oneshot_wait) err, self.target = vrep.simxGetObjectHandle(self.cid, "Quadricopter_target", vrep.simx_opmode_oneshot_wait) err, self.front_camera = vrep.simxGetObjectHandle(self.cid, 'Quadricopter_frontCamera', vrep.simx_opmode_oneshot) err, lin, ang = vrep.simxGetObjectVelocity(self.cid, self.copter, vrep.simx_opmode_streaming) self.getTargetStart() for i in range(4): self.propellerScripts[i] = vrep.simxGetObjectHandle(self.cid, 'Quadricopter_propeller_respondable' + str(i) + str(1), self.mode())
def __init__(self,clientID): self.desiredSteeringAngle=0 self.desiredWheelRotSpeed=0 self.steeringAngleDx=2*math.pi/180 self.wheelRotSpeedDx=1 self.d=0.755 self.l=2.5772 self.clientID=clientID self.absSpeed={-1:-3,0:0,1:3} self.absAngle={-1:-8*math.pi/180,0:0,1:8*math.pi/180} errorCodeML,ml = vrep.simxGetObjectHandle(clientID,'nakedCar_motorLeft',vrep.simx_opmode_oneshot_wait) errorCodeMR,mr = vrep.simxGetObjectHandle(clientID,'nakedCar_motorRight',vrep.simx_opmode_oneshot_wait) errorCodeSL,sl = vrep.simxGetObjectHandle(clientID,'nakedCar_steeringLeft',vrep.simx_opmode_oneshot_wait) errorCodeSR,sr = vrep.simxGetObjectHandle(clientID,'nakedCar_steeringRight',vrep.simx_opmode_oneshot_wait) self.ml = ml self.mr = mr self.sl = sl self.sr = sr
def __init__(self, **kwargs): self.id = str(kwargs.get("id", "")) self.generation = kwargs.get("generation", 0) self.model = kwargs.get("model") self.DNA = kwargs.get("DNA") self.opmode = kwargs.get("opmode", vrep.simx_opmode_oneshot_wait) print "bot number ", self.id, " args : ", self.DNA self.name = "2W1A" if self.id != "0": self.name += "#" + str(self.id) else: self.id = "" print "Bot [" + self.name + "] created" print "Internal caracteristique : A DEFINIR ASAP!" self.clientID = kwargs.get("clientID") vrep.simxCopyPasteObjects(self.clientID, self.model, self.opmode) ret1, self.wristHandle = vrep.simxGetObjectHandle(self.clientID, "WristMotor" + self.id, kwargs.get("opmode")) ret2, self.elbowHandle = vrep.simxGetObjectHandle(self.clientID, "ElbowMotor" + self.id, kwargs.get("opmode")) ret3, self.shoulderHandle = vrep.simxGetObjectHandle( self.clientID, "ShoulderMotor" + self.id, kwargs.get("opmode") ) ret4, self.robotHandle = vrep.simxGetObjectHandle(self.clientID, self.id, kwargs.get("opmode")) print ret1, ret2, ret3 if ret1 != 0 or ret2 != 0 or ret3 != 0: sys.exit("handlers problem")
def __init__( self, sim_dt=0.01, max_target_distance=3, noise=False, noise_std=[0,0,0,0,0,0], target_func=None, ): super(Quadcopter, self).__init__(sim_dt) err, self.copter = vrep.simxGetObjectHandle(self.cid, "Quadricopter_base", vrep.simx_opmode_oneshot_wait ) err, self.target = vrep.simxGetObjectHandle(self.cid, "Quadricopter_target", vrep.simx_opmode_oneshot_wait ) # Reset the motor commands to zero packedData=vrep.simxPackFloats([0,0,0,0]) raw_bytes = (ctypes.c_ubyte * len(packedData)).from_buffer_copy(packedData) err = vrep.simxSetStringSignal(self.cid, "rotorTargetVelocities", raw_bytes, vrep_mode) self.pos = [0,0,0] self.pos_err = [0,0,0] self.t_pos = [0,0,0] self.lin = [0,0,0] self.ori = [0,0,0] self.ori_err = [0,0,0] self.t_ori = [0,0,0] self.ang = [0,0,0] self.vert_prox_dist = 0 self.left_prox_dist = 0 self.right_prox_dist = 0 # Distance reading recorded when nothing is in range self.max_vert_dist = 1.5 self.max_left_dist = 1.0 self.max_right_dist = 1.0 # Maximum target distance error that can be returned self.max_target_distance = max_target_distance # If noise is being modelled self.noise = noise # Standard Deviation of the noise for the 4 state variables self.noise_std = noise_std # Overwrite the get_target method if the target is to be controlled by a # function instead of by V-REP if target_func is not None: self.step = 0 self.target_func = target_func def get_target(): self.t_pos, self.t_ori = self.target_func( self.step ) self.step += 1 self.get_target = get_target
def getSensorHandles_dr20_(clientID): err_h_infra6, infra6 = vrep.simxGetObjectHandle(clientID,'dr20_infraredSensor6_', vrep.simx_opmode_oneshot_wait) err_h_infra2, infra2 = vrep.simxGetObjectHandle(clientID,'dr20_infraredSensor2_', vrep.simx_opmode_oneshot_wait) err_h_infra5, infra5 = vrep.simxGetObjectHandle(clientID,'dr20_infraredSensor5_', vrep.simx_opmode_oneshot_wait) err_h_infra1, infra1 = vrep.simxGetObjectHandle(clientID,'dr20_infraredSensor1_', vrep.simx_opmode_oneshot_wait) err_h_sonic, sonic = vrep.simxGetObjectHandle(clientID,'dr20_ultrasonicSensor_', vrep.simx_opmode_oneshot_wait) return (err_h_infra6, err_h_infra2, err_h_infra5, err_h_infra1, err_h_sonic, infra6, infra2, infra5, infra1, sonic)
def __init__(self, simulatorModel=None): self.robotOrientationFirstTime = True self.getDistanceToGoalFirstTime = True self.getUpDistanceFirstTime = True self.getObjectPositionFirstTime = True self.sysConf = LoadSystemConfiguration() #this data structure is like a cache for the joint handles self.jointHandleMapping = {} robotConf = LoadRobotConfiguration() self.model = simulatorModel self.LucyJoints = robotConf.getJointsName() for joint in self.LucyJoints: self.jointHandleMapping[joint]=0 self.clientId = self.connectVREP() RETRY_ATTEMPTS = 50 if simulatorModel: for i in range(RETRY_ATTEMPTS): #TODO try to reutilize the same scene for the sake of performance error = self.loadscn(self.clientId, simulatorModel) if not error: break print "retrying connection to vrep" if error: raise VrepException("error loading Vrep robot model", -1) if int(self.sysConf.getProperty("synchronous mode?"))==1: self.synchronous = True vrep.simxSynchronousTrigger(self.clientId) else: self.synchronous = False self.speedmodifier = int(self.sysConf.getProperty("speedmodifier")) #setting the simulation time step self.simulationTimeStepDT = float(self.sysConf.getProperty("simulation time step")) '''#testing printing in vrep error, consoleHandler = vrep.simxAuxiliaryConsoleOpen(self.clientId, "lucy debugging", 8, 22, None, None, None, None, vrep.simx_opmode_oneshot_wait) print "console handler: ", consoleHandler vrep.simxAuxiliaryConsoleShow(self.clientId, consoleHandler, 1, vrep.simx_opmode_oneshot_wait) error = vrep.simxAuxiliaryConsolePrint(self.clientId, consoleHandler, "Hello World", vrep.simx_opmode_oneshot_wait)''' error, self.upDistanceHandle = vrep.simxGetDistanceHandle(self.clientId, "upDistance#", vrep.simx_opmode_blocking) error, self.distLFootToGoalHandle = vrep.simxGetDistanceHandle(self.clientId, "distanceLFootGoal#", vrep.simx_opmode_blocking) error, self.distRFootToGoalHandle = vrep.simxGetDistanceHandle(self.clientId, "distanceRFootGoal#", vrep.simx_opmode_blocking) error, self.bioloidHandle = vrep.simxGetObjectHandle(self.clientId,"Bioloid", vrep.simx_opmode_oneshot_wait) error, self.cuboidHandle = vrep.simxGetObjectHandle(self.clientId,"Cuboid", vrep.simx_opmode_oneshot_wait) error, self.armHandler = vrep.simxGetObjectHandle(self.clientId, "Pivot", vrep.simx_opmode_oneshot_wait) self.populateJointHandleCache(self.clientId) self.isRobotUpFirstCall = True self.getDistanceToSceneGoal() #to fix the first invocation self.getUpDistance() self.isRobotUp(self.clientId) self.armPositionXAxis = -9.0000e-02
def config_handles(self): # Object-handle-configuration errorFlag, self.Quadbase = vrep.simxGetObjectHandle(self.clientID, 'Quadricopter_base', vrep.simx_opmode_oneshot_wait) errorFlag, self.Quadobj = vrep.simxGetObjectHandle(self.clientID, 'Quadricopter', vrep.simx_opmode_oneshot_wait) self.getState(initial=True) time.sleep(0.05)
def __init__( self, *args, **kwargs ): super(SensorQuadcopter, self).__init__(*args, **kwargs) err, self.vert_prox = vrep.simxGetObjectHandle(self.cid, "vert_prox", vrep.simx_opmode_oneshot_wait ) err, self.left_prox = vrep.simxGetObjectHandle(self.cid, "left_prox", vrep.simx_opmode_oneshot_wait ) err, self.right_prox = vrep.simxGetObjectHandle(self.cid, "right_prox", vrep.simx_opmode_oneshot_wait )
def __init__(self, sim_dt=0.05): super(Arm, self).__init__(sim_dt) err, self.hand = vrep.simxGetObjectHandle(self.cid, "hand_end", vrep.simx_opmode_oneshot_wait ) err, self.target = vrep.simxGetObjectHandle(self.cid, "target", vrep.simx_opmode_oneshot_wait ) err, self.hand_joint = vrep.simxGetObjectHandle(self.cid, "hand_joint", vrep.simx_opmode_oneshot_wait ) err, self.arm_joint = vrep.simxGetObjectHandle(self.cid, "arm_joint", vrep.simx_opmode_oneshot_wait )
def __init__(self, address, port): print ('Program started') vrep.simxFinish(-1) self.clientID=vrep.simxStart(address,port,True,True,5000,5) if self.clientID!=-1: print "Conexion establecida, todo en orden" else: print "Conexion con el simulador fallida, reinicia el simulador" error,self.motorFL=vrep.simxGetObjectHandle(self.clientID,'Pioneer_p3dx_rightMotor',vrep.simx_opmode_oneshot_wait) error,self.motorFR=vrep.simxGetObjectHandle(self.clientID,'Pioneer_p3dx_leftMotor',vrep.simx_opmode_oneshot_wait) error,self.camera=vrep.simxGetObjectHandle(self.clientID,'Vision_sensor',vrep.simx_opmode_oneshot_wait) error,self.robot=vrep.simxGetObjectHandle(self.clientID,'Pioneer_p3dx',vrep.simx_opmode_oneshot_wait)
def start(self): print ('Program started') vrep.simxFinish(-1) # just in case, close all opened connections self.clientID = vrep.simxStart('127.0.0.1',19999,True,True,5000,5) # Connect to V-REP if self.clientID != -1: print ('Connected to remote API server') self.LWMotor_hdl = vrep.simxGetObjectHandle(self.clientID,'LeftWheel_Motor', vrep.simx_opmode_oneshot_wait) # LeftWheel Motor handle self.RWMotor_hdl = vrep .simxGetObjectHandle(self.clientID,'RightWheel_Motor', vrep.simx_opmode_oneshot_wait) # RightWheel Motor handle self.Robot_hdl = vrep.simxGetObjectHandle(self.clientID, 'Cubot', vrep.simx_opmode_oneshot_wait) print ('Handle acquired !!') else: print ('Error : connection to vrep failed')
def getDistanceBetwObjects(self, objectNameA, objectNameB): """Get the distance between two named objects in V-REP. Raise exception if either does not exist""" eCode, handleA = vrep.simxGetObjectHandle(self._VREP_clientId, objectNameA, vrep.simx_opmode_oneshot_wait) if eCode != 0: raise Exception("Could not get handle of object", objectNameA) eCode, poseA = vrep.simxGetObjectPosition(self._VREP_clientId, handleA, -1, vrep.simx_opmode_oneshot_wait) eCode, handleB = vrep.simxGetObjectHandle(self._VREP_clientId, objectNameB, vrep.simx_opmode_oneshot_wait) if eCode != 0: raise Exception("Could not get handle of object", objectNameB) eCode, poseB = vrep.simxGetObjectPosition(self._VREP_clientId, handleB, -1, vrep.simx_opmode_oneshot_wait) return distance(poseA,poseB)
def fetchKinect(depthSTR,rgbSTR): errorCodeKinectRGB,kinectRGB=vrep.simxGetObjectHandle(clientID,rgbSTR,vrep.simx_opmode_oneshot_wait) errorCodeKinectDepth,kinectDepth=vrep.simxGetObjectHandle(clientID,depthSTR,vrep.simx_opmode_oneshot_wait) errorHere,resolution,image=vrep.simxGetVisionSensorImage(clientID,kinectRGB,0,vrep.simx_opmode_oneshot_wait) img,imgArr=Raftaar.ProcessImage(image,resolution) rgbArr=np.array(imgArr) errorHere,resol,depth=vrep.simxGetVisionSensorDepthBuffer(clientID,kinectDepth,vrep.simx_opmode_oneshot_wait) depthArr=np.array(depth) return depthArr,rgbArr
def setJointPositionNonBlock(self, clientID, joint, angle): handle = self.jointHandleMapping[joint] error = False if (handle == 0): error, handle=vrep.simxGetObjectHandle(clientID,joint,vrep.simx_opmode_oneshot) self.jointHandleMapping[joint]=handle vrep.simxSetJointPosition(clientID,handle,angle,vrep.simx_opmode_streaming)
def __init__( self, sim_dt=0.01, max_target_distance=3, noise=False, noise_std=[0,0,0,0,0,0], ): # Call the superclass of Quadcopter, which is Robot super(Quadcopter, self).__init__(sim_dt) err, self.copter = vrep.simxGetObjectHandle(self.cid, "Quadricopter_base", vrep.simx_opmode_oneshot_wait ) # Reset the motor commands to zero packedData=vrep.simxPackFloats([0,0,0,0]) raw_bytes = (ctypes.c_ubyte * len(packedData)).from_buffer_copy(packedData) err = vrep.simxSetStringSignal(self.cid, "rotorTargetVelocities", raw_bytes, vrep_mode) self.pos = [0,0,0] self.pos_err = [0,0,0] self.t_pos = [0,0,0] self.lin = [0,0,0] self.ori = [0,0,0] self.ori_err = [0,0,0] self.t_ori = [0,0,0] self.ang = [0,0,0] # Maximum target distance error that can be returned self.max_target_distance = max_target_distance # If noise is being modelled self.noise = noise # Standard Deviation of the noise for the 4 state variables self.noise_std = noise_std
def prox_sens_initialize(clientID): """ Initialize proximity sensor. Maybe helpful later """ proxSens=[] for i in range(8): _, oneProxSens = vrep.simxGetObjectHandle(clientID, 'ePuck_proxSensor%d' % (i+1), vrep.simx_opmode_streaming) proxSens.append(oneProxSens) return proxSens
def get_object_handle(object_name,all_switch): #if all_switch == 1: # res,objs = vrep.simxGetObjects(clientID,vrep.sim_handle_all,vrep.simx_opmode_oneshow_wait) #else: res,objs = vrep.simxGetObjectHandle(clientID,object_name,vrep.simx_opmode_oneshot_wait) return objs
def getPosition(clientID,goal_new): errorcode,newgoal_handle=vrep.simxGetObjectHandle(clientID,goal_new,vrep.simx_opmode_oneshot_wait) #time.sleep(1) errorCode,newgoal_position=vrep.simxGetObjectPosition(clientID,newgoal_handle,-1,vrep.simx_opmode_streaming) time.sleep(1) errorCode,newgoal_position=vrep.simxGetObjectPosition(clientID,newgoal_handle,-1,vrep.simx_opmode_buffer) return newgoal_position
def getVisionSensor(visionSensorName,clientID,sample_time): #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) time.sleep(0.5) res2,resolution,image=vrep.simxGetVisionSensorImage(clientID,visionSensorHandle,0,vrep.simx_opmode_buffer) print resolution img = I.new("RGB", (resolution[0], resolution[1])) while (vrep.simxGetConnectionId(clientID)!=-1): start = time.time() #Get the image of the vision sensor res,resolution,image = vrep.simxGetVisionSensorImage(clientID,visionSensorHandle,0,vrep.simx_opmode_buffer) image_byte_array = array.array('b',image) im = I.frombuffer("RGB", (resolution[0],resolution[1]), image_byte_array, "raw", "RGB", 0, 1) # #Set the image for naoqi # imagepixel = []#*(resolution[0]*resolution[1]) # for i in xrange(resolution[0] * resolution[1]): # r = i*3 # g = r+1 # b = r+2 # imagepixel.append((image[b], image[g], image[r])) # img.putdata(imagepixel) # print image # print imagepixel videoDeviceProxy.putImage(vision_definitions.kTopCamera, resolution[0], resolution[1], im.rotate(180).tostring()) end = time.time() dt = end-start # if dt < sample_time: # time.sleep(sample_time - dt) # else: # print "Can't keep a period (%gs>%gs)" % (dt,sample_time) print 'End of Simulation'
def setJointPosition(clientID, joint, angle): if (jointHandleMapping[joint] > 0): jhandle=jointHandleMapping[joint] else: error, jhandle=vrep.simxGetObjectHandle(clientID,joint,vrep.simx_opmode_oneshot_wait) jointHandleMapping[joint]=jhandle vrep.simxSetJointPosition(clientID,jhandle,angle,vrep.simx_opmode_oneshot_wait)
def configure_handles(self): # Handles of body and joints rc, body = vrep.simxGetObjectHandle(self.client_id, 'body', vrep.simx_opmode_oneshot_wait) assert rc == 0, rc rc, joint_0 = vrep.simxGetObjectHandle(self.client_id, 'joint_0', vrep.simx_opmode_oneshot_wait) assert rc == 0, rc rc, joint_1 = vrep.simxGetObjectHandle(self.client_id, 'joint_1', vrep.simx_opmode_oneshot_wait) assert rc == 0, rc self.body = body self.joints = [joint_0, joint_1] self.get_body_position(initial=True) self.get_joint_angles(initial=True) time.sleep(0.5)
def readDummyPath(self): fp = open(self.dummyPath, 'r') lines = fp.readlines() # 1行毎にファイル終端まで全て読む(改行文字も含まれる) fp.close() for line in lines: buf = line.split(",") for x in buf: params = x.split(":") if len(params) >= 2: name = params[0] try: portNb = 19998 # int(params[1]) self.dummyID = vrep.simxStart("127.0.0.1", portNb, True, True, 2000, 5) if self.dummyID == -1: print >> sys.stderr, "Fatal: No client ID while creating Dummy Communicator." else: self.setClientID(self.dummyID) except ValueError: print >> sys.stderr, "Fatal: non integer value while creating Dummy Communicator." time.sleep(1) exit() else: name = params[0] returnCode, handle = vrep.simxGetObjectHandle(self.dummyID, name, vrep.simx_opmode_oneshot_wait) if returnCode != vrep.simx_return_ok: print >> sys.stderr, "Fatal: Error obtaining a handle for " + name + "!" else: print name, handle item = VRepItem(name, self.dummyID, handle) self.addItem(item)
def streamVisionSensor(visionSensorName,clientID,pause=0.0001): #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) #Allow the display to be refreshed plt.ion() #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") #Give a title to the figure fig = plt.figure(1) fig.canvas.set_window_title(visionSensorName) #inverse the picture 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): #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 using pyplot image_byte_array = array.array('b',image) im = I.frombuffer("RGB", (resolution[0],resolution[1]), image_byte_array, "raw", "RGB", 0, 1) #Update the image plotimg.set_data(im) #Refresh the display plt.draw() #The mandatory pause ! (or it'll not work) plt.pause(pause) print 'End of Simulation'
def add_actuator(self, name, func, dim=1): if name is None: handle = None else: err, handle = vrep.simxGetObjectHandle(self.cid, name, vrep.simx_opmode_oneshot_wait ) self.actuators.append([handle, func, dim]) self.size_in += dim
def __init__(self, ip, port): """Initializes connectivity to V-REP environment, initializes environmental variables. """ vrep.simxFinish(-1) self.clientID = vrep.simxStart(ip, port, True, True, 5000, 5) if self.clientID != -1: print('Successfully connected to V-REP') else: raise RuntimeError('Could not connect to V-REP') # Some artificial delays to let V-REP stabilize after an action self.sleep_sec = config.SLEEP_VAL self.sleep_sec_min = config.SLEEP_VAL_MIN # id of the Robot Arm in V-REP environment self.main_object = 'uarm' # Initial state of the Gripper self.gripper_enabled = False # Get hands to various objects in the scene err, self.cylinder_handle = vrep.simxGetObjectHandle(self.clientID, 'uarm_pickupCylinder2', vrep.simx_opmode_blocking) if err != vrep.simx_return_ok: raise RuntimeError("Could not get handle to the cylinder object. Halting program.") err, self.bin_handle = vrep.simxGetObjectHandle(self.clientID, 'uarm_bin', vrep.simx_opmode_blocking) if err != vrep.simx_return_ok: raise RuntimeError("Could not get handle to the Bin object. Halting program.") err, self.gripper_handle = vrep.simxGetObjectHandle(self.clientID, 'uarmGripper_motor1Method2', vrep.simx_opmode_blocking) if err != vrep.simx_return_ok: raise RuntimeError("Could not get handle to the Gripper object. Halting program.") # Distance between cylinder and bin when former is inside later # This was measured after putting the cylinder in the bin self.cylinder_bin_distance = config.CYLINDER_BIN_DISTANCE # Various values, to be set in the start_sim() function, because their values can be obtained # only after the simulation starts self.cylinder_height = None self.cylinder_z_locus = None self.bin_position = None self.objects = None self.object_positions = None
def isRobotUp(clientID): error, LSP_Handle=vrep.simxGetObjectHandle(clientID,"Bioloid", vrep.simx_opmode_oneshot_wait) error, bioloid_position = vrep.simxGetObjectPosition(clientID, LSP_Handle, -1, vrep.simx_opmode_streaming) #in case of problems consulting the robot position, consider that the robot is up if error: return error, True else: return error, bioloid_position[2]>FALL_THRESHOLD
def getHandles(self): "Get handles for KJunior, motors, and sensors" ecodeE, self.KJuniorHandle = vrep.simxGetObjectHandle(self.simulID, "KJunior", vrep.simx_opmode_oneshot_wait) vrep.simxSynchronousTrigger(self.simulID) eCodeR, self.KJrightMotor = vrep.simxGetObjectHandle(self.simulID, "KJunior_motorRight#0", vrep.simx_opmode_oneshot_wait) vrep.simxSynchronousTrigger(self.simulID) eCodeL, self.KJleftMotor = vrep.simxGetObjectHandle(self.simulID, "KJunior_motorLeft#0", vrep.simx_opmode_oneshot_wait) vrep.simxSynchronousTrigger(self.simulID) eCodeR, self.KJrightEye = vrep.simxGetObjectHandle(self.simulID, "KJunior_proxSensor4#0", vrep.simx_opmode_oneshot_wait) vrep.simxSynchronousTrigger(self.simulID) eCodeL, self.KJleftEye = vrep.simxGetObjectHandle(self.simulID, "KJunior_proxSensor2#0", vrep.simx_opmode_oneshot_wait) vrep.simxSynchronousTrigger(self.simulID) eCodeL, self.KJcenterEye = vrep.simxGetObjectHandle(self.simulID, "KJunior_proxSensor3#0", vrep.simx_opmode_oneshot_wait) vrep.simxSynchronousTrigger(self.simulID) eCode,self.targetID = vrep.simxGetObjectHandle(self.simulID,"Sphere", vrep.simx_opmode_oneshot_wait) vrep.simxSynchronousTrigger(self.simulID) if (self.KJrightMotor == 0 or self.KJleftMotor == 0 or self.KJrightEye == 0 or self.KJleftEye == 0): self.cleanUp() sys.exit("Exiting: Could not connect to motors or sensors") else: print "Connected to Right Motor: %d, leftMotor: %d, Right eye: %d, Left eye: %d, with target ID:%d" % (self.KJrightMotor, self.KJleftMotor, self.KJrightEye, self.KJleftEye, self.targetID)
def run_benchmark_training(sim_number, sim_time): print("start running benchmark model") prefs.codegen.target = "numpy" start_scope() # number of sensor in robot N_sensor = 8 N_vision = 2 # robot radius r = 0.0586 # CD neurons number N_CD = 36 # vision detection_neurons number N_VD = 10 # HD neuron number N_HD = 72 # coordinate neuron N_x_axis, N_y_axis = 32, 32 N_PI = N_x_axis * N_y_axis # Simulation time # Speed cell number N_speed = 6 m = 5 # 60 iter = 0 # width and length of the arena x=width/ y=lenght # 1 square in vrep = 0.5*0.5 x_scale = 2 y_scale = 2 # distance unit per neuron N = 200 / (N_x_axis * N_y_axis) #define the initial mismatch encounter last_mismatch = 0 ##--------------------------------------------------------------------------------------------------------------------## ##--------------------Collision detection Neural Architecture--------------------------------------------## ##--------------------------------------------------------------------------------------------------------------------## Notred_red_inh, spikemon_red, spikemon_notred, spikemon_estimatedlandmark, spikemon_nonlandmark, wall_landmark_synapse, landmark_wall_synapse, Poisson_group, Poisson_vision, Collision_neuron, Collision_or_not, Color, Wall, Landmark, Poisson_synapse, PoissonVision_synapse, Self_ex_synapse, Self_ex_synapse_color, Self_in_synapse, Self_in_synapse_color, Collide_or_not_synapse, Red_landmark_ex, Red_wall_inh, Notred_landmark_inh, spikemon_CD, spikemon_collision, spikemon_landmark, spikemon_poisson, spikemon_wall, Poisson_non_collision, Non_collision_neuron, Poisson_non_synapse, CON_noncollision_synapse, Non_collision_color_synapses, spikemon_noncollison, Estimated_landmark_neuron, Non_landmark_neuron, Poisson_nonlandmark_synapse, Landmark_nonlandmark_synapse, CON_landmark_ex, CON_wall_ex, Notred_wall_ex, Red, Notred, color_notred_synapses, color_red_synapses = CD_net( N_CD, N_vision, N_VD) # gaussian input for 8 collison sensors stimuli = np.array([gaussian_spike(N_CD, j * N_CD / N_sensor, 10, 0.2) for j in range(N_sensor)]) # gaussion distribution for 2 vision input(wall or landmark) stimuli_vision = np.array([gaussian_spike(N_VD, j * N_VD / N_vision, 10, 0.2) for j in range(N_vision)]) ##--------------------------------------------------------------------------------------------------------------------## ##--------------------Head Direction and position Neural Architecture----------------------------------------------------------## ##--------------------------------------------------------------------------------------------------------------------## IHD_in_synapse, str_inh_synapse, Poisson_straight, Go_straight_neuron, Go_straight, HD_str_synapse, str_IHD_synapse, sti_PI, sti, Poisson_Left, Poisson_Right, Poisson_Compass, Left_drive, Right_drive, HD_Neuron, IHD_Neuron, Left_shift_neuron, Left_shift_speed_neuron, Right_shift_neuron, Right_shift_speed_neuron, HD_Left_synapse, Left_IHD_synapse, HD_Right_synapse, Left_drive_synapse, Right_drive_synapse, Right_IHD_synapse, HD_IHD_synapse, Reset_synapse, HD_ex_synapse, HD_in_synapse, spikemon_HD, spikemon_IHD, Poisson_PI, PI_Neurons, IPI_Neurons, Directional_Neurons, HD_directional_synapse, directional_PI_synapse, PI_shifting_neurons, North_shifting_neurons, South_shifting_neurons, East_shifting_neurons, West_shifting_neurons, PI_ex_synapse, PI_in_synapse, PI_N_synapse, PI_S_synapse, PI_E_synapse, PI_W_synapse, IPI_N_synapse, IPI_S_synapse, IPI_E_synapse, IPI_W_synapse, IPI_PI_synapse, PI_Reset_synapse, spikemon_PI, spikemon_IPI, NE_shifting_neurons, SE_shifting_neurons, WS_shifting_neurons, WN_shifting_neurons, PI_NE_synapse, PI_SE_synapse, PI_WS_synapse, PI_WN_synapse, IPI_NE_synapse, IPI_SE_synapse, IPI_WS_synapse, IPI_WN_synapse, Left_inh_synapse, Right_IHD_synapse, IPI_in_synapse, IPI_stay_synapse, Stay_stay_layer, Stay_layer, Stay, PI_stay_synapse = HD_PI_integrated_net( N_HD, N_speed, N_x_axis, N_y_axis, sim_time) ##-----------------head direction correcting matrix network-------------# # Poisson_IMU,IMU_Neuron,HD_errormatrix_neurons,HD_positive_error,HD_negative_error,IMU_poi_synapses,IMU_errors_connecting_synapses,HD_error_connect_synapses,Error_negative_synapses,Error_positive_synapses,Ex_speed_pool_Neurons,Inh_speed_pool_Neurons,Positive_ex_pool_synapses,Negative_inh_pool_synapses,statemon_positiveHD_error,statemon_negativeHD_error,spikemon_positiveHD_error,spikemon_negativeHD_error,spiketrain_pos=HD_error_correcting(N_HD,HD_Neuron) ##--------------------------------------------------------------------------------------------------------------------## ##--------------------Fusi Synapse between Position - Landmark Neuron----------------------------------------------------------## ##--------------------------------------------------------------------------------------------------------------------## '''measure collision weight by fusi synapse landmark''' landmark_PI_plastic = fusi_landmark(PI_Neurons, Landmark) w_landmark_plas_shape = np.shape(landmark_PI_plastic.w_fusi_landmark)[0] w_plastic_landmark = landmark_PI_plastic.w_fusi_landmark all_fusi_weights_landmark = landmark_PI_plastic.w_fusi_landmark ##--------------------------------------------------------------------------------------------------------------------## ##--------------------Fusi Synapse between Position - Wall Neuron----------------------------------------------------------## ##--------------------------------------------------------------------------------------------------------------------## '''measure collision weight by fusi synapse landmark''' wall_PI_plastic = fusi_wall(PI_Neurons, Wall) w_plas_shape_wall = np.shape(wall_PI_plastic.w_fusi_wall)[0] w_plastic_wall = wall_PI_plastic.w_fusi_wall all_fusi_weights_wall = wall_PI_plastic.w_fusi_wall ##--------------------------------------------------------------------------------------------------------------------## ##-------------------------------------------Mismatch Network--------------------------------------------------------------## ##--------------------------------------------------------------------------------------------------------------------## '''store previously estimated landmark position by fusi synapse estimatedlandmark''' preeq = ''' v_post += 1*(w_piest >= 0.5) ''' PI_Neurons_est_synapse = Synapses(PI_Neurons, Estimated_landmark_neuron, 'w_piest : 1', on_pre=preeq) PI_Neurons_est_synapse.connect() Mismatch_landmark_inh_synpase, landmark_mismatch_inh_synapse, spikemon_nonestimatedlandmark, NonEst_landmark_poisson, Non_estimatedlandmark_neuron, Mismatch_neuron, Non_Mismatch_neuron, Nonestimatedlandmark_poi_synapse, Est_nonest_inh_synapse, NonCol_mismatch_synapse, NonCol_nonmismatch_synapse, NonEst_mistmatch_inh_synapse, Est_mismatch_ex_synapse, Est_nonmismatch_inh_synapses, Mismatch_est_inh_synpase, spikemon_Mismatch = mismatch_net( Non_landmark_neuron, Estimated_landmark_neuron, Landmark) ##--------------------------------------------------------------------------------------------------------------------## ##----------------------------------------------Network--------------------------------------------------------------## ##--------------------------------------------------------------------------------------------------------------------## print("adding network") # Network PINet = Network() # add collision network PINet.add(Notred_red_inh, spikemon_red, spikemon_notred, spikemon_estimatedlandmark, spikemon_nonlandmark, wall_landmark_synapse, landmark_wall_synapse, Poisson_group, Poisson_vision, Collision_neuron, Collision_or_not, Color, Wall, Landmark, Poisson_synapse, PoissonVision_synapse, Self_ex_synapse, Self_ex_synapse_color, Self_in_synapse, Self_in_synapse_color, Collide_or_not_synapse, Red_landmark_ex, Red_wall_inh, Notred_landmark_inh, spikemon_CD, spikemon_collision, spikemon_landmark, spikemon_poisson, spikemon_wall, Poisson_non_collision, Non_collision_neuron, Poisson_non_synapse, CON_noncollision_synapse, Non_collision_color_synapses, spikemon_noncollison, Estimated_landmark_neuron, Non_landmark_neuron, Poisson_nonlandmark_synapse, Landmark_nonlandmark_synapse, CON_landmark_ex, CON_wall_ex, Notred_wall_ex, Red, Notred, color_notred_synapses, color_red_synapses ) # add position network PINet.add(IHD_in_synapse, str_inh_synapse, Poisson_straight, Go_straight_neuron, Go_straight, HD_str_synapse, str_IHD_synapse, Poisson_Left, Poisson_Right, Poisson_Compass, Left_drive, Right_drive, HD_Neuron, IHD_Neuron, Left_shift_neuron, Left_shift_speed_neuron, Right_shift_neuron, Right_shift_speed_neuron, HD_Left_synapse, Left_IHD_synapse, HD_Right_synapse, Left_drive_synapse, Right_drive_synapse, Right_IHD_synapse, HD_IHD_synapse, Reset_synapse, HD_ex_synapse, HD_in_synapse, spikemon_HD, spikemon_IHD, Poisson_PI, PI_Neurons, IPI_Neurons, Directional_Neurons, HD_directional_synapse, directional_PI_synapse, PI_shifting_neurons, North_shifting_neurons, South_shifting_neurons, East_shifting_neurons, West_shifting_neurons, PI_ex_synapse, PI_in_synapse, PI_N_synapse, PI_S_synapse, PI_E_synapse, PI_W_synapse, IPI_N_synapse, IPI_S_synapse, IPI_E_synapse, IPI_W_synapse, IPI_PI_synapse, PI_Reset_synapse, spikemon_PI, spikemon_IPI, NE_shifting_neurons, SE_shifting_neurons, WS_shifting_neurons, WN_shifting_neurons, PI_NE_synapse, PI_SE_synapse, PI_WS_synapse, PI_WN_synapse, IPI_NE_synapse, IPI_SE_synapse, IPI_WS_synapse, IPI_WN_synapse, Left_inh_synapse, Right_IHD_synapse, IPI_in_synapse, IPI_stay_synapse, Stay_stay_layer, Stay_layer, Stay, PI_stay_synapse) # add fusi plastic synapses for landmark,wall neurons with PI neurons PINet.add(landmark_PI_plastic) PINet.add(wall_PI_plastic) # add mismatch network PINet.add(PI_Neurons_est_synapse, Mismatch_landmark_inh_synpase, landmark_mismatch_inh_synapse, spikemon_nonestimatedlandmark, NonEst_landmark_poisson, Non_estimatedlandmark_neuron, Mismatch_neuron, Non_Mismatch_neuron, Nonestimatedlandmark_poi_synapse, Est_nonest_inh_synapse, NonCol_mismatch_synapse, NonCol_nonmismatch_synapse, NonEst_mistmatch_inh_synapse, Est_mismatch_ex_synapse, Est_nonmismatch_inh_synapses, Mismatch_est_inh_synpase, spikemon_Mismatch) ##-----------------------------------------------------------------------------------------## ##---------------------------------Robot controller----------------------------------------## ##-----------------------------------------------------------------------------------------## # Connect to the server print('finished adding network') vrep.simxFinish(-1) # just in case, close all opened connections clientID = vrep.simxStart('127.0.0.1', 19999, True, True, 5000, 5) if clientID != -1: print("Connected to remote API server") else: print("Not connected to remote API server") sys.exit("Could not connect") vrep.simxSynchronous(clientID, 1) ##----------------------------------Controller initialized------------------------------------------## # set motor err_code, l_motor_handle = vrep.simxGetObjectHandle(clientID, "KJunior_motorLeft", vrep.simx_opmode_blocking) err_code, r_motor_handle = vrep.simxGetObjectHandle(clientID, "KJunior_motorRight", vrep.simx_opmode_blocking) # Compass output=orientation # define robot err_code, robot = vrep.simxGetObjectHandle(clientID, 'KJunior', vrep.simx_opmode_blocking) # define Angles err_code, Angles = vrep.simxGetObjectOrientation(clientID, robot, -1, vrep.simx_opmode_streaming) # define object position err_code, Position = vrep.simxGetObjectPosition(clientID, robot, -1, vrep.simx_opmode_streaming) # get sensor sensor1, sensor2, sensor3, sensor4, sensor5, sensor6, sensor7, sensor8 = getSensor(clientID) # read point detectedPoint1, detectedPoint2, detectedPoint3, detectedPoint4, detectedPoint5, detectedPoint6, detectedPoint7, detectedPoint8 = getDetectedpoint( sensor1, sensor2, sensor3, sensor4, sensor5, sensor6, sensor7, sensor8, clientID) # Distance from sensor to obstacle sensor_val1, sensor_val2, sensor_val3, sensor_val4, sensor_val5, sensor_val6, sensor_val7, sensor_val8 = getSensorDistance( detectedPoint1, detectedPoint2, detectedPoint3, detectedPoint4, detectedPoint5, detectedPoint6, detectedPoint7, detectedPoint8) # get sensor for mismatch landmark detection(get sensor point and distance) err_code, sensorMismatch = vrep.simxGetObjectHandle(clientID, "Proximity_sensor", vrep.simx_opmode_blocking) err_code,detectionStateMismatch,detectedPointMismatch,detectedObjectHandleMismatch,detectedSurfaceNormalVectorMismatch=vrep.simxReadProximitySensor(clientID,sensorMismatch,vrep.simx_opmode_streaming) mismatch_sensordistance = np.linalg.norm(detectedPointMismatch) # get vision sensor err_code, camera = vrep.simxGetObjectHandle(clientID, "Vision_sensor", vrep.simx_opmode_blocking) err_code, resolution, image = vrep.simxGetVisionSensorImage(clientID, camera, 1, vrep.simx_opmode_streaming) ##---------------------------------Sensor initial condition (for CD)------------------------------------## # sensor value of every sensor for every neuron # Use inv_filter to filter out noise # sensor_val = np.array([np.repeat(inv_filter(sensor_val1),N_CD), np.repeat(inv_filter(sensor_val2),N_CD),np.repeat(inv_filter(sensor_val3),N_CD),np.repeat(inv_filter(sensor_val7),N_CD),np.repeat(inv_filter(sensor_val8),N_CD)]) sensor_val = np.array([np.repeat(inv_filter(sensor_val1), N_CD), np.repeat(inv_filter(sensor_val2), N_CD), np.repeat(inv_filter(sensor_val3), N_CD), np.repeat(inv_filter(sensor_val4), N_CD), np.repeat(inv_filter(sensor_val5), N_CD), np.repeat(inv_filter(sensor_val6), N_CD), np.repeat(inv_filter(sensor_val7), N_CD), np.repeat(inv_filter(sensor_val8), N_CD)]) sensor_val_vision = np.zeros([N_vision, N_VD]) # sum of each sensor value * its gaussian distribution --> sum to find all activity for each neurons --> WTA All_stimuli = np.sum(sensor_val * stimuli, axis=0) All_stimuli_vision = np.sum(sensor_val_vision * stimuli_vision, axis=0) # find the winner winner = WTA(All_stimuli) for w in winner: Poisson_synapse.w_poi[w] = All_stimuli[w] winner_vision = WTA(All_stimuli_vision) for w in winner_vision: PoissonVision_synapse.w_vision_poi[w] = All_stimuli_vision[w] ##------------------------------------------------------------------------------------------------## # initial speed of wheel l_steer = 0.24 r_steer = 0.24 # record the initial time t_int = time.time() # record compass angle compass_angle = np.array([]) # record x and y axis at each time x_axis = np.array([]) y_axis = np.array([]) # path that is passing r = np.array([]) # all_time all_time = np.array([]) # collision index that the robot collided collision_index = np.array([]) collision_index_during_run = [] # record weight parameter # record every m sec weight_record_time = 1 weight_during_run_landmark = np.zeros([np.shape(w_plastic_landmark)[0], 3 * int(sim_time / 5)]) weight_during_run_wall = np.zeros([np.shape(w_plastic_wall)[0], 3 * int(sim_time / 5)]) time_step_list = np.array([]) # start simulation while (time.time() - t_int) < sim_time: t1 = time.time() - t_int # record proximity sensor at each time step sensor_val1, sensor_val2, sensor_val3, sensor_val4, sensor_val5, sensor_val6, sensor_val7, sensor_val8 = getSensorDistance( detectedPoint1, detectedPoint2, detectedPoint3, detectedPoint4, detectedPoint5, detectedPoint6, detectedPoint7, detectedPoint8) all_sensor = np.array([sensor_val1, sensor_val2, sensor_val3, sensor_val4, sensor_val5]) all_sensor[all_sensor < 4.1e-20] = np.infty activated_sensor = np.argmin(all_sensor) # obtain vision sensor values pixelimage = set(image) if all_sensor[activated_sensor] < 0.1: if activated_sensor == 3: r_steer, l_steer, zeta = TurnRight(r_steer, l_steer, delta_t) elif activated_sensor == 4: r_steer, l_steer, zeta = TurnRight(r_steer, l_steer, delta_t) elif activated_sensor == 0: r_steer, l_steer, zeta = TurnLeft(r_steer, l_steer, delta_t) elif activated_sensor == 1: r_steer, l_steer, zeta = TurnLeft(r_steer, l_steer, delta_t) elif activated_sensor == 2: r_steer, l_steer, zeta = TurnLeft(r_steer, l_steer, delta_t) else: l_steer = 0.24 r_steer = 0.24 zeta = 0 else: l_steer = 0.24 r_steer = 0.24 zeta = 0 ####-------------------- Record weight------------------------#### weight_during_run_landmark[:, iter] = w_plastic_landmark weight_during_run_wall[:, iter] = w_plastic_wall collision_index_during_run.append(collision_index) ####-------------------- Start recording spike (CD) ------------------------#### # cleaning noises for the collision distance sensor value sensor_val = np.array([np.repeat(inv_filter(sensor_val1), N_CD), np.repeat(inv_filter(sensor_val2), N_CD), np.repeat(inv_filter(sensor_val3), N_CD), np.repeat(inv_filter(sensor_val4), N_CD), np.repeat(inv_filter(sensor_val5), N_CD), np.repeat(inv_filter(sensor_val6), N_CD), np.repeat(inv_filter(sensor_val7), N_CD), np.repeat(inv_filter(sensor_val8), N_CD)]) # get the vision sensor value sensor_val_vision = generate_vision_val(pixelimage, N_VD) # reset weight to 0 again Poisson_synapse.w_poi = 0 PoissonVision_synapse.w_vision_poi = 0 err_code = vrep.simxSetJointTargetVelocity(clientID, l_motor_handle, l_steer, vrep.simx_opmode_streaming) err_code = vrep.simxSetJointTargetVelocity(clientID, r_motor_handle, r_steer, vrep.simx_opmode_streaming) # All stimuli and WTA All_stimuli = (np.sum(sensor_val * stimuli, axis=0)) winner = WTA(All_stimuli) for w in winner: Poisson_synapse.w_poi[w] = All_stimuli[w] All_stimuli_vision = np.sum(sensor_val_vision * stimuli_vision, axis=0) winner_vision = WTA(All_stimuli_vision) for w in winner_vision: PoissonVision_synapse.w_vision_poi[w] = All_stimuli_vision[w] / 10 # --------mismatching detecting---------## PI_Neurons_est_synapse.w_piest = landmark_PI_plastic.w_fusi_landmark ####-------------------- End recording spike ----------------------------#### ####-------------------- Start recording Head direction ------------------------#### # Choose neuron that is the nearest to the turning speed/direction # if turn left if r_steer == 0: neuron_index = nearest_neuron_speed(zeta, N_speed) for syn in range(N_speed): Left_drive_synapse[syn].w_left_drive = 0 Right_drive_synapse[syn].w_right_drive = 0 Left_drive_synapse[neuron_index].w_left_drive = 5 Right_drive_synapse[neuron_index].w_right_drive = 0 Go_straight.w_str = -3 directional_PI_synapse.w_dir_PI = 0 Stay_stay_layer.w_stay_stay = 2 # if turn right elif l_steer == 0: neuron_index = nearest_neuron_speed(zeta, N_speed) for syn in range(N_speed): Left_drive_synapse[syn].w_left_drive = 0 Right_drive_synapse[syn].w_right_drive = 0 Left_drive_synapse[neuron_index].w_left_drive = 0 Right_drive_synapse[neuron_index].w_right_drive = 5 Go_straight.w_str = -3 directional_PI_synapse.w_dir_PI = 0 # if turn position PI stay Stay_stay_layer.w_stay_stay = 2 # if go straight else: for syn in range(N_speed): Left_drive_synapse[syn].w_left_drive = 0 Right_drive_synapse[syn].w_right_drive = 0 Go_straight.w_str = 10 directional_PI_synapse.w_dir_PI = 4 # if move position PI run Stay_stay_layer.w_stay_stay = -4 ##-----------------reset IHD (Compass) -----------------## # Get actual heading direction err_code, Angles = vrep.simxGetObjectOrientation(clientID, robot, -1, vrep.simx_opmode_streaming) heading_dir = getHeadingdirection(Angles) # print("IMU head direction is", heading_dir, "angle from IMU is", Angles) compass_angle = np.append(compass_angle, heading_dir) # recalibrate head direction to nearest neuron recal = nearest_neuron_head(heading_dir, N_HD) # set reset by compass weight upon angle atm Reset_synapse.w_reset = np.array(gaussian_spike(N_HD, recal, 30, 0.03)) # print("reset synapse",Reset_synapse.w_reset) ##----------------Active searching for landmark when encountering mismatch -----------------## if len(spikemon_Mismatch) > last_mismatch: print('encounter mismatch') err_code, detectionStateMismatch, detectedPointMismatch, detectedObjectHandleMismatch, detectedSurfaceNormalVectorMismatch = vrep.simxReadProximitySensor( clientID, sensorMismatch, vrep.simx_opmode_streaming) mismatch_sensordistance = np.linalg.norm(detectedPointMismatch) while mismatch_sensordistance <= 0.01: l_steer = 0 r_steer = 0.5 err_code = vrep.simxSetJointTargetVelocity(clientID, l_motor_handle, l_steer,vrep.simx_opmode_streaming) err_code = vrep.simxSetJointTargetVelocity(clientID, r_motor_handle, r_steer,vrep.simx_opmode_streaming) err_code, detectionStateMismatch, detectedPointMismatch, detectedObjectHandleMismatch, detectedSurfaceNormalVectorMismatch = vrep.simxReadProximitySensor( clientID, sensorMismatch, vrep.simx_opmode_streaming) mismatch_sensordistance = np.linalg.norm(detectedPointMismatch) current_error_distance = mismatch_sensordistance l_steer = 0.24 r_steer = 0.24 err_code = vrep.simxSetJointTargetVelocity(clientID, l_motor_handle, l_steer, vrep.simx_opmode_streaming) err_code = vrep.simxSetJointTargetVelocity(clientID, r_motor_handle, r_steer, vrep.simx_opmode_streaming) last_mismatch = len(spikemon_Mismatch) ##----------------- Read position -----------------## err_code, Position = vrep.simxGetObjectPosition(clientID, robot, -1, vrep.simx_opmode_streaming) # print("position value is",Position) x_pos = Position[0] y_pos = Position[1] # recalibrate head direction to nearest neuron x_axis = np.append(x_axis, x_pos) y_axis = np.append(y_axis, y_pos) # recalibrate recal_x_axis = nearest_neuron_x_axis(x_pos, N_x_axis, x_scale) recal_y_axis = nearest_neuron_y_axis(y_pos, N_y_axis, y_scale) recal_index = N_x_axis * recal_y_axis + recal_x_axis r = np.append(r, recal_index) # is an array keeping all index that neuron fire during the run ##----------------- reset position neuron with IMU recalibration ---------------------## '''coment this line to disable IMU''' #PI_Reset_synapse.w_poi_PI = np.array(gaussian_spike(N_PI, recal_index, 20, 0.01)) ##----------------- Recording Position Index when collision -----------------## if l_steer == 0 or r_steer == 0: collision_index = np.append(collision_index, recal_index) ##----------------- Collect time-----------------## all_time = np.append(all_time, time.time() - t_int) # run PINet.run(15 * ms) print("current plas landmark synapese index", np.where(w_plastic_landmark >= 0.5)) print('wall synpases values index bigger', np.where(w_plastic_wall >= 0.5)) print('count mismatch', len(spikemon_Mismatch)) print('count landmark', len(spikemon_landmark)) print('####-------------------- Read sensor (new round) ----------------------------####') # Start new measurement in the next time step detectedPoint1, detectedPoint2, detectedPoint3, detectedPoint4, detectedPoint5, detectedPoint6, detectedPoint7, detectedPoint8 = getDetectedpoint( sensor1, sensor2, sensor3, sensor4, sensor5, sensor6, sensor7, sensor8, clientID) err_code, detectionStateMismatch, detectedPointMismatch, detectedObjectHandleMismatch, detectedSurfaceNormalVectorMismatch = vrep.simxReadProximitySensor( clientID, sensorMismatch, vrep.simx_opmode_streaming) err_code, resolution, image = vrep.simxGetVisionSensorImage(clientID, camera, 1, vrep.simx_opmode_streaming) # record time step and final time t2 = time.time() - t_int delta_t = t2 - t1 final_time = time.time() time_step_list = np.append(time_step_list, delta_t) print("delta-t", delta_t,iter) print("final time t2", int(t2)) iter += 1 ##---------------end of simulation ---------------## print("Done") # saving data # setting pathway pathway = "/Users/jieyab/SNN/for_plotting/" exp_number = str(sim_number) np.save(pathway + "r" + exp_number, r) np.save(pathway + "spikemon_CD_i" + exp_number, spikemon_CD.i) np.save(pathway + "spikemon_CD_t" + exp_number, spikemon_CD.t / ms) np.save(pathway + "spikemon_HD_i" + exp_number, spikemon_HD.i) np.save(pathway + "spikemon_HD_t" + exp_number, spikemon_HD.t / ms) np.save(pathway + "spikemon_PI_i" + exp_number, spikemon_PI.i) np.save(pathway + "spikemon_PI_t" + exp_number, spikemon_PI.t / ms) np.save(pathway + "spikemon_noncollision_i" + exp_number, spikemon_noncollison.i) np.save(pathway + "spikemon_noncollision_t" + exp_number, spikemon_noncollison.t / ms) np.save(pathway + "spikemon_wall_i" + exp_number, spikemon_wall.i) np.save(pathway + "spikemon_wall_t" + exp_number, spikemon_wall.t / ms) np.save(pathway + "spikemon_nonlandmark_i" + exp_number, spikemon_nonlandmark.i) np.save(pathway + "spikemon_nonlandmark_t" + exp_number, spikemon_nonlandmark.t / ms) np.save(pathway + "spikemon_landmark_i" + exp_number, spikemon_landmark.i) np.save(pathway + "spikemon_landmark_t" + exp_number, spikemon_landmark.t / ms) np.save(pathway + "weight_during_run_landmark" + exp_number, weight_during_run_landmark) np.save(pathway + "weight_during_run_wall" + exp_number, weight_during_run_wall) np.save(pathway + "weight_landmark" + exp_number, w_plastic_landmark) np.save(pathway + "weight__wall" + exp_number, w_plastic_wall) np.save(pathway + "spikemon_mismatch_t" + exp_number, spikemon_Mismatch.t / ms) np.save(pathway + "spikemon_mismatch_v" + exp_number, spikemon_Mismatch.v) np.save(pathway + "collision_index" + exp_number, collision_index) np.save(pathway + "collision_index_during_run" + exp_number, collision_index_during_run) np.save(pathway + "compass_angle" + exp_number, compass_angle) np.save(pathway + "all_time" + exp_number, all_time) np.save(pathway + "spikemon_estimatedlandmark_i" + exp_number, spikemon_estimatedlandmark.i) np.save(pathway + "spikemon_estimatedlandmark_t" + exp_number, spikemon_estimatedlandmark.t / ms) np.save(pathway + "spikemon_nonestimatedlandmark_i" + exp_number, spikemon_nonestimatedlandmark.i) np.save(pathway + "spikemon_nonestimatedlandmark_t" + exp_number, spikemon_nonestimatedlandmark.t / ms) np.save(pathway + "spikemon_red_i" + exp_number, spikemon_red.i) np.save(pathway + "spikemon_red_t" + exp_number, spikemon_red.t / ms) np.save(pathway + "spikemon_notred_i" + exp_number, spikemon_notred.i) np.save(pathway + "spikemon_notred_t" + exp_number, spikemon_notred.t / ms) np.save(pathway + "step_time" + exp_number, time_step_list) return clientID
def main(): # Start V-REP connection try: vrep.simxFinish(-1) print("Connecting to simulator...") clientID = vrep.simxStart('127.0.0.1', 19999, True, True, 5000, 5) if clientID == -1: print("Failed to connect to remote API Server") vrep_exit(clientID) except KeyboardInterrupt: vrep_exit(clientID) return # Setup V-REP simulation print("Setting simulator to async mode...") vrep.simxSynchronous(clientID, True) dt = .001 vrep.simxSetFloatingParameter(clientID, vrep.sim_floatparam_simulation_time_step, dt, # specify a simulation time step vrep.simx_opmode_oneshot) # Load V-REP scene print("Loading scene...") vrep.simxLoadScene(clientID, scene_name, 0xFF, vrep.simx_opmode_blocking) # Get quadrotor handle err, quad_handle = vrep.simxGetObjectHandle(clientID, quad_name, vrep.simx_opmode_blocking) print(err,quad_handle) # Initialize quadrotor position and orientation vrep.simxGetObjectPosition(clientID, quad_handle, -1, vrep.simx_opmode_streaming) vrep.simxGetObjectOrientation(clientID, quad_handle, -1, vrep.simx_opmode_streaming) vrep.simxGetObjectVelocity(clientID, quad_handle, vrep.simx_opmode_streaming) # Start simulation vrep.simxStartSimulation(clientID, vrep.simx_opmode_blocking) # Initialize rotors print("Initializing propellers...") for i in range(len(propellers)): vrep.simxClearFloatSignal(clientID, propellers[i], vrep.simx_opmode_oneshot) # Get quadrotor initial position and orientation err, pos_start = vrep.simxGetObjectPosition(clientID, quad_handle, -1, vrep.simx_opmode_buffer) err, euler_start = vrep.simxGetObjectOrientation(clientID, quad_handle, -1, vrep.simx_opmode_buffer) err, vel_start, angvel_start = vrep.simxGetObjectVelocity(clientID, quad_handle, vrep.simx_opmode_buffer) pos_start = np.asarray(pos_start) euler_start = np.asarray(euler_start)*10. vel_start = np.asarray(vel_start) angvel_start = np.asarray(angvel_start) pos_old = pos_start euler_old = euler_start vel_old = vel_start angvel_old = angvel_start pos_new = pos_old euler_new = euler_old vel_new = vel_old angvel_new = angvel_old pos_error = (pos_start + setpoint_delta[0:3]) - pos_new euler_error = (euler_start + setpoint_delta[3:6]) - euler_new vel_error = (vel_start + setpoint_delta[6:9]) - vel_new angvel_error = (angvel_start + setpoint_delta[9:12]) - angvel_new pos_error_all = pos_error euler_error_all = euler_error n_input = 6 n_forces=4 init_f=7. state = [0 for i in range(n_input)] state = torch.from_numpy(np.asarray(state, dtype=np.float32)).view(-1, n_input) propeller_vels = [init_f, init_f, init_f, init_f] delta_forces = [0., 0., 0., 0.] extended_state=torch.cat((state,torch.from_numpy(np.asarray([propeller_vels], dtype=np.float32))),1) memory = ReplayMemory(ROLLOUT_LEN) test_num = 1 timestep = 1 while (vrep.simxGetConnectionId(clientID) != -1): # Set propeller thrusts print("Setting propeller thrusts...") # Only PD control bc can't find api function for getting simulation time propeller_vels = pid(pos_error,euler_new,euler_error[2],vel_error,angvel_error) #propeller_vels = apply_forces(propeller_vels, delta_forces) # for dqn # Send propeller thrusts print("Sending propeller thrusts...") [vrep.simxSetFloatSignal(clientID, prop, vels, vrep.simx_opmode_oneshot) for prop, vels in zip(propellers, propeller_vels)] # Trigger simulator step print("Stepping simulator...") vrep.simxSynchronousTrigger(clientID) # Get quadrotor initial position and orientation err, pos_new = vrep.simxGetObjectPosition(clientID, quad_handle, -1, vrep.simx_opmode_buffer) err, euler_new = vrep.simxGetObjectOrientation(clientID, quad_handle, -1, vrep.simx_opmode_buffer) err, vel_new, angvel_new = vrep.simxGetObjectVelocity(clientID, quad_handle, vrep.simx_opmode_buffer) pos_new = np.asarray(pos_new) euler_new = np.asarray(euler_new)*10 vel_new = np.asarray(vel_new) angvel_new = np.asarray(angvel_new) #euler_new[2]/=100 valid = is_valid_state(pos_start, pos_new, euler_new) if valid: next_state = torch.FloatTensor(np.concatenate([euler_new, pos_new - pos_old])) #next_state = torch.FloatTensor(euler_new ) next_extended_state=torch.FloatTensor([np.concatenate([next_state,np.asarray(propeller_vels)])]) else: next_state = None next_extended_state = None reward=np.float32(0) memory.push(extended_state, torch.from_numpy(np.asarray([delta_forces],dtype=np.float32)), next_extended_state, torch.from_numpy(np.asarray([[reward]]))) state = next_state extended_state=next_extended_state pos_old = pos_new euler_old = euler_new vel_old = vel_new angvel_old = angvel_new print("Propeller Velocities:") print(propeller_vels) print("\n") pos_error = (pos_start + setpoint_delta[0:3]) - pos_new euler_error = (euler_start + setpoint_delta[3:6]) - euler_new euler_error %= 2*np.pi for i in range(len(euler_error)): if euler_error[i] > np.pi: euler_error[i] -= 2*np.pi vel_error = (vel_start + setpoint_delta[6:9]) - vel_new angvel_error = (angvel_start + setpoint_delta[9:12]) - angvel_new pos_error_all = np.vstack([pos_error_all,pos_error]) euler_error_all = np.vstack([euler_error_all,euler_error]) print("Errors (pos,ang):") print(pos_error,euler_error) print("\n") timestep += 1 if not valid or timestep > ROLLOUT_LEN: np.savetxt('csv/pos_error{0}.csv'.format(test_num), pos_error_all, delimiter=',', fmt='%8.4f') np.savetxt('csv/euler_error{0}.csv'.format(test_num), euler_error_all, delimiter=',', fmt='%8.4f') print('RESET') # reset vrep.simxStopSimulation(clientID, vrep.simx_opmode_oneshot_wait) time.sleep(0.1) # Setup V-REP simulation print("Setting simulator to async mode...") vrep.simxSynchronous(clientID, True) dt = .001 vrep.simxSetFloatingParameter(clientID, vrep.sim_floatparam_simulation_time_step, dt, # specify a simulation time step vrep.simx_opmode_oneshot) print("Loading scene...") vrep.simxLoadScene(clientID, scene_name, 0xFF, vrep.simx_opmode_blocking) # Get quadrotor handle err, quad_handle = vrep.simxGetObjectHandle(clientID, quad_name, vrep.simx_opmode_blocking) # Initialize quadrotor position and orientation vrep.simxGetObjectPosition(clientID, quad_handle, -1, vrep.simx_opmode_streaming) vrep.simxGetObjectOrientation(clientID, quad_handle, -1, vrep.simx_opmode_streaming) vrep.simxGetObjectVelocity(clientID, quad_handle, vrep.simx_opmode_streaming) # Start simulation vrep.simxStartSimulation(clientID, vrep.simx_opmode_blocking) for i in range(len(propellers)): vrep.simxClearFloatSignal(clientID, propellers[i], vrep.simx_opmode_oneshot) err, pos_start = vrep.simxGetObjectPosition(clientID, quad_handle, -1, vrep.simx_opmode_buffer) err, euler_start = vrep.simxGetObjectOrientation(clientID, quad_handle, -1, vrep.simx_opmode_buffer) err, vel_start, angvel_start = vrep.simxGetObjectVelocity(clientID, quad_handle, vrep.simx_opmode_buffer) pos_start = np.asarray(pos_start) euler_start = np.asarray(euler_start)*10. vel_start = np.asarray(vel_start) angvel_start = np.asarray(angvel_start)*10. pos_old = pos_start euler_old = euler_start vel_old = vel_start angvel_old = angvel_start pos_new = pos_old euler_new = euler_old vel_new = vel_old angvel_new = angvel_old pos_error = (pos_start + setpoint_delta[0:3]) - pos_new euler_error = (euler_start + setpoint_delta[3:6]) - euler_new vel_error = (vel_start + setpoint_delta[6:9]) - vel_new angvel_error = (angvel_start + setpoint_delta[9:12]) - angvel_new pos_error_all = np.vstack([pos_error_all,pos_error]) euler_error_all = np.vstack([euler_error_all,euler_error]) state = [0 for i in range(n_input)] state = torch.FloatTensor(np.asarray(state)).view(-1, n_input) propeller_vels = [init_f, init_f, init_f, init_f] extended_state = torch.cat((state, torch.FloatTensor(np.asarray([propeller_vels]))), 1) print('duration: ',len(memory)) memory = ReplayMemory(ROLLOUT_LEN) test_num += 1 timestep = 1
def generate(save_path, number_episodes, steps_per_episode, jointvel_factor=0.5): # Initialize connection connection = VrepConnection() connection.synchronous_mode() connection.start() # 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, distance_handle = vrep.simxGetDistanceHandle( clientID, "tipToTarget", vrep.simx_opmode_blocking) err, distance_to_target = vrep.simxReadDistance(clientID, distance_handle, vrep.simx_opmode_streaming) # Initialize data file f = h5py.File(save_path, "w") episode_count = 0 total_datapoints = number_episodes * steps_per_episode size_of_image = resolution[0] * resolution[1] * 3 dset1 = f.create_dataset("images", (total_datapoints, size_of_image), dtype="uint") dset2 = f.create_dataset("joint_pos", (total_datapoints, 6), dtype="float") dset3 = f.create_dataset("joint_vel", (total_datapoints, 6), dtype="float") dset4 = f.create_dataset("distance", (total_datapoints, 1), dtype="float") # 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) for i in range(total_datapoints): # At start of each episode, check if path has been found in vrep if (i % steps_per_episode) == 0: 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) episode_count += 1 print "Episode: ", episode_count # obtain image and place into array err, resolution, image = vrep.simxGetVisionSensorImage( clientID, v1, 0, vrep.simx_opmode_buffer) img = np.array(image, dtype=np.uint8) dset1[i] = img # obtain joint pos and place into array jointpos = np.zeros(6) for j in range(6): err, jp = vrep.simxGetJointPosition(clientID, jhList[j], vrep.simx_opmode_buffer) jointpos[j] = jp dset2[i] = jointpos # obtain distance and place into array distance = np.zeros(1) err, distance_to_target = vrep.simxReadDistance( clientID, distance_handle, vrep.simx_opmode_buffer) distance[0] = distance_to_target dset4[i] = distance # 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) # calculate joint velocities excluding final image for k in range(number_episodes): for i in range(steps_per_episode - 1): #jointvel = dset2[k*steps_per_episode + i+1]-dset2[k*steps_per_episode + i] jointvel = joint_difference(dset2[k * steps_per_episode + i], dset2[k * steps_per_episode + i + 1]) abs_sum = np.sum(np.absolute(jointvel)) if abs_sum == 0: dset3[k * steps_per_episode + i] = np.zeros(6) else: # abs sum norm dset3[k * steps_per_episode + i] = jointvel / abs_sum * jointvel_factor * dset4[ k * steps_per_episode + i] # close datafile f.close() return
clientID = vrep.simxStart( '127.0.0.1', 19997, True, True, -500000, 5) # Connect to V-REP, set a very large time-out for blocking commands if clientID != -1: print('Connected to remote API server') emptyBuff = bytearray() # Start the simulation: vrep.simxStartSimulation(clientID, vrep.simx_opmode_oneshot_wait) # Load a robot instance: res,retInts,retFloats,retStrings,retBuffer=vrep.simxCallScriptFunction(clientID,'remoteApiCommandServer',vrep.sim_scripttype_childscript,'loadRobot',[],[0,0,0,0],['d:/v_rep/qrelease/release/test.ttm'],emptyBuff,vrep.simx_opmode_oneshot_wait) # robotHandle=retInts[0] # Retrieve some handles: res, robotHandle = vrep.simxGetObjectHandle(clientID, 'UR5#', vrep.simx_opmode_oneshot_wait) res, target1 = vrep.simxGetObjectHandle(clientID, 'testPose1#', vrep.simx_opmode_oneshot_wait) # Retrieve the poses (i.e. transformation matrices, 12 values, last row is implicit) of some dummies in the scene res, retInts, target1Pose, retStrings, retBuffer = vrep.simxCallScriptFunction( clientID, 'remoteApiCommandServer', vrep.sim_scripttype_childscript, 'getObjectPose', [target1], [], [], emptyBuff, vrep.simx_opmode_oneshot_wait) res, retInts, robotInitialState, retStrings, retBuffer = vrep.simxCallScriptFunction( clientID, 'remoteApiCommandServer', vrep.sim_scripttype_childscript, 'getRobotState', [robotHandle], [], [], emptyBuff, vrep.simx_opmode_oneshot_wait) # Some parameters:
def get_handle(self, name): """ Get a handle of an object identified by [name] in vrep simulation """ return vrep.simxGetObjectHandle(self.client_id, name, vrep.simx_opmode_blocking)[1]
returnProlog = (check_output("swipl -s script_prolog.pl %s %s %s" % (sensors[0], sensors[1], sensors[2]), shell=True).decode('utf-8')) splitProlog = [] splitProlog = teste = returnProlog.split(',') vl = splitProlog[0].split('=')[1] vr = splitProlog[1].split('=')[1] if(vl == 'Y'): vl = vr return (float(vl), float(vr)) clientID = vrep.simxStart(serverIP,serverPort,True,True,2000,5) if clientID <> -1: print ('Servidor conectado!') # inicialização dos motores erro, leftMotorHandle = vrep.simxGetObjectHandle(clientID,'Pioneer_p3dx_leftMotor',vrep.simx_opmode_oneshot_wait) if erro <> 0: print 'Handle do motor esquerdo nao encontrado!' else: print 'Conectado ao motor esquerdo!' erro, rightMotorHandle = vrep.simxGetObjectHandle(clientID,'Pioneer_p3dx_rightMotor',vrep.simx_opmode_oneshot_wait) if erro <> 0: print 'Handle do motor direito nao encontrado!' else: print 'Conectado ao motor direito!' #inicialização dos sensores (remoteApi) for i in range(4): erro, sensorHandle[i] = vrep.simxGetObjectHandle(clientID,"Pioneer_p3dx_ultrasonicSensor%d" % (i+1),vrep.simx_opmode_oneshot_wait) if erro <> 0:
def invRightArm(xr, yr, zr): #start at zero position r = 0 rpose = rightArmPose(0, r, r, r, r, r, r, r) l = 0 lpose = leftArmPose(0, l, l, l, l, l, l, l) moveObj(rpose, clientID, objHandleRightTheoretical) moveObj(lpose, clientID, objHandleLeftTheoretical) for i in range(0, 7): vrep.simxSetJointTargetPosition(clientID, rightArm[i], math.radians(r), vrep.simx_opmode_oneshot) vrep.simxSetJointTargetPosition(clientID, leftArm[i], math.radians(l), vrep.simx_opmode_oneshot) rightTip = int( vrep.simxGetObjectHandle(clientID, 'Baxter_rightArm_tip', vrep.simx_opmode_blocking)[1]) T_2in0 = np.array([[1, 0, 0, xr], [0, 1, 0, yr], [0, 0, 1, zr], [0.00000000, 0.00000000, 0.00000000, 1.00000000]]) moveObj(T_2in0, clientID, objHandleRightTheoretical) rightLimits = [(math.radians(-168), math.radians(-168 + 336)), (math.radians(-96.5), math.radians(-96.5 + 191)), (math.radians(-121), math.radians(-121 + 179)), (math.radians(-173), math.radians(-173 + 346)), (math.radians(0), math.radians(0 + 148)), (math.radians(-173.3), math.radians(-173.3 + 346.5)), (math.radians(-88), math.radians(-88 + 206)), (math.radians(-173.3), math.radians(-173.3 + 346.5))] SR = rightArmS() thetaR = np.zeros((SR.shape[1], 1)) thetadot = np.array([100]) TR_1in0 = MR J = Jmaker(thetaR, SR) V0 = dvskew(sl.logm(T_2in0.dot(nl.inv(TR_1in0)))) V = nl.inv(admaker(TR_1in0)) @ V0 while (True): count = 0 failCount = 0 while nl.norm(V) >= .01 and nl.norm(thetadot) >= .0001: thetadot = td(J, V0, thetaR) thetaR = thetaR + thetadot * 1 TR_1in0 = TtoM(thetaR, SR, MR) J = Jmaker(thetaR, SR) V0 = dvskew(sl.logm(T_2in0.dot(nl.inv(TR_1in0)))) V = nl.inv(admaker(TR_1in0)) @ V0 count = count + 1 if (count == 60): print( "Norm of V could not merge - Using new starting position and trying again" ) print() thetaR = np.array([[random.uniform(-3.14, 3.14)], [random.uniform(-3.14, 3.14)], [random.uniform(-3.14, 3.14)], [random.uniform(-3.14, 3.14)], [random.uniform(-3.14, 3.14)], [random.uniform(-3.14, 3.14)], [random.uniform(-3.14, 3.14)], [random.uniform(-3.14, 3.14)]]) thetadot = np.array([100]) TR_1in0 = TtoM(thetaR, SR, MR) J = Jmaker(thetaR, SR) V0 = dvskew(sl.logm(T_2in0.dot(nl.inv(TR_1in0)))) V = dvskew(sl.logm(T_2in0.dot(nl.inv(TR_1in0)))) count = 0 failCount = failCount + 1 if (failCount >= 7): print("Position Unreachable") print() return # print(repr(thetaR)) #checking if the angles are within limit of their respective joints if (check(thetaR, rightLimits)): time.sleep(1) vrep.simxSetJointTargetPosition(clientID, rotJoint, thetaR[0], vrep.simx_opmode_oneshot) for i in range(0, 7): vrep.simxSetJointTargetPosition(clientID, rightArm[i], thetaR[i + 1], vrep.simx_opmode_oneshot) time.sleep(.5) time.sleep(.5) leftEndTip = vrep.simxGetObjectPosition( clientID, rightTip, -1, vrep.simx_opmode_blocking)[1] if (nl.norm(T_2in0[:3, 3] - np.array(leftEndTip)) > 0.01): #checking final pose, in case any obsticles got in the way (even though joint angles were okay.) print( "Hmmmm, the end of the baxter arm is not where it should be. Collision detected" ) print("Difference: " + str(nl.norm(T_2in0[:3, 3] - np.array(leftEndTip)))) print() thetaR = np.array([[random.uniform(-3.14, 3.14)], [random.uniform(-3.14, 3.14)], [random.uniform(-3.14, 3.14)], [random.uniform(-3.14, 3.14)], [random.uniform(-3.14, 3.14)], [random.uniform(-3.14, 3.14)], [random.uniform(-3.14, 3.14)], [random.uniform(-3.14, 3.14)]]) thetadot = np.array([100]) TR_1in0 = TtoM(thetaR, SR, MR) J = Jmaker(thetaR, SR) V0 = dvskew(sl.logm(T_2in0.dot(nl.inv(TR_1in0)))) V = dvskew(sl.logm(T_2in0.dot(nl.inv(TR_1in0)))) continue print("BINGO! Position Achieved!") print() time.sleep(3) break else: print( "Came up with set of thetas where some of them where out of range, trying again with different starting position" ) print() #try different starting orientation to get different end angles thetaR = np.array([[random.uniform(-3.14, 3.14)], [random.uniform(-3.14, 3.14)], [random.uniform(-3.14, 3.14)], [random.uniform(-3.14, 3.14)], [random.uniform(-3.14, 3.14)], [random.uniform(-3.14, 3.14)], [random.uniform(-3.14, 3.14)], [random.uniform(-3.14, 3.14)]]) thetadot = np.array([100]) TR_1in0 = TtoM(thetaR, SR, MR) J = Jmaker(thetaR, SR) V0 = dvskew(sl.logm(T_2in0.dot(nl.inv(TR_1in0)))) V = dvskew(sl.logm(T_2in0.dot(nl.inv(TR_1in0))))
DirectionProbability = 50 Turning = False LeftMotorSignal = 2 RightMotorSignal = 2 Speed = 2 print ('Program started') vrep.simxFinish(-1) # just in case, close all opened connections clientID = vrep.simxStart('127.0.0.1', 19999, True, True, 5000, 5) # Connect to V-REP if clientID != -1: print ('Connected to remote API server') errorCode, LeftMotor = vrep.simxGetObjectHandle(clientID, "Pioneer_p3dx_leftMotor", vrep.simx_opmode_oneshot_wait) print errorCode print LeftMotor errorCode, RightMotor = vrep.simxGetObjectHandle(clientID, "Pioneer_p3dx_rightMotor", vrep.simx_opmode_oneshot_wait) print errorCode print RightMotor errorCode, front_left = vrep.simxGetObjectHandle(clientID, "Front_Left", vrep.simx_opmode_oneshot_wait) errorCode, front_right = vrep.simxGetObjectHandle(clientID, "Front_Left", vrep.simx_opmode_oneshot_wait) errorCode, sLeft = vrep.simxGetObjectHandle(clientID, "Pioneer_p3dx_ultrasonicSensor2", vrep.simx_opmode_oneshot_wait) errorCode, sRight = vrep.simxGetObjectHandle(clientID, "Pioneer_p3dx_ultrasonicSensor7", vrep.simx_opmode_oneshot_wait) while True: errorCode, detectionState1, detectedPoint1, detectedObjectHandle, detectedSurfaceNormalVector = vrep.simxReadProximitySensor(
def get_robot_handles (clientID, robo): if robo == 0: # Get "handle" to ALL joints of robot # Get "handle" to the first joint of robot result, robot0_joint_one_handle = vrep.simxGetObjectHandle(clientID, 'UR3_joint1', vrep.simx_opmode_blocking) if result != vrep.simx_return_ok: raise Exception('could not get object handle for first joint') # Get "handle" to the second joint of robot result, robot0_joint_two_handle = vrep.simxGetObjectHandle(clientID, 'UR3_joint2', vrep.simx_opmode_blocking) if result != vrep.simx_return_ok: raise Exception('could not get object handle for first joint') # Get "handle" to the third joint of robot result, robot0_joint_three_handle = vrep.simxGetObjectHandle(clientID, 'UR3_joint3', vrep.simx_opmode_blocking) if result != vrep.simx_return_ok: raise Exception('could not get object handle for first joint') # Get "handle" to the fourth joint of robot result, robot0_joint_four_handle = vrep.simxGetObjectHandle(clientID, 'UR3_joint4', vrep.simx_opmode_blocking) if result != vrep.simx_return_ok: raise Exception('could not get object handle for first joint') # Get "handle" to the fifth joint of robot result, robot0_joint_five_handle = vrep.simxGetObjectHandle(clientID, 'UR3_joint5', vrep.simx_opmode_blocking) if result != vrep.simx_return_ok: raise Exception('could not get object handle for first joint') # Get "handle" to the sixth joint of robot result, robot0_joint_six_handle = vrep.simxGetObjectHandle(clientID, 'UR3_joint6', vrep.simx_opmode_blocking) if result != vrep.simx_return_ok: raise Exception('could not get object handle for first joint') # Wait two seconds time.sleep(2) robo_handles = np.array([robot0_joint_one_handle, robot0_joint_two_handle, robot0_joint_three_handle, robot0_joint_four_handle, robot0_joint_five_handle, robot0_joint_six_handle]) return robo_handles elif robo == 1: # Get "handle" to ALL joints of robot # Get "handle" to the first joint of robot result, robot1_joint_one_handle = vrep.simxGetObjectHandle(clientID, 'UR3_joint1#0', vrep.simx_opmode_blocking) if result != vrep.simx_return_ok: raise Exception('could not get object handle for first joint') # Get "handle" to the second joint of robot result, robot1_joint_two_handle = vrep.simxGetObjectHandle(clientID, 'UR3_joint2#0', vrep.simx_opmode_blocking) if result != vrep.simx_return_ok: raise Exception('could not get object handle for second joint') # Get "handle" to the third joint of robot result, robot1_joint_three_handle = vrep.simxGetObjectHandle(clientID, 'UR3_joint3#0', vrep.simx_opmode_blocking) if result != vrep.simx_return_ok: raise Exception('could not get object handle for third joint') # Get "handle" to the fourth joint of robot result, robot1_joint_four_handle = vrep.simxGetObjectHandle(clientID, 'UR3_joint4#0', vrep.simx_opmode_blocking) if result != vrep.simx_return_ok: raise Exception('could not get object handle for fourth joint') # Get "handle" to the fifth joint of robot result, robot1_joint_five_handle = vrep.simxGetObjectHandle(clientID, 'UR3_joint5#0', vrep.simx_opmode_blocking) if result != vrep.simx_return_ok: raise Exception('could not get object handle for fifth joint') # Get "handle" to the sixth joint of robot result, robot1_joint_six_handle = vrep.simxGetObjectHandle(clientID, 'UR3_joint6#0', vrep.simx_opmode_blocking) if result != vrep.simx_return_ok: raise Exception('could not get object handle for sixth joint') # Wait two seconds time.sleep(2) robo_handles = np.array([robot1_joint_one_handle, robot1_joint_two_handle, robot1_joint_three_handle, robot1_joint_four_handle, robot1_joint_five_handle, robot1_joint_six_handle]) return robo_handles else: raise Exception('Wrong input, could not get handles')
# lastdist = -1 # bestdist = -1 # dist_ckp = [False]*5 print('Program started') vrep.simxFinish(-1) # just in case, close all opened connections clientID = vrep.simxStart( '127.0.0.1', 19997, True, True, -500000, 5) # Connect to V-REP, set a very large time-out for blocking commands if clientID != -1: print('Connected to remote API server') emptyBuff = bytearray() # Start the simulation: vrep.simxStartSimulation(clientID, vrep.simx_opmode_oneshot_wait) #Retrive the body res, body = vrep.simxGetObjectHandle(clientID, 'body', vrep.simx_opmode_blocking) res, goal = vrep.simxGetObjectHandle(clientID, 'Target', vrep.simx_opmode_blocking) # Retrieve the poles pole = np.zeros(19, dtype='int32') for i in range(1, 19): res, pole[i] = vrep.simxGetObjectHandle(clientID, 'P' + str(i), vrep.simx_opmode_blocking) # Retrive the U1 U1 = np.zeros(6, dtype='int32') for i in range(0, 6): res, U1[i] = vrep.simxGetObjectHandle(clientID, 'Hip' + str(i + 1), vrep.simx_opmode_blocking) # Retrive the U2 U2 = np.zeros(6, dtype='int32') res, U2[0] = vrep.simxGetObjectHandle(clientID, 'shaft2',
def getPosition(clientID): res, obj = vrep.simxGetObjectHandle(clientID, 'Sphere', vrep.simx_opmode_blocking) pose = vrep.simxGetObjectPosition(clientID, obj, -1, vrep.simx_opmode_blocking) return pose
try: 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 ('--------------------------------------------------------------') print ('') import time print('Program Started') vrep.simxFinish(-1) #close all opened connections clientID = vrep.simxStart('127.0.0.1', 19997, True, True, 5000, 5) if clientID != -1: print('Connected to Remote API Server...') vrep.simxStartSimulation(clientID, vrep.simx_opmode_oneshot) res, planeHandle = vrep.simxGetObjectHandle(clientID, 'Quadricopter#', vrep.simx_opmode_blocking) #res, retInt, retFloat, retString, retBuffer = vrep.simxCallScriptFunction(clientID, 'misson_landing', ) time.sleep(10) vrep.simxStopSimulation(clientID, vrep.simx_opmode_blocking) vrep.simxFinish(clientID) else: print('Failed connecting to remote API server') print('Program ended')
clientID = vrep.simxStart('127.0.0.1', 19997, True, True, 5000, 5) if clientID == -1: raise Exception('Failed connecting to remote API server') ''' # Get "handle" to the first joint of robot result, joint_one_handle = vrep.simxGetObjectHandle(clientID, 'UR3_joint1', vrep.simx_opmode_blocking) if result != vrep.simx_return_ok: raise Exception('could not get object handle for first joint') ''' ## define Baxter's joints # define the body joints bodyJoints = np.zeros(3) bodyError = np.zeros(3) vertError, vertJoint = vrep.simxGetObjectHandle(clientID, 'Baxter_verticalJoint', vrep.simx_opmode_blocking) rotError, rotJoint = vrep.simxGetObjectHandle(clientID, 'Baxter_rotationJoint', vrep.simx_opmode_blocking) monitorError, monitorJoint = vrep.simxGetObjectHandle( clientID, 'Baxter_monitorJoint', vrep.simx_opmode_blocking) bodyJoints[0] = vertJoint bodyError[0] = vertError bodyJoints[1] = rotJoint bodyError[1] = rotError bodyJoints[2] = monitorJoint bodyError[2] = monitorError bodyJoints = bodyJoints.astype(int) bodyError = bodyError.astype(int)
import matplotlib.pyplot as plt vrep.simxFinish(-1) #Terminar todas las conexiones clientID = vrep.simxStart( '127.0.0.1', 19999, True, True, 5000, 5) #Iniciar una nueva conexion en el puerto 19999 (direccion por defecto) if clientID != -1: print('Conexion establecida') else: sys.exit("Error: no se puede conectar") #Terminar este script #Guardar la referencia de los motores _, left_motor_handle = vrep.simxGetObjectHandle(clientID, 'Pioneer_p3dx_leftMotor', vrep.simx_opmode_oneshot_wait) _, right_motor_handle = vrep.simxGetObjectHandle(clientID, 'Pioneer_p3dx_rightMotor', vrep.simx_opmode_oneshot_wait) #Guardar la referencia de la camara _, camhandle = vrep.simxGetObjectHandle(clientID, 'Vision_sensor', vrep.simx_opmode_oneshot_wait) #acceder a los datos del laser _, datosLaserComp = vrep.simxGetStringSignal(clientID, 'LaserData', vrep.simx_opmode_streaming) velocidad = 0.35 #Variable para la velocidad de los motores
_, tip_position = vrep.simxGetObjectPosition(arm.clientID, arm.tip, -1, vrep.simx_opmode_blocking) print("vrep_tip", tip_position) exit()""" dist = [] for i in range(1000): joint_positions = (np.random.rand(5) * 2.0) - 1.0 print(joint_positions) arm.set_joint_positions(joint_positions) print(arm.get_joint_positions()) print() arm.render() DH_tip = arm.get_tip_position() _, tip = vrep.simxGetObjectHandle(arm.clientID, 'AL5D_tip', vrep.simx_opmode_blocking) _, real_tip = vrep.simxGetObjectPosition(arm.clientID, tip, -1, vrep.simx_opmode_blocking) real_tip = np.array(real_tip) d = np.sqrt(np.sum(np.power(DH_tip - real_tip, 2))) dist.append(d) print(DH_tip) print(real_tip) time.sleep(3) print("mean:", np.mean(dist)) print("min:", np.min(dist)) print("max:", np.max(dist)) print("std:", np.std(dist)) exit() exit()
sensorHandle = [] dist = [] leftMotorHandle = 0 rightMotorHandle = 0 global v_Left, v_Right, tacoDir, tacoEsq v_Left = 1 v_Right = 1 if (clientID!=-1): print ("Servidor Conectado!") #------------------------------Inicializa Sensores ---------------------------- for i in range(0,8): nomeSensor.append("sensor" + str(i+1)) res, handle = vrep.simxGetObjectHandle(clientID, nomeSensor[i], vrep.simx_opmode_oneshot_wait) if(res != vrep.simx_return_ok): print (nomeSensor[i] + " nao conectado") else: print (nomeSensor[i] + " conectado") sensorHandle.append(handle) #------------------------------Inicializa Motores ---------------------------- resLeft, leftMotorHandle = vrep.simxGetObjectHandle(clientID, "Pioneer_p3dx_leftMotor", vrep.simx_opmode_oneshot_wait) if(resLeft != vrep.simx_return_ok): print("Motor Esquerdo : Handle nao encontrado!") else: print("Motor Esquerdo: Conectado") resRight, rightMotorHandle = vrep.simxGetObjectHandle(clientID, "Pioneer_p3dx_rightMotor", vrep.simx_opmode_oneshot_wait)
import forwardKinematics import vrep import time import numpy as np # Close all open connections (just in case) vrep.simxFinish(-1) # Connect to V-REP (raise exception on failure) clientID = vrep.simxStart('127.0.0.1', 19997, True, True, 5000, 5) if clientID == -1: raise Exception('Failed connecting to remote API server') # Get "handle" to the first joint of robot result, joint_one_handle = vrep.simxGetObjectHandle(clientID, 'UR3_joint1', vrep.simx_opmode_blocking) if result != vrep.simx_return_ok: raise Exception('could not get object handle for first joint') # Get "handle" to the second joint of robot result, joint_two_handle = vrep.simxGetObjectHandle(clientID, 'UR3_joint2', vrep.simx_opmode_blocking) if result != vrep.simx_return_ok: raise Exception('could not get object handle for second joint') # Get "handle" to the third joint of robot result, joint_three_handle = vrep.simxGetObjectHandle( clientID, 'UR3_joint3', vrep.simx_opmode_blocking) if result != vrep.simx_return_ok: raise Exception('could not get object handle for third joint')
initialize() print(clientID) generation = 1 genome = initgenome() #initialization cgenome = convert(genome) #normalization best_genome = cgenome[:] for j in range(0, 5000): print("Generation:") print(generation) cur_genome = mutate(best_genome[:]) #Mutation print("Genome:") print(cur_genome) vrep.simxLoadModel(clientID, "C:\quad0_1.ttm", 0, vrep.simx_opmode_blocking) #simulation loading time.sleep(1) errorcode, model_handle = vrep.simxGetObjectHandle( clientID, "quadruped", vrep.simx_opmode_blocking) i = 0 while i < 8: #8 joints reduced to 4 name = "quad_joint" + str(i + 1) jointhandles.append( vrep.simxGetObjectHandle(clientID, name, vrep.simx_opmode_blocking)[1]) i = i + 1 errorcode, quad = vrep.simxGetObjectHandle(clientID, 'quad_body', vrep.simx_opmode_blocking) k = 0 while k <= 10: for i in range(0, 4): vrep.simxSetJointTargetVelocity(clientID, jointhandles[i],
i = 0 ur5ready = 0 while i < TIMEOUT and ur5ready == 0: i = i + 1 errorCode, ur5ready = vrep.simxGetIntegerSignal(clientID, 'UR5READY', vrep.simx_opmode_blocking) time.sleep(step) if i >= TIMEOUT: print('An error occurred in your V-REP server') vrep.simxFinish(clientID) ##### Obtain the handle jointHandle = np.zeros((jointNum, ), dtype=np.int) # 注意是整型 for i in range(jointNum): errorCode, returnHandle = vrep.simxGetObjectHandle( clientID, jointName + str(i + 1), vrep.simx_opmode_blocking) jointHandle[i] = returnHandle time.sleep(2) errorCode, holeHandle = vrep.simxGetObjectHandle(clientID, 'Hole', vrep.simx_opmode_blocking) errorCode, ikTipHandle = vrep.simxGetObjectHandle(clientID, 'UR5_ikTip', vrep.simx_opmode_blocking) errorCode, connectionHandle = vrep.simxGetObjectHandle( clientID, 'UR5_connection', vrep.simx_opmode_blocking) print('Handles available!') ##### Obtain the position of the hole errorCode, targetPosition = vrep.simxGetObjectPosition( clientID, holeHandle, -1, vrep.simx_opmode_streaming)
[0, 0, 1, 0.0771], [0, 0, 0, 1]]) pos_wrt_robot = np.linalg.solve(transmation_matrix, np.array(pos_wrtworld)) return pos_wrt_robot if __name__ == "__main__": rospy.init_node('points_talker', anonymous=True) error_collect = [] vrep.simxFinish(-1) # just in case, close all opened connections clientID = vrep.simxStart('127.0.0.1', 19997, True, True, 5000, 5) # Connect to V-REP joints = [1] * 3 angles = np.zeros(3) for i in range(3): errorCode, joints[i] = vrep.simxGetObjectHandle( clientID, 'IRB140_joint' + str(i + 1), vrep.simx_opmode_blocking) angles[i] = (vrep.simxGetJointPosition(clientID, joints[i], vrep.simx_opmode_streaming)[1]) epsilon = 6e-2 for i in range(3): vrep.simxSetJointTargetVelocity(clientID, joints[i], 0., vrep.simx_opmode_oneshot) vrep.simxSetJointForce(clientID, joints[i], 9999., vrep.simx_opmode_oneshot) # Kp = np.abs(error/np.sum(np.abs(error))) #Kp = 2000 #Kv = 100 # kp = [2000, 2000,2000] # kv = [100,100,100] res, floor_handle = vrep.simxGetObjectHandle(clientID, 'floor_handle', vrep.simx_opmode_blocking)
clientID = vrep.simxStart( '127.0.0.1', 19997, True, True, -500000, 5) # Connect to V-REP, set a very large time-out for blocking commands if clientID != -1: print('Connected to remote API server') emptyBuff = bytearray() # Start the simulation: vrep.simxStartSimulation(clientID, vrep.simx_opmode_oneshot_wait) # Load a robot instance: res,retInts,retFloats,retStrings,retBuffer=vrep.simxCallScriptFunction(clientID,'remoteApiCommandServer',vrep.sim_scripttype_childscript,'loadRobot',[],[0,0,0,0],['d:/v_rep/qrelease/release/test.ttm'],emptyBuff,vrep.simx_opmode_oneshot_wait) # robotHandle=retInts[0] # Retrieve some handles: res, robotHandle = vrep.simxGetObjectHandle(clientID, 'IRB4600#', vrep.simx_opmode_oneshot_wait) res, target1 = vrep.simxGetObjectHandle(clientID, 'testPose1#', vrep.simx_opmode_oneshot_wait) res, target2 = vrep.simxGetObjectHandle(clientID, 'testPose2#', vrep.simx_opmode_oneshot_wait) res, target3 = vrep.simxGetObjectHandle(clientID, 'testPose3#', vrep.simx_opmode_oneshot_wait) res, target4 = vrep.simxGetObjectHandle(clientID, 'testPose4#', vrep.simx_opmode_oneshot_wait) # Retrieve the poses (i.e. transformation matrices, 12 values, last row is implicit) of some dummies in the scene res, retInts, target1Pose, retStrings, retBuffer = vrep.simxCallScriptFunction( clientID, 'remoteApiCommandServer', vrep.sim_scripttype_childscript, 'getObjectPose', [target1], [], [], emptyBuff, vrep.simx_opmode_oneshot_wait) res, retInts, target2Pose, retStrings, retBuffer = vrep.simxCallScriptFunction(
# Close all open connections (Clear bad cache) vrep.simxFinish(-1) # Connect to V-REP (raise exception on failure) clientID = vrep.simxStart('127.0.0.1', 19999, True, True, 5000, 5) if clientID == -1: raise Exception('Failed connecting to remote API server') # ======================================== Setup "handle" =========================================== # # Print object name list result, joint_name, intData, floatData, stringData = vrep.simxGetObjectGroupData( clientID, vrep.sim_appobj_object_type, 0, vrep.simx_opmode_blocking) #print(stringData) # Get "handle" to the base of robot result, base_handle = vrep.simxGetObjectHandle(clientID, 'UR5_link1_visible', vrep.simx_opmode_blocking) if result != vrep.simx_return_ok: raise Exception('could not get object handle for base frame') # Get "handle" to the all joints of robot result, joint_one_handle = vrep.simxGetObjectHandle(clientID, 'UR5_joint1', vrep.simx_opmode_blocking) if result != vrep.simx_return_ok: raise Exception('could not get object handle for first joint') result, joint_two_handle = vrep.simxGetObjectHandle(clientID, 'UR5_joint2', vrep.simx_opmode_blocking) if result != vrep.simx_return_ok: raise Exception('could not get object handle for second joint') result, joint_three_handle = vrep.simxGetObjectHandle( clientID, 'UR5_joint3', vrep.simx_opmode_blocking) if result != vrep.simx_return_ok:
import sys import time vrep.simxFinish(-1) # just in case, close all opened connections clientID = vrep.simxStart('127.0.0.1', 19999, True, True, 5000, 5) # Connect to V-REP if clientID != -1: print('Connected to remote API server') else: print("connection not successful") sys.exit('Could not connect') errorCode, left_motor_handle = vrep.simxGetObjectHandle( clientID, 'left_joint', vrep.simx_opmode_oneshot_wait) #初始化 errorCode, right_motor_handle = vrep.simxGetObjectHandle( clientID, 'right_joint', vrep.simx_opmode_oneshot_wait) sensor_h = [] #empty list for handles #for loop to retrieve sensor arrays and initiate sensors for x in range(0, 5 + 1): errorCode, sensor_handle = vrep.simxGetObjectHandle( clientID, 'line_sensor' + str(x), vrep.simx_opmode_oneshot_wait) sensor_h.append(sensor_handle) #keep list of handles errorCode, detectionState, auxPackets = vrep.simxReadVisionSensor( clientID, sensor_handle, vrep.simx_opmode_streaming) t = time.time()
def _get_object_handle(self, name): code, handle = v.simxGetObjectHandle(self._id, name, self._def_op_mode) if code == v.simx_return_ok: return handle else: return None
def get_object_handle(self, string): #provides object handle for V-REP object errorCode, handle = vrep.simxGetObjectHandle( self.clientID, string, vrep.simx_opmode_oneshot_wait) assert errorCode == 0, 'Conection to ' + string + 'failed' return handle
if clientID == -1: print("ERROR: Cannot establish connection to vrep.") sys.exit() #getPath.setID(clientID) # Set Sampling time vrep.simxSetFloatingParameter(clientID, vrep.sim_floatparam_simulation_time_step, dt, vrep.simx_opmode_oneshot) vrep.simxSynchronous(clientID, True) ########## Get handles from vrep ########### _, vehicle_handle = vrep.simxGetObjectHandle(clientID, "dyros_vehicle0", vrep.simx_opmode_blocking) _, goal_handle = vrep.simxGetObjectHandle(clientID, "GoalPoint0", vrep.simx_opmode_blocking) _, colHandle = vrep.simxGetCollectionHandle(clientID, "Ref", vrep.simx_opmode_blocking) _, sensorColHandle = vrep.simxGetCollectionHandle( clientID, "Sensors", vrep.simx_opmode_blocking) _, refHandle, intData, doubleData, strData = vrep.simxGetObjectGroupData( clientID, colHandle, 9, vrep.simx_opmode_blocking) ref_from_scene_pos = np.reshape(doubleData, (len(refHandle), 6))[:, 0:3] ref_from_scene_ori = np.reshape(doubleData, (len(refHandle), 6))[:, 3:6] _, goalPos = vrep.simxGetObjectPosition(clientID, goal_handle, -1, vrep.simx_opmode_blocking)
# Option2. Move dummy # res, target_handle = vrep.simxGetObjectHandle(clientID, 'UR5_ikTarget', vrep.simx_opmode_blocking) # vrep.simxSetObjectPosition(clientID, target_handle, -1, [x, y, z], vrep.simx_opmode_oneshot) # # Option3. Move robot with given joint angle NO PATH PLANNING # jointHandles = [-1, -1, -1, -1, -1, -1] # targetPos1 = [90 * 3.14 / 180, 90 * 3.14 / 180, -90 * 3.14 / 180, 90 * 3.14 / 180, 90 * 3.14 / 180, 90 * 3.14 / 180] # # for j in range(0, len(data), 1): # for i in range(0, 6, 1): # res, jointHandles[i] = vrep.simxGetObjectHandle(clientID, 'UR5_joint' + str(i + 1), vrep.simx_opmode_blocking) # res= vrep.simxSetJointTargetPosition(clientID, jointHandles[i], data[j][i], vrep.simx_opmode_blocking) emptyBuff = bytearray() # Step1. Get robot and target handle res, robotHandle = vrep.simxGetObjectHandle(clientID, 'UR5', vrep.simx_opmode_oneshot_wait) # Step2. Retrieve the poses (i.e. transformation matrices, 12 values, last row is implicit) of some dummies in the scene # Step3. Get the robot initial state: res, retInts, robotInitialState, retStrings, retBuffer = vrep.simxCallScriptFunction( clientID, 'remoteApiCommandServer', vrep.sim_scripttype_childscript, 'getRobotState', [robotHandle], [], [], emptyBuff, vrep.simx_opmode_oneshot_wait) # Set Some parameters: approachVector = [ 0, 0, 1 ] # often a linear approach is required. This should also be part of the calculations when selecting an appropriate state for a given pose maxConfigsForDesiredPose = 20 # we will try to find 10 different states corresponding to the goal pose and order them according to distance from initial state maxTrialsForConfigSearch = 300 # a parameter needed for finding appropriate goal states
clientID = vrep.simxStart('127.0.0.1', 19997, True, True, 500, 5) if clientID != -1: # if we connected successfully print('Connected to remote API server') # --------------------- Setup the simulation vrep.simxSynchronous(clientID, True) joint_names = ['shoulder', 'elbow'] # joint target velocities discussed below joint_target_velocities = np.ones(len(joint_names)) * 10000.0 # get the handles for each joint and set up streaming joint_handles = [ vrep.simxGetObjectHandle(clientID, name, vrep.simx_opmode_blocking)[1] for name in joint_names ] # get handle for target and set up streaming _, target_handle = vrep.simxGetObjectHandle(clientID, 'target', vrep.simx_opmode_blocking) dt = .001 vrep.simxSetFloatingParameter( clientID, vrep.sim_floatparam_simulation_time_step, dt, # specify a simulation time step vrep.simx_opmode_oneshot) # --------------------- Start the simulation
def get_new_nao_handles(nbrOfNao, clientID, Body): for i in range(0, nbrOfNao - 1): print '-> Head for NAO -> ' + str(i + 2) Body[0].append( vrep.simxGetObjectHandle(clientID, 'HeadYaw#' + str(i), vrep.simx_opmode_oneshot_wait)[1]) Body[1].append( vrep.simxGetObjectHandle(clientID, 'HeadPitch#' + str(i), vrep.simx_opmode_oneshot_wait)[1]) #Left Leg print '-> Left Leg for NAO -> ' + str(i + 2) Body[2].append( vrep.simxGetObjectHandle(clientID, 'LHipYawPitch3#' + str(i), vrep.simx_opmode_oneshot_wait)[1]) Body[3].append( vrep.simxGetObjectHandle(clientID, 'LHipRoll3#' + str(i), vrep.simx_opmode_oneshot_wait)[1]) Body[4].append( vrep.simxGetObjectHandle(clientID, 'LHipPitch3#' + str(i), vrep.simx_opmode_oneshot_wait)[1]) Body[5].append( vrep.simxGetObjectHandle(clientID, 'LKneePitch3#' + str(i), vrep.simx_opmode_oneshot_wait)[1]) Body[6].append( vrep.simxGetObjectHandle(clientID, 'LAnklePitch3#' + str(i), vrep.simx_opmode_oneshot_wait)[1]) Body[7].append( vrep.simxGetObjectHandle(clientID, 'LAnkleRoll3#' + str(i), vrep.simx_opmode_oneshot_wait)[1]) #Right Leg print '-> Right Leg for NAO -> ' + str(i + 2) Body[8].append( vrep.simxGetObjectHandle(clientID, 'RHipYawPitch3#' + str(i), vrep.simx_opmode_oneshot_wait)[1]) Body[9].append( vrep.simxGetObjectHandle(clientID, 'RHipRoll3#' + str(i), vrep.simx_opmode_oneshot_wait)[1]) Body[10].append( vrep.simxGetObjectHandle(clientID, 'RHipPitch3#' + str(i), vrep.simx_opmode_oneshot_wait)[1]) Body[11].append( vrep.simxGetObjectHandle(clientID, 'RKneePitch3#' + str(i), vrep.simx_opmode_oneshot_wait)[1]) Body[12].append( vrep.simxGetObjectHandle(clientID, 'RAnklePitch3#' + str(i), vrep.simx_opmode_oneshot_wait)[1]) Body[13].append( vrep.simxGetObjectHandle(clientID, 'RAnkleRoll3#' + str(i), vrep.simx_opmode_oneshot_wait)[1]) #Left Arm print '-> Left Arm for NAO -> ' + str(i + 2) Body[14].append( vrep.simxGetObjectHandle(clientID, 'LShoulderPitch3#' + str(i), vrep.simx_opmode_oneshot_wait)[1]) Body[15].append( vrep.simxGetObjectHandle(clientID, 'LShoulderRoll3#' + str(i), vrep.simx_opmode_oneshot_wait)[1]) Body[16].append( vrep.simxGetObjectHandle(clientID, 'LElbowYaw3#' + str(i), vrep.simx_opmode_oneshot_wait)[1]) Body[17].append( vrep.simxGetObjectHandle(clientID, 'LElbowRoll3#' + str(i), vrep.simx_opmode_oneshot_wait)[1]) Body[18].append( vrep.simxGetObjectHandle(clientID, 'LWristYaw3#' + str(i), vrep.simx_opmode_oneshot_wait)[1]) #Right Arm print '-> Right Arm for NAO -> ' + str(i + 2) Body[19].append( vrep.simxGetObjectHandle(clientID, 'RShoulderPitch3#' + str(i), vrep.simx_opmode_oneshot_wait)[1]) Body[20].append( vrep.simxGetObjectHandle(clientID, 'RShoulderRoll3#' + str(i), vrep.simx_opmode_oneshot_wait)[1]) Body[21].append( vrep.simxGetObjectHandle(clientID, 'RElbowYaw3#' + str(i), vrep.simx_opmode_oneshot_wait)[1]) Body[22].append( vrep.simxGetObjectHandle(clientID, 'RElbowRoll3#' + str(i), vrep.simx_opmode_oneshot_wait)[1]) Body[23].append( vrep.simxGetObjectHandle(clientID, 'RWristYaw3#' + str(i), vrep.simx_opmode_oneshot_wait)[1]) #Left fingers print '-> Left Fingers for NAO -> ' + str(i + 2) Body[24].append( vrep.simxGetObjectHandle(clientID, 'NAO_LThumbBase#' + str(i), vrep.simx_opmode_oneshot_wait)[1]) Body[24].append( vrep.simxGetObjectHandle(clientID, 'Revolute_joint8#' + str(i), vrep.simx_opmode_oneshot_wait)[1]) Body[24].append( vrep.simxGetObjectHandle(clientID, 'NAO_LLFingerBase#' + str(i), vrep.simx_opmode_oneshot_wait)[1]) Body[24].append( vrep.simxGetObjectHandle(clientID, 'Revolute_joint12#' + str(i), vrep.simx_opmode_oneshot_wait)[1]) Body[24].append( vrep.simxGetObjectHandle(clientID, 'Revolute_joint14#' + str(i), vrep.simx_opmode_oneshot_wait)[1]) Body[24].append( vrep.simxGetObjectHandle(clientID, 'NAO_LRFinger_Base#' + str(i), vrep.simx_opmode_oneshot_wait)[1]) Body[24].append( vrep.simxGetObjectHandle(clientID, 'Revolute_joint11#' + str(i), vrep.simx_opmode_oneshot_wait)[1]) Body[24].append( vrep.simxGetObjectHandle(clientID, 'Revolute_joint13#' + str(i), vrep.simx_opmode_oneshot_wait)[1]) Body[25].append(Body[24][i * 8:(i + 1) * 8]) #Right Fingers print '-> Right Fingers for NAO -> ' + str(i + 2) Body[26].append( vrep.simxGetObjectHandle(clientID, 'NAO_RThumbBase#' + str(i), vrep.simx_opmode_oneshot_wait)[1]) Body[26].append( vrep.simxGetObjectHandle(clientID, 'Revolute_joint0#' + str(i), vrep.simx_opmode_oneshot_wait)[1]) Body[26].append( vrep.simxGetObjectHandle(clientID, 'NAO_RLFingerBase#' + str(i), vrep.simx_opmode_oneshot_wait)[1]) Body[26].append( vrep.simxGetObjectHandle(clientID, 'Revolute_joint5#' + str(i), vrep.simx_opmode_oneshot_wait)[1]) Body[26].append( vrep.simxGetObjectHandle(clientID, 'Revolute_joint6#' + str(i), vrep.simx_opmode_oneshot_wait)[1]) Body[26].append( vrep.simxGetObjectHandle(clientID, 'NAO_RRFinger_Base#' + str(i), vrep.simx_opmode_oneshot_wait)[1]) Body[26].append( vrep.simxGetObjectHandle(clientID, 'Revolute_joint2#' + str(i), vrep.simx_opmode_oneshot_wait)[1]) Body[26].append( vrep.simxGetObjectHandle(clientID, 'Revolute_joint3#' + str(i), vrep.simx_opmode_oneshot_wait)[1]) Body[27].append(Body[26][(i + 1) * 8:(i + 2) * 8])
def get_object_handle(self, name): handle, = self.RAPI_rc( vrep.simxGetObjectHandle(self.cID, name, vrep.simx_opmode_blocking)) return handle