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 copyObject(self,name,position,relativeName=None): """ Copy and paste an object in a specific position """ if name in self.handles.keys(): pass else: self.initHandle(name) objectHandle=self.handles[name] if relativeName: if relativeName in self.handles.keys(): pass else: self.initHandle(relativeName) relativeHandle=self.handles[relativeName] else: relativeHandle = -1 returnCode, newObjectHandles=vrep.simxCopyPasteObjects(self.clientID, [objectHandle], vrep.simx_opmode_oneshot_wait) print("newOH",newObjectHandles) newObjectHandle=newObjectHandles[0] vrep.simxSetObjectPosition(self.clientID,newObjectHandle,relativeHandle,position,vrep.simx_opmode_oneshot) return newObjectHandle
def copyRobot(id_robot, num): ret, item = vrep.simxCopyPasteObjects(0, [id_robot], vrep.simx_opmode_blocking) print(num) wrist = getHandle("WristMotor#" + str(num)) elbow = getHandle("ElbowMotor#" + str(num)) shoulder = getHandle("ShoulderMotor#" + str(num)) return item[0], wrist, elbow, shoulder
def duplicate(self): ret, handles = vrep.simxCopyPasteObjects(self.client_id, [self.handle], self.BLOCK) if ret == 0: # TODO return proper objects, we are not savages. return handles else: raise ConnectionError(VRepError.create(ret))
def __init__(self,clientID): motors =['motor1','motor2','motor3','motor4','motor5','motor6','motor7'] self.motor_handles = [vrep.simxGetObjectHandle(clientID,name, vrep.simx_opmode_blocking)[1] for name in motors] sensors = ['head_sensor','sensor1r','sensor1l','sensor2r','sensor2l','sensor3r','sensor3l','sensor4r','sensor4l', 'sensor5r','sensor5l','sensor6r','sensor6l','sensor7r','sensor7l','sensor8r','sensor8l'] self.sensor_handles = [vrep.simxGetObjectHandle(clientID, sensor, vrep.simx_opmode_blocking)[1] for sensor in sensors] obstacles = ['Obj0','Obj1','Obj2','Obj3','Obj4','Obj5','Obj6','Obj7','Obj8','Obj9','Obj10'] self.obst_handles = [vrep.simxGetObjectHandle(clientID, obj, vrep.simx_opmode_blocking)[1] for obj in obstacles] body_modules = ['snake_v2', 'module2', 'module3', 'module4', 'module5', 'module6', 'module7', 'module8'] self.body_module_handles = [vrep.simxGetObjectHandle(clientID, obj, vrep.simx_opmode_blocking)[1] for obj in body_modules] self.snake_body = vrep.simxGetObjectHandle(clientID, 'snake_v2', vrep.simx_opmode_blocking)[1] print ("\nSet motor torques...\n") for motor_handle in self.motor_handles: _=vrep.simxSetJointForce(clientID,motor_handle,5,vrep.simx_opmode_blocking) if _ !=0: raise Exception() self.setMotor(clientID, 0, 1) print ("\nAssign sensor values\n") for sensor_handle in self.sensor_handles: _,_,_,_,_ = vrep.simxReadProximitySensor(clientID, sensor_handle, vrep.simx_opmode_streaming) print ("\nSetting Object postions...\n") self.obst_count = r.randint(5,8) for i in range (self.obst_count): obst = r.choice(self.obst_handles) _, [obst_copy] = vrep.simxCopyPasteObjects(clientID, [obst], vrep.simx_opmode_blocking) x = round(r.uniform(-5,8),3) y = round(r.uniform(-7.5,8),3) _ = vrep.simxSetObjectPosition(clientID, obst_copy, -1, (x, y, 0.25),vrep.simx_opmode_oneshot) _ = vrep.simxSetObjectOrientation(clientID, obst_copy, obst_copy, (r.randint(0,180),0,0), vrep.simx_opmode_oneshot) #_ = vrep.simxSetObjectPosition(clientID, self.snake_body, -1, (-9.5, 10, 0.085), vrep.simx_opmode_oneshot) #---------------------------------------------Initialising the motor position buffer self.modulepos = list() for motor_handle in self.motor_handles: _,moduleCord = vrep.simxGetObjectPosition(clientID,motor_handle,-1,vrep.simx_opmode_streaming) self.modulepos.append(moduleCord) if _ !=1 : print (_) raise Exception() #---------------------------------------------Initialising body_modules collision buffer for module in self.body_module_handles: _,collision_state = vrep.simxReadCollision(clientID, module, vrep.simx_opmode_streaming) print _
def _add_to_env(self, cell): # cell[0]: type of cell. 1: red. 2: white. if (cell[0] == 1): cell_type = 'red' elif (cell[0] == 2): cell_type = 'white' # get object handle to copy res, obj_h = vrep.simxGetObjectHandle(self.clientID, cell_type, vrep.simx_opmode_blocking) if res == vrep.simx_return_ok: print( "Get Red cell handle" ) # display the reply from V-REP (in this case, the handle of the created dummy) return True else: print('Remote function call failed: Get red cell handle') return False res = vrep.simxCopyPasteObjects(self.clientID, obj_h, vrep.simx_opmode_blocking)
def copy_paste(self): """Copy and paste object.""" if self._handle < 0: if self._handle == MISSING_HANDLE: raise RuntimeError("Could not copy and paste {}: missing name " "or handle.".format(self._name)) if self._handle == REMOVED_OBJ_HANDLE: raise RuntimeError("Could not copy and paste {}: object " "removed.".format(self._name)) client_id = self.client_id if client_id is None: raise ConnectionError( "Could not copy and paste {}: not connected to V-REP remote " "API server.".format(self._name)) res, handles = vrep.simxCopyPasteObjects(client_id, [self._handle], vrep.simx_opmode_blocking) if res != vrep.simx_return_ok: raise ServerError("Could not copy and paste {}.".format( self._name)) return handles[0]
if nbrOfNao == 1: motionProxy.append(ALProxy('ALMotion', naoIP[0], naoPort[0])) postureProxy.append(ALProxy('ALRobotPosture', naoIP[0], naoPort[0])) else: motionProxy.append(ALProxy('ALMotion', naoIP[0], naoPort[0])) postureProxy.append(ALProxy('ALRobotPosture', naoIP[0], naoPort[0])) y = 0.8 # Pause the simulation to avoid collision problems vrep.simxPauseSimulation(clientID, vrep.simx_opmode_oneshot) time.sleep(2) for i in range(0, nbrOfNao - 1): motionProxy.append(ALProxy('ALMotion', naoIP[i + 1], naoPort[i + 1])) postureProxy.append( ALProxy('ALRobotPosture', naoIP[i + 1], naoPort[i + 1])) # Create new NAo in VRep vrep.simxCopyPasteObjects(clientID, NAO_Handle, vrep.simx_opmode_oneshot_wait) # Get the handle of the new NAO copyNAO = vrep.simxGetObjectHandle(clientID, 'NAO#' + str(i), vrep.simx_opmode_oneshot_wait) # Change the position of the new NAO so it won't collide with others vrep.simxSetObjectPosition(clientID, copyNAO[1], -1, [0, y, 0.3518], vrep.simx_opmode_oneshot) y += 0.8 # Restart the simulation vrep.simxStartSimulation(clientID, vrep.simx_opmode_oneshot) print '================ Posture Initialization ================' # Go to the posture StandZero print 'Posture Initialization : StandZero' for i in range(0, nbrOfNao): motionProxy[i].stiffnessInterpolation('Body', 1.0, 1.0)
cv2.imshow("img",img) cv2.waitKey() cv2.destroyAllWindows() paths = rPoly(img) vrep_paths = [] for path in paths: vrep_path = [] for point in path: vrep_point = (point[0] * 25. / 512. - 12.5, (511 - point[1]) * 25. / 512. - 12.5) vrep_path.append(vrep_point) vrep_paths.append(vrep_path) path = vrep_paths[0] _, [disc_copy] = vrep.simxCopyPasteObjects(clientID, [disc], vrep.simx_opmode_blocking) _ = vrep.simxSetObjectPosition(clientID, disc_copy, -1,(path[0][0], path[0][1], 1), vrep.simx_opmode_oneshot) #---------------------------------------------------------testing remove plane to see the snake _ = vrep.simxSetObjectPosition(clientID, black_plane, -1, (0,0,-2), vrep.simx_opmode_oneshot) n=0 #------------------------------------------index of next point on path ret =1000#--------------------------------------return value of dist2disc count = 0.0 # --------------------------------Simulation time counter while count<5000 and n< (len(path)-1): #----------------------------------------------Reading collision status # collision = False
y_modulus = np.sqrt(b[0]**2+b[1]**2+b[2]**2) cos_angle = dot / x_modulus / y_modulus angle = np.arccos(cos_angle) # angle in radiant if a[2]>0: return angle else: return -angle errorCode,Ball=vrep.simxGetObjectHandle(clientID,'A_star_points',vrep.simx_opmode_oneshot_wait) objectHandles=np.array([Ball]) for next in path2: (a,b,c)=next x=0.4+0.4*a y=0.2+0.4*b z=0.3+0.4*c returnCode,newObjectHandles=vrep.simxCopyPasteObjects(clientID,objectHandles,vrep.simx_opmode_oneshot_wait) Ball_new=newObjectHandles[0] vrep.simxSetObjectPosition (clientID,Ball_new,-1,(x,y,z),vrep.simx_opmode_oneshot) #interpolation #1. step elimination of unnecessary nodes in the path, makes the path shorter, because of more direct movements in_progress=1 while in_progress>0: in_progress=0 i=0 while i <(len(path2)-2): if collision(path2[i],path2[i+2]): path2.pop(i+1) in_progress=1 i=i+1
#Proxy initialization if nbrOfNao == 1: motionProxy.append(ALProxy('ALMotion',naoIP[0],naoPort[0])) postureProxy.append(ALProxy('ALRobotPosture', naoIP[0], naoPort[0])) else: motionProxy.append(ALProxy('ALMotion',naoIP[0],naoPort[0])) postureProxy.append(ALProxy('ALRobotPosture', naoIP[0], naoPort[0])) y=0.8 #Pause the simulation to avoid collision problems vrep.simxPauseSimulation(clientID,vrep.simx_opmode_oneshot) time.sleep(2) for i in range(0,nbrOfNao-1): motionProxy.append(ALProxy('ALMotion',naoIP[i+1],naoPort[i+1])) postureProxy.append(ALProxy('ALRobotPosture', naoIP[i+1], naoPort[i+1])) #Create new NAo in VRep vrep.simxCopyPasteObjects(clientID,NAO_Handle,vrep.simx_opmode_oneshot_wait) #Get the handle of the new NAO copyNAO = vrep.simxGetObjectHandle(clientID,'NAO#'+str(i),vrep.simx_opmode_oneshot_wait) #Change the position of the new NAO so it won't collide with others vrep.simxSetObjectPosition(clientID,copyNAO[1],-1,[0,y,0.3518],vrep.simx_opmode_oneshot ) y+=0.8 #Restart the simulation vrep.simxStartSimulation(clientID,vrep.simx_opmode_oneshot) print '================ Posture Initialization ================' #Go to the posture StandZero print 'Posture Initialization : StandZero' for i in range(0,nbrOfNao): motionProxy[i].stiffnessInterpolation('Body', 1.0, 1.0) postureProxy[i].goToPosture('StandZero',1.0)
def mapgen_fast(scene_name,x,y,z,clientID): #load mapdata if file exits if os.path.isfile('./Mapdata/'+scene_name+'.txt'): fo = open('./Mapdata/'+scene_name+'.txt', "r") data_string = fo.read(); print "Read String is : ", str # Close opend file fo.close() arr = np.loads(data_string) return arr #otherwise generate mapdata else: x0=x y0=y #generate the 100 sensors and put them to theirstarting position errorCode,sensor1=vrep.simxGetObjectHandle(clientID,'Sensor_1',vrep.simx_opmode_oneshot_wait) objectHandles=np.array([sensor1]) sensor_data=np.zeros(shape=(10,10,1),dtype=int) for x in range(10): for y in range(10): returnCode,new_sensor_handle=vrep.simxCopyPasteObjects(clientID,objectHandles,vrep.simx_opmode_oneshot_wait) vrep.simxReadProximitySensor(clientID,new_sensor_handle[0],vrep.simx_opmode_streaming) sensor_data[x,y]=new_sensor_handle[0] x1=0.4+x*0.4 y1=0.2+y*0.4 z1=0.21 vrep.simxSetObjectPosition (clientID,sensor_data[x,y],-1,(x1,y1,z1),vrep.simx_opmode_oneshot) #init some values xmax=x0/4 ymax=y0/4 zmax=z/0.4 xmax=int(math.ceil(xmax)) ymax=int(math.ceil(ymax)) zmax=int(math.ceil(zmax)) arr=np.zeros(shape=(xmax*10,ymax*10,zmax),dtype=int) #loop to detect the whole area for index in range(xmax): for index2 in range(ymax): for index3 in range(zmax): for x in range(10): for y in range(10): errorCode,detectionState1,detectedPoint,detectedObjectHandle,detectedSurfaceNormalVector=vrep.simxReadProximitySensor (clientID,sensor_data[x,y],vrep.simx_opmode_buffer) #save result if (detectionState1): arr[index*10+x,index2*10+y,index3]=1 else: arr[index*10+x,index2*10+y,index3]=0 for x in range(10): for y in range(10): x1=0.4+4*(index)+x*0.4 y1=0.2+4*(index2)+y*0.4 z1=0.4*index3+0.21 vrep.simxSetObjectPosition (clientID,sensor_data[x,y],-1,(x1,y1,z1),vrep.simx_opmode_oneshot) time.sleep(1) #save mapdata to file arr=save(arr) data_string=np.ndarray.dumps(arr) print data_string fo = open('./Mapdata/'+scene_name+'.txt', "w") fo.write(data_string); fo.close() return arr
def mapgen_fast(scene_name,x,y,z,clientID): if os.path.isfile('./Mapdata/'+scene_name+'.txt'): fo = open('./Mapdata/'+scene_name+'.txt', "rb") arr = np.load(fo)['arr_0'] #data_string = fo.read(); #print ("Read String is : ", str) # Close opend file fo.close() #arr = np.loads(data_string) return arr else: x0=x y0=y errorCode,sensor1=vrep.simxGetObjectHandle(clientID,'Sensor_1',vrep.simx_opmode_oneshot_wait) objectHandles=np.array([sensor1]) #create hundred sensors sensor_data=np.zeros(shape=(10,10,1),dtype=int) for x in range(10): for y in range(10): returnCode,new_sensor_handle=vrep.simxCopyPasteObjects(clientID,objectHandles,vrep.simx_opmode_oneshot_wait) vrep.simxReadProximitySensor(clientID,new_sensor_handle[0],vrep.simx_opmode_streaming) sensor_data[x,y]=new_sensor_handle[0] x1=0.0+x*0.2 y1=0.0+y*0.2 z1=0.0 vrep.simxSetObjectPosition (clientID,sensor_data[x,y],-1,(x1,y1,z1),vrep.simx_opmode_oneshot) #move sensors to every point of map xmax=x0/2 ymax=y0/2 zmax=z/0.2 xmax=int(math.ceil(xmax)) ymax=int(math.ceil(ymax)) zmax=int(math.ceil(zmax)) arr=np.zeros(shape=(xmax*10,ymax*10,zmax),dtype=int) for index in range(xmax): for index2 in range(ymax): for index3 in range(zmax): for x in range(10): for y in range(10): #judge barrier errorCode,detectionState1,detectedPoint,detectedObjectHandle,detectedSurfaceNormalVector=vrep.simxReadProximitySensor (clientID,sensor_data[x,y],vrep.simx_opmode_buffer) if (detectionState1): arr[index*10+x,index2*10+y,index3]=1 #save result else: arr[index*10+x,index2*10+y,index3]=0 for x in range(10): for y in range(10): x1=0.0 + 2*index + x*0.2 y1=0.0 + 2*index2 + y*0.2 z1=0.0 + 0.2*index3 vrep.simxSetObjectPosition (clientID,sensor_data[x,y],-1,(x1,y1,z1),vrep.simx_opmode_oneshot) time.sleep(0.2) #arr=save(arr) data_string=np.ndarray.dumps(arr) #print ("Data string is:", data_string) fo = open('./Mapdata/'+scene_name+'.txt', "wb") np.savez(fo,arr) #fo.write(data_string); fo.close() return arr
def mapgen_fast(scene_name, x, y, z, clientID): #load mapdata if file exits if os.path.isfile('./Mapdata/' + scene_name + '.txt'): fo = open('./Mapdata/' + scene_name + '.txt', "r") data_string = fo.read() print "Read String is : ", str # Close opend file fo.close() arr = np.loads(data_string) return arr #otherwise generate mapdata else: x0 = x y0 = y #generate the 100 sensors and put them to theirstarting position errorCode, sensor1 = vrep.simxGetObjectHandle( clientID, 'Sensor_1', vrep.simx_opmode_oneshot_wait) objectHandles = np.array([sensor1]) sensor_data = np.zeros(shape=(10, 10, 1), dtype=int) for x in range(10): for y in range(10): returnCode, new_sensor_handle = vrep.simxCopyPasteObjects( clientID, objectHandles, vrep.simx_opmode_oneshot_wait) vrep.simxReadProximitySensor(clientID, new_sensor_handle[0], vrep.simx_opmode_streaming) sensor_data[x, y] = new_sensor_handle[0] x1 = 0.4 + x * 0.4 y1 = 0.2 + y * 0.4 z1 = 0.21 vrep.simxSetObjectPosition(clientID, sensor_data[x, y], -1, (x1, y1, z1), vrep.simx_opmode_oneshot) #init some values xmax = x0 / 4 ymax = y0 / 4 zmax = z / 0.4 xmax = int(math.ceil(xmax)) ymax = int(math.ceil(ymax)) zmax = int(math.ceil(zmax)) arr = np.zeros(shape=(xmax * 10, ymax * 10, zmax), dtype=int) #loop to detect the whole area for index in range(xmax): for index2 in range(ymax): for index3 in range(zmax): for x in range(10): for y in range(10): errorCode, detectionState1, detectedPoint, detectedObjectHandle, detectedSurfaceNormalVector = vrep.simxReadProximitySensor( clientID, sensor_data[x, y], vrep.simx_opmode_buffer) #save result if (detectionState1): arr[index * 10 + x, index2 * 10 + y, index3] = 1 else: arr[index * 10 + x, index2 * 10 + y, index3] = 0 for x in range(10): for y in range(10): x1 = 0.4 + 4 * (index) + x * 0.4 y1 = 0.2 + 4 * (index2) + y * 0.4 z1 = 0.4 * index3 + 0.21 vrep.simxSetObjectPosition( clientID, sensor_data[x, y], -1, (x1, y1, z1), vrep.simx_opmode_oneshot) time.sleep(1) #save mapdata to file arr = save(arr) data_string = np.ndarray.dumps(arr) print data_string fo = open('./Mapdata/' + scene_name + '.txt', "w") fo.write(data_string) fo.close() return arr