def initialize(): global clientID vrep.simxFinish(-1) clientID = vrep.simxStart('127.0.0.1', 19997, True, True, 5000, 5) # Connect to V-REP if clientID != -1: print('Connected to remote API server') res, v1 = vrep.simxGetObjectHandle(clientID, 'Quadricopter', vrep.simx_opmode_oneshot_wait) print(res) rc, sv = vrep.simxGetFloatSignal(clientID, 'particlesTargetVelocities', vrep.simx_opmode_oneshot_wait) print(rc, sv) # enable the synchronous mode on the client: vrep.simxSynchronous(clientID, True) # load the quadroter model vrep.simxLoadModel(clientID, "C:\quadrotor.ttm", 0, vrep.simx_opmode_blocking) # start the simulation: vrep.simxStartSimulation(clientID, vrep.simx_opmode_blocking) #functional/handle code: # with Listener( # on_press=on_press, # on_release=on_release) as listener: # listener.join() return clientID else: print('Not Connected') sys.exit('Not Connected') return -1
def reset_robot_pos(self): #reset robot position to origin returnCode = vrep.simxRemoveModel(self.clientID, self.robot_handle, vrep.simx_opmode_oneshot_wait) #print("Removing robot, robot handler:",self.robot_handle," | return code:",returnCode) returnCode, self.robot_handle = vrep.simxGetObjectHandle( self.clientID, 'turtlebot2i', vrep.simx_opmode_oneshot_wait) while (returnCode == 0): returnCode = vrep.simxRemoveModel(self.clientID, self.robot_handle, vrep.simx_opmode_oneshot_wait) rospy.loginfo( "Previous removal failed. Remove robot again, robot handler:", self.robot_handle, " | return code:", returnCode) returnCode, self.robot_handle = vrep.simxGetObjectHandle( self.clientID, 'turtlebot2i', vrep.simx_opmode_oneshot_wait) returnCode, self.robot_handle = vrep.simxLoadModel( self.clientID, self.model_location, 0, vrep.simx_opmode_oneshot_wait) #print("Loading robot, robot handler:",self.robot_handle," | return code:",returnCode) while (returnCode != 0): returnCode, self.robot_handle = vrep.simxLoadModel( self.clientID, self.model_location, 0, vrep.simx_opmode_oneshot_wait) rospy.loginfo( "Previous loading failed. Reload robot. robot handler:", self.robot_handle, " | return code:", returnCode)
def add_robot(self, model): """Add new robot to the scene""" postfix = self.__get_next_postfix() vrep.simxLoadModel(self.client_id, MODELS_DIR + model, 0xFF, BLOCKING_MODE) new_youbot = YouBot(self.client_id, postfix) self.robots.append(new_youbot)
def render_chair(self, table, position, offset): x, chair = vrep.simxLoadModel(self.clientID, self.models_path + "dining chair.ttm", 0, vrep.simx_opmode_blocking) x = vrep.simxSetObjectParent(self.clientID, chair, table, True, vrep.simx_opmode_blocking) offset_z = math.fabs( vrep.simxGetObjectFloatParameter( self.clientID, chair, vrep.sim_objfloatparam_modelbbox_min_z, vrep.simx_opmode_blocking)[1]) vrep.simxSetObjectPosition( self.clientID, chair, -1, (position[0] + offset[0], position[1] + offset[1], position[2] + offset_z + offset[2]), vrep.simx_opmode_oneshot) vrep.simxSetObjectOrientation(self.clientID, chair, -1, (offset[3], offset[4], offset[5]), vrep.simx_opmode_oneshot) #returnCode, prop = vrep.simxGetModelProperty(self.clientID,chair,vrep.simx_opmode_blocking) # print (prop, prop + vrep.sim_modelproperty_not_respondable + vrep.sim_modelproperty_not_collidable) #returnCode2 = vrep.simxSetModelProperty(self.clientID, chair, prop + vrep.sim_modelproperty_not_respondable, vrep.simx_opmode_blocking) #returnCode3, prop = vrep.simxGetModelProperty(self.clientID,chair,vrep.simx_opmode_blocking) # print (returnCode, returnCode2, returnCode3, prop) # vrep.simxSetModelProperty(self.clientID,chair,vrep.sim_modelproperty_not_collidable,vrep.simx_opmode_oneshot) return chair
def seat( self, data, chair ): #Seats person at desired chair. In VREP, switches model from a standing bill to a sitting bill data.bills.remove(self.billID) data.billID = getBillID(data.bills) self.type = "Seated" self.location = chair.location self.vrepOrientation = [0, 0, chair.vrepOrientation[2] - math.pi / 2] if self.vrepID != None: vrep.simxRemoveModel(clientID, self.vrepID, vrep.simx_opmode_oneshot) self.vrepPos = [ chair.vrepPos[0], chair.vrepPos[1] - math.cos(chair.vrepOrientation[2]) * -0.05, 0 ] error, self.vrepID = vrep.simxLoadModel( clientID, "/home/steelshot/vrep/models/people/IK Bill.ttm", 0, vrep.simx_opmode_blocking) vrep.simxSetObjectPosition( clientID, self.vrepID, -1, (self.vrepPos[0], self.vrepPos[1], self.vrepPos[2] - 0.4), vrep.simx_opmode_oneshot) vrep.simxSetObjectOrientation(clientID, self.vrepID, -1, self.vrepOrientation, vrep.simx_opmode_oneshot) error, self.rightLegID = vrep.simxGetObjectHandle( clientID, 'Bill_rightLegJoint' + getBillString(self.billID), vrep.simx_opmode_oneshot_wait) error, rLeg = vrep.simxGetJointPosition(clientID, self.rightLegID, vrep.simx_opmode_streaming) error, self.leftLegID = vrep.simxGetObjectHandle( clientID, 'Bill_leftLegJoint' + getBillString(self.billID), vrep.simx_opmode_oneshot_wait) error, self.rightKneeID = vrep.simxGetObjectHandle( clientID, 'Bill_rightKneeJoint' + getBillString(self.billID), vrep.simx_opmode_oneshot_wait) error, self.leftKneeID = vrep.simxGetObjectHandle( clientID, 'Bill_leftKneeJoint' + getBillString(self.billID), vrep.simx_opmode_oneshot_wait) error, self.rightAnkleID = vrep.simxGetObjectHandle( clientID, 'Bill_rightAnkleJoint' + getBillString(self.billID), vrep.simx_opmode_oneshot_wait) error, self.leftAnkleID = vrep.simxGetObjectHandle( clientID, 'Bill_leftAnkleJoint' + getBillString(self.billID), vrep.simx_opmode_oneshot_wait) error, self.neckID = vrep.simxGetObjectHandle( clientID, 'Bill_neck' + getBillString(self.billID), vrep.simx_opmode_oneshot_wait) error, self.rightArmID = vrep.simxGetObjectHandle( clientID, 'Bill_rightShoulderJoint' + getBillString(self.billID), vrep.simx_opmode_oneshot_wait) error, self.leftArmID = vrep.simxGetObjectHandle( clientID, 'Bill_leftShoulderJoint' + getBillString(self.billID), vrep.simx_opmode_oneshot_wait) error, self.rightElbowID = vrep.simxGetObjectHandle( clientID, 'Bill_rightElbowJoint' + getBillString(self.billID), vrep.simx_opmode_oneshot_wait) error, self.leftElbowID = vrep.simxGetObjectHandle( clientID, 'Bill_leftElbowJoint' + getBillString(self.billID), vrep.simx_opmode_oneshot_wait)
def render_human(self, chair, position, offset): x, person = vrep.simxLoadModel(self.clientID, self.models_path + "Sitting Bill.ttm", 0, vrep.simx_opmode_blocking) x = vrep.simxSetObjectParent(self.clientID, person, chair, True, vrep.simx_opmode_blocking) offset_z = vrep.simxGetObjectFloatParameter( self.clientID, person, vrep.sim_objfloatparam_modelbbox_max_z, vrep.simx_opmode_blocking)[1] vrep.simxSetObjectPosition(self.clientID, person, -1, (position[0] + offset[0], position[1] + offset[1], position[2] + offset[2]), vrep.simx_opmode_oneshot) vrep.simxSetObjectOrientation(self.clientID, person, -1, (offset[3], offset[4], offset[5]), vrep.simx_opmode_oneshot) # vrep.simxSetModelProperty(self.clientID,person,vrep.sim_modelproperty_not_collidable,vrep.simx_opmode_oneshot) # x, objs = vrep.simxGetObjects(self.clientID, vrep.sim_handle_all, vrep.simx_opmode_oneshot_wait) # x, joint = vrep.simxGetObjectHandle(self.clientID, 'Bill_leftElbowJoint', vrep.simx_opmode_oneshot_wait) # x, shoulder = vrep.simxGetObjectHandle(self.clientID, 'Bill_leftShoulderJoint', vrep.simx_opmode_oneshot_wait) # rotationMatrix = (-1,0,0,0,0,-1,0,0,0,0,-1,0) # x = vrep.simxSetSphericalJointMatrix(self.clientID, shoulder, rotationMatrix,vrep.simx_opmode_streaming) #Moving Rotational Joint # ang = -math.pi/2 # x = vrep.simxSetJointPosition(self.clientID, joint, ang, vrep.simx_opmode_oneshot) # x, pos = vrep.simxGetJointPosition(self.clientID, joint, vrep.simx_opmode_streaming) return person
def check_robot_correctness(self): returnCode, self.robot_handle = vrep.simxGetObjectHandle(self.clientID, 'turtlebot2i', vrep.simx_opmode_oneshot_wait) while(returnCode != 0): rospy.loginfo("the exact 'turtlebot2i' is not found! Try to delete all possible robot and then load again.") self.remove_all_turtlebot2i() returnCode, self.robot_handle = vrep.simxLoadModel(self.clientID, self.model_location, 0, vrep.simx_opmode_oneshot_wait) time.sleep(10) returnCode, self.robot_handle = vrep.simxGetObjectHandle(self.clientID, 'turtlebot2i', vrep.simx_opmode_oneshot_wait)
def reset(self): """ reset the state retruns: the current state after reset """ reset_object(self.client_id, self.target_reset) vrep.simxRemoveModel(self.client_id, self.ref_frame, vrep.simx_opmode_oneshot_wait) vrep.simxLoadModel( self.client_id, "/home/user/V-REP/models/robots/mobile/pioneer_p3dx_script_disabled.ttm", 1, vrep.simx_opmode_oneshot_wait) self._load_robot_handles() self.vleft = 0 self.vright = 0 return self.get_state().to_array()
def __init__(self, position): self.position = position x, self.robot = vrep.simxLoadModel(clientID, address + "Sample Chef Bot.ttm", 0, vrep.simx_opmode_blocking) vrep.simxSetObjectPosition( clientID, self.robot, -1, (self.position[0], self.position[1], self.position[2]), vrep.simx_opmode_oneshot)
def loadVREPModel(clientID, path_to_model): ret, base_handle = vrep.simxLoadModel(clientID, path_to_model, False, blocking) if ret != return_ok: raise Exception('FAIL: Model failed to load') #else: #print ('SUCCESS: Model loaded') return base_handle
def load_model(path, file_on_server=True): ''' clientID: the client ID. refer to simxStart. path: the model filename, including the path and extension ("ttm"). The file is relative to the client or server system depending on the options value (see next argument) options: options, bit-coded: bit0 set: the specified file is located on the client side (in that case the function will be blocking since the model first has to be transferred to the server). Otherwise it is located on the server side operationMode: a remote API function operation mode. Recommended operation mode for this function is simx_opmode_blocking ''' res, handle = vrep.simxLoadModel(clientID, path, not file_on_server, vrep.simx_opmode_blocking) __check_resp(res, vrep.simx_return_ok) return res, handle
def reset(self): _, robot = vrep.simxGetObjectHandle(self.clientID, 'Pioneer_p3dx', vrep.simx_opmode_oneshot_wait) # vrep.simxResetDynamicObject(self.clientID,robot) vrep.simxRemoveModel(self.clientID, robot, vrep.simx_opmode_oneshot_wait) # vrep.simxStopSimulation(self.clientID,vrep.simx_opmode_oneshot) # a,b=vrep.simxLoadModel(self.clientID, '/home/rip/Downloads/V-REP_PRO_EDU_V3_4_0_Linux/models/Custom_model/Pioneer_p3dx.ttm',0, vrep.simx_opmode_blocking) a, b = vrep.simxLoadModel( self.clientID, '/home/rip/BTP/V-REP_PRO_EDU_V3_4_0_Linux/models/Custom_model/Poineer_p3dx_3_sensors.ttm', 0, vrep.simx_opmode_blocking) # _,prop = vrep.simxSetModelProperty(self.clientID,robot, vrep.sim_modelproperty_scripts_inactive,vrep.simx_opmode_oneshot) # # errorCode=vrep.simxSetJointTargetVelocity(self.clientID,self.left_motor_handle,0, vrep.simx_opmode_streaming) # errorCode=vrep.simxSetJointTargetVelocity(self.clientID,self.right_motor_handle,0, vrep.simx_opmode_streaming) # time.sleep(4) errorCode, self.left_motor_handle = vrep.simxGetObjectHandle( self.clientID, 'Pioneer_p3dx_leftMotor', vrep.simx_opmode_oneshot_wait) errorCode, self.right_motor_handle = vrep.simxGetObjectHandle( self.clientID, 'Pioneer_p3dx_rightMotor', vrep.simx_opmode_oneshot_wait) # Preallocaation self.sensor_h = [] #empty list for handles sensor_val = np.array([]) #empty array for sensor measurements #for loop to retrieve sensor arrays and initiate sensors for x in range(1, 16 + 1): errorCode, sensor_handle = vrep.simxGetObjectHandle( self.clientID, 'Pioneer_p3dx_ultrasonicSensor' + str(x), vrep.simx_opmode_oneshot_wait) self.sensor_h.append(sensor_handle) #keep list of handles errorCode, detectionState, detectedPoint, detectedObjectHandle, detectedSurfaceNormalVector = vrep.simxReadProximitySensor( self.clientID, sensor_handle, vrep.simx_opmode_streaming) sensor_val = np.append( sensor_val, np.linalg.norm(detectedPoint)) #get list of values print(sensor_val) # vrep.simxSetObjectPosition(self.clientID,robot,-1,[-1.4945,0.77500,0.13879],vrep.simx_opmode_oneshot_wait) # vrep.simxSetJointPosition(self.clientID,self.left_motor_handle,-1,vrep.simx_opmode_oneshot) # vrep.simxSetJointPosition(self.clientID,self.right_motor_handle,-1,vrep.simx_opmode_oneshot) time.sleep(0.2) # vrep.simxStartSimulation(self.clientID,vrep.simx_opmode_oneshot) state, done = self.discretize_observation(3) return state
def createVrep(self, pos, orientation): error, self.vrepID = vrep.simxLoadModel( clientID, "/home/steelshot/vrep/models/furniture/chairs/dining chair.ttm", 0, vrep.simx_opmode_blocking) self.vrepPos = pos self.vrepOrientation = orientation vrep.simxSetObjectPosition(clientID, self.vrepID, -1, (pos[0], pos[1], pos[2]), vrep.simx_opmode_oneshot) vrep.simxSetObjectOrientation(clientID, self.vrepID, -1, orientation, vrep.simx_opmode_oneshot)
def render(self, table, sim_env): position = [ table.get_feature("x").value - env_margin, table.get_feature("y").value - env_margin, 0 ] x, sim_table = vrep.simxLoadModel(self.clientID, self.models_path + "diningTable.ttm", 0, vrep.simx_opmode_blocking) # x = vrep.simxSetObjectParent(self.clientID, sim_table, sim_env, True, vrep.simx_opmode_blocking) vrep.simxSetObjectPosition(self.clientID, sim_table, -1, (position[0], position[1], position[2] + \ vrep.simxGetObjectFloatParameter (self.clientID, sim_table, vrep.sim_objfloatparam_modelbbox_max_x, vrep.simx_opmode_blocking)[1]), vrep.simx_opmode_oneshot) # vrep.simxSetModelProperty(self.clientID,sim_table,vrep.sim_modelproperty_not_collidable,vrep.simx_opmode_oneshot) #returnCode, prop = vrep.simxGetModelProperty(self.clientID,sim_table,vrep.simx_opmode_blocking) #returnCode2 = vrep.simxSetModelProperty(self.clientID, sim_table, prop + vrep.sim_modelproperty_not_respondable, vrep.simx_opmode_blocking) #returnCode3, prop = vrep.simxGetModelProperty(self.clientID, sim_table, vrep.simx_opmode_blocking) #print (prop) offset_z = math.fabs( vrep.simxGetObjectFloatParameter( self.clientID, sim_table, vrep.sim_objfloatparam_modelbbox_min_z, vrep.simx_opmode_blocking)[1]) offset_y = math.fabs( vrep.simxGetObjectFloatParameter( self.clientID, sim_table, vrep.sim_objfloatparam_modelbbox_min_y, vrep.simx_opmode_blocking)[1]) margin = 0.2 bill_margin = 0.05 self.chairs_offset = [(0, offset_y + margin, 0, 0, 0, 0), (0, -offset_y - margin, 0, 0, 0, math.pi), (offset_z + margin, 0, 0, 0, 0, -math.pi / 2), (-offset_z - margin, 0, 0, 0, 0, math.pi / 2)] self.humans_offset = [ (0, offset_y - margin, bill_margin, 0, 0, -math.pi / 2), (0, -offset_y + margin, bill_margin, 0, 0, math.pi / 2), (offset_z - margin, 0, bill_margin, 0, 0, math.pi), (-offset_z + margin, 0, bill_margin, 0, 0, 0) ] for i in range(len(table.humans)): sim_chair = self.render_chair(sim_table, position, self.chairs_offset[i]) self.chairs.append(sim_chair) self.humans.append( self.render_human(sim_chair, position, self.humans_offset[i])) self.sim_table = sim_table return sim_table
def createVrep(self, pos, orientation): #Create waiter model error, self.vrepID = vrep.simxLoadModel( clientID, "/home/steelshot/vrep/models/people/Walking Bill Chef Bot.ttm", 0, vrep.simx_opmode_blocking) self.type = "Walking" error, self.moveBoxID = vrep.simxGetObjectHandle( clientID, "Bill_goalDummy" + getBillString(self.billID), vrep.simx_opmode_oneshot_wait) self.vrepPos = pos self.vrepOrientation = orientation vrep.simxSetObjectPosition(clientID, self.vrepID, -1, (pos[0], pos[1], pos[2]), vrep.simx_opmode_oneshot) vrep.simxSetObjectPosition(clientID, self.moveBoxID, -1, (pos[0], pos[1], pos[2]), vrep.simx_opmode_oneshot) vrep.simxSetObjectOrientation(clientID, self.vrepID, -1, orientation, vrep.simx_opmode_oneshot) error, self.rightLegID = vrep.simxGetObjectHandle( clientID, 'Bill_rightLegJoint' + getBillString(self.billID), vrep.simx_opmode_oneshot_wait) error, self.leftLegID = vrep.simxGetObjectHandle( clientID, 'Bill_leftLegJoint' + getBillString(self.billID), vrep.simx_opmode_oneshot_wait) error, self.rightKneeID = vrep.simxGetObjectHandle( clientID, 'Bill_rightKneeJoint' + getBillString(self.billID), vrep.simx_opmode_oneshot_wait) error, self.leftKneeID = vrep.simxGetObjectHandle( clientID, 'Bill_leftKneeJoint' + getBillString(self.billID), vrep.simx_opmode_oneshot_wait) error, self.rightAnkleID = vrep.simxGetObjectHandle( clientID, 'Bill_rightAnkleJoint' + getBillString(self.billID), vrep.simx_opmode_oneshot_wait) error, self.leftAnkleID = vrep.simxGetObjectHandle( clientID, 'Bill_leftAnkleJoint' + getBillString(self.billID), vrep.simx_opmode_oneshot_wait) error, self.neckID = vrep.simxGetObjectHandle( clientID, 'Bill_neck' + getBillString(self.billID), vrep.simx_opmode_oneshot_wait) error, self.rightArmID = vrep.simxGetObjectHandle( clientID, 'Bill_rightShoulderJoint' + getBillString(self.billID), vrep.simx_opmode_oneshot_wait) error, self.leftArmID = vrep.simxGetObjectHandle( clientID, 'Bill_leftShoulderJoint' + getBillString(self.billID), vrep.simx_opmode_oneshot_wait) error, self.rightElbowID = vrep.simxGetObjectHandle( clientID, 'Bill_rightElbowJoint' + getBillString(self.billID), vrep.simx_opmode_oneshot_wait) error, self.leftElbowID = vrep.simxGetObjectHandle( clientID, 'Bill_leftElbowJoint' + getBillString(self.billID), vrep.simx_opmode_oneshot_wait)
def _reset(self): self.resetFlag = vrep.simxRemoveModel( clientID=self.clientID, objectHandle=self.robotObj, operationMode=vrep.simx_opmode_streaming) #_wait # print('Close Flag:', self.resetFlag) time.sleep(2.0) self.resetFlag, self.robotObj = vrep.simxLoadModel( clientID=self.clientID, modelPathAndName= '/Users/xuanliu/Documents/V-REP_PRO_EDU_V3_4_0_Mac/models/robots/mobile/ACM-R5-disabled.ttm', operationMode=vrep.simx_opmode_oneshot_wait, # _wait options=0)
def initialize_env(self): # close any open connections vrep.simxFinish(-1) # Connect to the V-REP continuous server self.clientID = vrep.simxStart('127.0.0.1', 19997, True, True, 500, 5) if self.clientID != -1: # if we connected successfully print('Connected to remote API server') err_model = vrep.simxLoadModel(self.clientID, self.modelPath, False, self.block_mode) if err_model != vrep.simx_return_ok: print("Error is ", err_model) if err_model == (0, 16): print('Taking some more time') time.sleep(10)
def analyze_model(clientID, model_path_and_name, qty_poses=9, save_location='imgs/models/'): """ Takes several qty_poses 'pictures' of a VREP model in different orientations. Saves each of the pictures in save_location. Each of the picture will be SIFT-described for instance detection. clientID is the VREP scene ID. qty_poses must be divisible by 3 """ assert qty_poses % 3 == 0, 'qty_poses must be divisible by 3' # finding model name model_name = '' for i in model_path_and_name: if i == '/': model_name = '' elif i == '.': break else: model_name += i # creating models directory, if not existent if model_name not in os.listdir(save_location): os.mkdir(save_location + '/' + model_name) # loading model and positioning res, model_handle = vrep.simxLoadModel(clientID, modelPathAndName=model_path_and_name, options=1, operationMode=vrep.simx_opmode_oneshot_wait) res = vrep.simxSetObjectPosition(clientID, model_handle, -1, (-0.8, 0, 5), vrep.simx_opmode_oneshot) count = 0 # grabbing camera2 image, saving it and rotating model if clientID!=-1: res,v1=vrep.simxGetObjectHandle(clientID,'camera2',vrep.simx_opmode_oneshot_wait) res,resolution,image=vrep.simxGetVisionSensorImage(clientID,v1,0,vrep.simx_opmode_streaming) while (vrep.simxGetConnectionId(clientID)!=-1) and count < qty_poses: cv2.waitKey(200) res,resolution,image=vrep.simxGetVisionSensorImage(clientID,v1,0,vrep.simx_opmode_buffer) if res==vrep.simx_return_ok: #cv2.waitKey(200) #img = img2rgbnp(image, resolution) #cv2.imshow('foo', img) #cv2.waitKey(50) # Cases for count: # when count < qty_poses / 3, poses are obtained rotating x axis # when count >= qty_poses / 3 and < qty_poses*2/3, poses are obtained rotating y axis # else, poses are obtained rotating z axis """ if count < qty_poses / 3: cv2.waitKey(200) res = vrep.simxSetObjectOrientation(clientID, model_handle,\ #model_handle, (360*3/qty_poses, 0, 0), vrep.simx_opmode_oneshot) model_handle, (2*np.pi*3/qty_poses, 0, 0), vrep.simx_opmode_oneshot) cv2.waitKey(200) elif count < (qty_poses*2/3): cv2.waitKey(200) res = vrep.simxSetObjectOrientation(clientID, model_handle,\ model_handle, (0, 2*np.pi*3/qty_poses, 0), vrep.simx_opmode_oneshot) cv2.waitKey(200) else: cv2.waitKey(200) res = vrep.simxSetObjectOrientation(clientID, model_handle,\ model_handle, (0, 0, 2*np.pi*3/qty_poses), vrep.simx_opmode_oneshot) cv2.waitKey(200) """ cv2.waitKey(200) res = vrep.simxSetObjectOrientation(clientID, model_handle,\ model_handle, (0, 2*np.pi/qty_poses, 0), vrep.simx_opmode_oneshot) cv2.waitKey(200) #else: # break cv2.waitKey(200) res,resolution,image=vrep.simxGetVisionSensorImage(clientID,v1,0,vrep.simx_opmode_buffer) img = img2rgbnp(image, resolution) cv2.imwrite(save_location +'/' + model_name + '/' + model_name + '_' + str(count) + '.png', img) count += 1 ret = vrep.simxRemoveModel(clientID, model_handle, vrep.simx_opmode_oneshot)
import numpy as np import matplotlib.pyplot as plt import math PI = math.pi # Establish Communication vrep.simxFinish(-1) # just in case, close all opened connections clientID = vrep.simxStart('127.0.0.1', 19998, True, True, 5000, 5) _, robot = vrep.simxGetObjectHandle(clientID, 'Pioneer_p3dx', vrep.simx_opmode_oneshot_wait) # vrep.simxResetDynamicObject(self.clientID,robot) vrep.simxRemoveModel(clientID, robot, vrep.simx_opmode_oneshot_wait) a, b = vrep.simxLoadModel( clientID, '/home/rip/BTP/V-REP_PRO_EDU_V3_4_0_Linux/models/Custom_model/Pioneer_p3dx.ttm', 0, vrep.simx_opmode_blocking) if clientID != -1: #check if client connection successful print('Connected to remote API server') else: print('Connection not successful') sys.exit('Could not connect') errorCode, left_motor_handle = vrep.simxGetObjectHandle( clientID, 'Pioneer_p3dx_leftMotor', vrep.simx_opmode_oneshot_wait) errorCode, right_motor_handle = vrep.simxGetObjectHandle( clientID, 'Pioneer_p3dx_rightMotor', vrep.simx_opmode_oneshot_wait) errorCode, robot = vrep.simxGetObjectHandle(clientID, 'Pioneer_p3dx', vrep.simx_opmode_oneshot_wait)
def learning(): frames = 100000 observation = int(frames/100) instances = 5 epsilon = 1 batchSize = 60 buffer = 500 replay = [] gamma = 0.9 bufferCount = 0 sensorInformation = np.array([]) #capturing the current state returnCode,collisionState=vrep.simxReadCollision(clientID,cH,vrep.simx_opmode_streaming) for x in range(1,16+1): errorCodeProximity3,detectionState,detectedPoint,detectedObjectHandle,detectedSurfaceNormalVector=vrep.simxReadProximitySensor(clientID,sensor_h[x-1],vrep.simx_opmode_streaming) for x in range(1,16+1): errorCodeProximity3,detectionState,detectedPoint,detectedObjectHandle,detectedSurfaceNormalVector=vrep.simxReadProximitySensor(clientID,sensor_h[x-1],vrep.simx_opmode_buffer) sensorInformation=np.append(sensorInformation,np.linalg.norm(detectedPoint)) #get list of values #sensorInformation=np.append(sensorInformation,detectedPoint[2]) print(detectedPoint) state=sensorInformation t = time.time() # no iter for i in range(frames): noOfCollisions=0 #sensorInformation = np.array([]) #check for randomness or for intially observation before training begins if (random.random() < epsilon) or (i < observation): action = random.randint(0,4) else: qVal = model.predict(state) action = np.argmax(qVal) new_state = makeMove(state,action) reward = rewardFunction(state,action) replay.append((state,action,reward,new_state)) if i > observation: if len(replay) > buffer: replay.pop(0) #!!!!!check for batchSize, it may not be correct minibatch = random.sample(replay,batchSize) Xtrain , ytrain = mini_batch_processing(minibatch,model) #============================================================================== # for j in range(batchSize): # print("j is ",j) # #============================================================================== for i in range(batchSize): model.fit(Xtrain[i], ytrain[i], batch_size = 1, epochs = 1 , verbose = 1 ) state = new_state if epsilon > 0.1 and i > observation: epsilon -= (1/frames) bufferCount+=1 print("\n\n\n\n\nGame no: " ,bufferCount) print(state) k="" with open('stateFile.txt', 'a') as the_file: for i in range(len(state)): k=k+","+str(state[i]) the_file.write(k+"\n") #write to file time.sleep(0.02) returnCode,collisionState=vrep.simxReadCollision(clientID,cH,vrep.simx_opmode_buffer) print("Collison State: " , collisionState) #if np.amin(state) >= 0.7 and bufferCount>10: if collisionState==True: sys.exit("HERE: ") vrep.simxRemoveModel(clientID,modelHandle,vrep.simx_opmode_oneshot) vrep.simxLoadModel(clientID,modelHandle,0,vrep.simx_opmode_blocking)
def randDrawTable(c): x, table = vrep.simxLoadModel( clientID, "/home/steelshot/vrep/models/furniture/tables/customizable table.ttm", 0, vrep.simx_opmode_blocking) x, chair1 = vrep.simxLoadModel( clientID, "/home/steelshot/vrep/models/furniture/chairs/dining chair.ttm", 0, vrep.simx_opmode_blocking) x, chair2 = vrep.simxLoadModel( clientID, "/home/steelshot/vrep/models/furniture/chairs/dining chair.ttm", 0, vrep.simx_opmode_blocking) x, spoon1 = vrep.simxLoadModel( clientID, "/home/steelshot/vrep/models/household/spoon.ttm", 0, vrep.simx_opmode_blocking) x, fork1 = vrep.simxLoadModel( clientID, "/home/steelshot/vrep/models/household/fork.ttm", 0, vrep.simx_opmode_blocking) x, pos = vrep.simxGetObjectPosition(clientID, spoon1, -1, vrep.simx_opmode_streaming) x, spoon2 = vrep.simxLoadModel( clientID, "/home/steelshot/vrep/models/household/spoon.ttm", 0, vrep.simx_opmode_blocking) x, fork2 = vrep.simxLoadModel( clientID, "/home/steelshot/vrep/models/household/fork.ttm", 0, vrep.simx_opmode_blocking) x, plate2 = vrep.simxLoadModel( clientID, "/home/steelshot/vrep/models/household/plate.ttm", 0, vrep.simx_opmode_blocking) x, plate1 = vrep.simxLoadModel( clientID, "/home/steelshot/vrep/models/household/plate.ttm", 0, vrep.simx_opmode_blocking) x, plate1Pos = vrep.simxGetObjectPosition(clientID, plate1, -1, vrep.simx_opmode_streaming) vrep.simxSetObjectPosition(clientID, table, -1, (c[0], c[1], c[2] + 0.65), vrep.simx_opmode_oneshot) vrep.simxSetObjectPosition(clientID, plate2, -1, (c[0], c[1] + 0.27, c[2] + 0.72), vrep.simx_opmode_oneshot) vrep.simxSetObjectPosition(clientID, plate1, -1, (c[0], c[1] - 0.27, c[2] + 0.72), vrep.simx_opmode_oneshot) vrep.simxSetObjectPosition(clientID, spoon1, -1, (c[0] + 0.2, c[1] - 0.27, c[2] + 0.72), vrep.simx_opmode_oneshot) vrep.simxSetObjectPosition(clientID, fork1, -1, (c[0] - 0.2, c[1] - 0.27, c[2] + 0.72), vrep.simx_opmode_oneshot) vrep.simxSetObjectPosition(clientID, spoon2, -1, (c[0] - 0.2, c[1] + 0.27, c[2] + 0.72), vrep.simx_opmode_oneshot) vrep.simxSetObjectPosition(clientID, fork2, -1, (c[0] + 0.2, c[1] + 0.27, c[2] + 0.72), vrep.simx_opmode_oneshot) vrep.simxSetObjectOrientation(clientID, spoon2, -1, (math.pi / 2, 0, math.pi / 2), vrep.simx_opmode_oneshot) vrep.simxSetObjectOrientation(clientID, fork2, -1, (math.pi / 2, 0, math.pi / 2), vrep.simx_opmode_oneshot) vrep.simxSetObjectOrientation(clientID, chair2, -1, (0, 0, math.pi), vrep.simx_opmode_oneshot) vrep.simxSetObjectPosition(clientID, chair2, -1, (c[0], c[1] - 0.82, c[2] + 0.45), vrep.simx_opmode_oneshot) vrep.simxSetObjectPosition(clientID, chair1, -1, (c[0], c[1] + 0.82, c[2] + 0.45), vrep.simx_opmode_oneshot) #x, person1 = vrep.simxLoadModel(clientID, "/home/steelshot/vrep/models/people/Sitting Bill.ttm", 0,vrep.simx_opmode_blocking) #vrep.simxSetObjectPosition(clientID, person1, -1, (c[0], c[1] + 0.5, c[2]), vrep.simx_opmode_oneshot)\ #print(pos) r = 2 #print(r) if r == 0: x, person1 = vrep.simxLoadModel( clientID, "/home/steelshot/vrep/models/people/IK Bill.ttm", 0, vrep.simx_opmode_blocking) vrep.simxSetObjectPosition(clientID, person1, -1, (c[0], c[1] + 0.45, c[2]), vrep.simx_opmode_oneshot) elif r == 1: x, person2 = vrep.simxLoadModel( clientID, "/home/steelshot/vrep/models/people/IK Bill.ttm", 0, vrep.simx_opmode_blocking) vrep.simxSetObjectPosition(clientID, person2, -1, (c[0], c[1] - 0.45, c[2]), vrep.simx_opmode_oneshot) vrep.simxSetObjectOrientation(clientID, person2, -1, (0, 0, math.pi / 2), vrep.simx_opmode_oneshot) else: x, person2 = vrep.simxLoadModel( clientID, "/home/steelshot/vrep/models/people/IK Bill.ttm", 0, vrep.simx_opmode_blocking) vrep.simxSetObjectPosition(clientID, person2, -1, (c[0], c[1] - 0.45, c[2]), vrep.simx_opmode_oneshot) vrep.simxSetObjectOrientation(clientID, person2, -1, (0, 0, math.pi / 2), vrep.simx_opmode_oneshot) x, person1 = vrep.simxLoadModel( clientID, "/home/steelshot/vrep/models/people/IK Bill.ttm", 0, vrep.simx_opmode_blocking) vrep.simxSetObjectPosition(clientID, person1, -1, (c[0], c[1] + 0.45, c[2]), vrep.simx_opmode_oneshot) time.sleep(40) x, pos = vrep.simxGetObjectPosition(clientID, spoon1, -1, vrep.simx_opmode_buffer) time.sleep(1) x, plate1Pos = vrep.simxGetObjectPosition(clientID, plate1, -1, vrep.simx_opmode_buffer) print(plate1Pos) print(pos) x, rightArm = vrep.simxGetObjectHandle(clientID, 'rightArm', vrep.simx_opmode_oneshot_wait) vrep.simxSetObjectPosition(clientID, rightArm, -1, pos, vrep.simx_opmode_oneshot) time.sleep(2) finalPos = [0, (-0.15) - 0.45, 0.95] diff = [finalPos[0] - pos[0], finalPos[1] - pos[1], finalPos[2] - pos[2]] for i in range(1, 10): diff = [ finalPos[0] - pos[0], finalPos[1] - pos[1], finalPos[2] - pos[2] ] finalPos = [ 0 + random.uniform(-0.0025, -0.0025), (-0.155) - 0.45 + random.uniform(-0.0025, -0.0025), 1.1 + random.uniform(-0.0025, -0.0025) ] diff = [ finalPos[0] - pos[0], finalPos[1] - pos[1], finalPos[2] - pos[2] ] for i in range(1, 100): #vrep.simxSetObjectOrientation(clientID,rightArm,-1,(math.radians(-48)*(i/100),math.radians(-17)*(i/100),math.radians(78)*(i/100)),vrep.simx_opmode_oneshot) vrep.simxSetObjectPosition(clientID, rightArm, -1, [ pos[0] + diff[0] * (i / 100), pos[1] + diff[1] * (i / 100), pos[2] + diff[2] * (i / 100) ], vrep.simx_opmode_oneshot) time.sleep(1 / 30) finalPos2 = [ plate1Pos[0] + random.uniform(-0.07, 0.07), plate1Pos[1] + random.uniform(-0.07, 0.07), plate1Pos[2] ] diff2 = [ finalPos2[0] - finalPos[0], finalPos2[1] - finalPos[1], finalPos2[2] - finalPos[2] ] for i in range(1, 100): #vrep.simxSetObjectOrientation(clientID,rightArm,-1,(math.radians(-48)*(i/100),math.radians(-17)*(i/100),math.radians(78)*(i/100)),vrep.simx_opmode_oneshot) vrep.simxSetObjectPosition(clientID, rightArm, -1, [ finalPos[0] + diff2[0] * (i / 100), finalPos[1] + diff2[1] * (i / 100), finalPos[2] + diff2[2] * (i / 100) ], vrep.simx_opmode_oneshot) time.sleep(1 / 30) pos = finalPos2[:]
def randDrawTable(c): x, table = vrep.simxLoadModel( clientID, "/home/steelshot/vrep/models/furniture/tables/customizable table.ttm", 0, vrep.simx_opmode_blocking) x, chair1 = vrep.simxLoadModel( clientID, "/home/steelshot/vrep/models/furniture/chairs/dining chair.ttm", 0, vrep.simx_opmode_blocking) x, chair2 = vrep.simxLoadModel( clientID, "/home/steelshot/vrep/models/furniture/chairs/dining chair.ttm", 0, vrep.simx_opmode_blocking) x, spoon1 = vrep.simxLoadModel( clientID, "/home/steelshot/vrep/models/household/spoon.ttm", 0, vrep.simx_opmode_blocking) x, fork1 = vrep.simxLoadModel( clientID, "/home/steelshot/vrep/models/household/fork.ttm", 0, vrep.simx_opmode_blocking) x, pos = vrep.simxGetObjectPosition(clientID, spoon1, -1, vrep.simx_opmode_streaming) x, spoon2 = vrep.simxLoadModel( clientID, "/home/steelshot/vrep/models/household/spoon.ttm", 0, vrep.simx_opmode_blocking) x, fork2 = vrep.simxLoadModel( clientID, "/home/steelshot/vrep/models/household/fork.ttm", 0, vrep.simx_opmode_blocking) x, plate2 = vrep.simxLoadModel( clientID, "/home/steelshot/vrep/models/household/plate.ttm", 0, vrep.simx_opmode_blocking) x, plate1 = vrep.simxLoadModel( clientID, "/home/steelshot/vrep/models/household/plate.ttm", 0, vrep.simx_opmode_blocking) vrep.simxSetObjectPosition(clientID, table, -1, (c[0], c[1], c[2] + 0.65), vrep.simx_opmode_oneshot) vrep.simxSetObjectPosition(clientID, plate2, -1, (c[0], c[1] + 0.27, c[2] + 0.72), vrep.simx_opmode_oneshot) vrep.simxSetObjectPosition(clientID, plate1, -1, (c[0], c[1] - 0.27, c[2] + 0.72), vrep.simx_opmode_oneshot) vrep.simxSetObjectPosition(clientID, spoon1, -1, (c[0] + 0.2, c[1] - 0.27, c[2] + 0.72), vrep.simx_opmode_oneshot) vrep.simxSetObjectPosition(clientID, fork1, -1, (c[0] - 0.2, c[1] - 0.27, c[2] + 0.72), vrep.simx_opmode_oneshot) vrep.simxSetObjectPosition(clientID, spoon2, -1, (c[0] - 0.2, c[1] + 0.27, c[2] + 0.72), vrep.simx_opmode_oneshot) vrep.simxSetObjectPosition(clientID, fork2, -1, (c[0] + 0.2, c[1] + 0.27, c[2] + 0.72), vrep.simx_opmode_oneshot) vrep.simxSetObjectOrientation(clientID, spoon2, -1, (math.pi / 2, 0, math.pi / 2), vrep.simx_opmode_oneshot) vrep.simxSetObjectOrientation(clientID, fork2, -1, (math.pi / 2, 0, math.pi / 2), vrep.simx_opmode_oneshot) vrep.simxSetObjectOrientation(clientID, chair2, -1, (0, 0, math.pi), vrep.simx_opmode_oneshot) vrep.simxSetObjectPosition(clientID, chair2, -1, (c[0], c[1] - 0.82, c[2] + 0.45), vrep.simx_opmode_oneshot) vrep.simxSetObjectPosition(clientID, chair1, -1, (c[0], c[1] + 0.82, c[2] + 0.45), vrep.simx_opmode_oneshot) r = random.randint(0, 2) if r == 0: x, person1 = vrep.simxLoadModel( clientID, "/home/steelshot/vrep/models/people/IK Bill.ttm", 0, vrep.simx_opmode_blocking) vrep.simxSetObjectPosition(clientID, person1, -1, (c[0], c[1] + 0.45, c[2]), vrep.simx_opmode_oneshot) elif r == 1: x, person2 = vrep.simxLoadModel( clientID, "/home/steelshot/vrep/models/people/IK Bill.ttm", 0, vrep.simx_opmode_blocking) vrep.simxSetObjectPosition(clientID, person2, -1, (c[0], c[1] - 0.45, c[2]), vrep.simx_opmode_oneshot) vrep.simxSetObjectOrientation(clientID, person2, -1, (0, 0, math.pi / 2), vrep.simx_opmode_oneshot) else: x, person2 = vrep.simxLoadModel( clientID, "/home/steelshot/vrep/models/people/IK Bill.ttm", 0, vrep.simx_opmode_blocking) vrep.simxSetObjectPosition(clientID, person2, -1, (c[0], c[1] - 0.45, c[2]), vrep.simx_opmode_oneshot) vrep.simxSetObjectOrientation(clientID, person2, -1, (0, 0, math.pi / 2), vrep.simx_opmode_oneshot) x, person1 = vrep.simxLoadModel( clientID, "/home/steelshot/vrep/models/people/IK Bill.ttm", 0, vrep.simx_opmode_blocking) vrep.simxSetObjectPosition(clientID, person1, -1, (c[0], c[1] + 0.45, c[2]), vrep.simx_opmode_oneshot)
if res != vrep.simx_return_ok: print("ERROR : Cannot remove object (" + str(res) + ")") res, floor_id = vrep.simxGetObjectHandle(client_id, "ResizableFloor_5_25_visibleElement", vrep.simx_opmode_blocking) if res != vrep.simx_return_ok: print("ERROR : Cannot get handle (" + str(res) + ")") res = vrep.simxRemoveObject(client_id, floor_id, vrep.simx_opmode_blocking) if res != vrep.simx_return_ok: print("ERROR : Cannot remove object (" + str(res) + ")") # Add sim models res, ceres_id = vrep.simxLoadModel(client_id, os.path.join(MODEL_PATH, "ceres.ttm"), 0, vrep.simx_opmode_blocking) if res != vrep.simx_return_ok: print("ERROR : Cannot load model (" + str(res) + ")") res, table_id = vrep.simxLoadModel(client_id, os.path.join(MODEL_PATH, "table.ttm"), 0, vrep.simx_opmode_blocking) if res != vrep.simx_return_ok: print("ERROR : Cannot load model (" + str(res) + ")") # Get Camera res, cam_id = vrep.simxGetObjectHandle(client_id, "Vision_sensor", vrep.simx_opmode_blocking) if res != vrep.simx_return_ok: print("ERROR : Cannot get handle (" + str(res) + ")")
def LoadModel(self, modelPathAndName): return vrep.simxLoadModel(self.clientId, modelPathAndName, 0, vrep.simx_opmode_blocking)
def __init__(self, size, position, people=0): #setting variables self.size = size self.position = position self.people = people #generating table- table handle stored in self.table x, self.table = vrep.simxLoadModel(clientID, address + "diningTable.ttm", 0, vrep.simx_opmode_blocking) vrep.simxSetObjectPosition( clientID, self.table, -1, (self.position[0], self.position[1], self.position[2] + 0.38), vrep.simx_opmode_oneshot) #generating chairs and cutlery #chair 1 x, self.chair1 = vrep.simxLoadModel(clientID, address + "dining chair.ttm", 0, vrep.simx_opmode_blocking) vrep.simxSetObjectPosition(clientID, self.chair1, -1, (self.position[0], self.position[1] + 0.85, self.position[2] + 0.45), vrep.simx_opmode_oneshot) x, self.plate1 = vrep.simxLoadModel(clientID, address + "plate.ttm", 0, vrep.simx_opmode_blocking) vrep.simxSetObjectPosition(clientID, self.plate1, -1, (self.position[0], self.position[1] - 0.3, self.position[2] + 0.77), vrep.simx_opmode_oneshot) x, self.spoon1 = vrep.simxLoadModel(clientID, address + "spoon.ttm", 0, vrep.simx_opmode_blocking) vrep.simxSetObjectPosition(clientID, self.spoon1, -1, (self.position[0] + 0.2, self.position[1] - 0.3, self.position[2] + 0.77), vrep.simx_opmode_oneshot) x, self.fork1 = vrep.simxLoadModel(clientID, address + "fork.ttm", 0, vrep.simx_opmode_blocking) vrep.simxSetObjectPosition(clientID, self.fork1, -1, (self.position[0] - 0.2, self.position[1] - 0.3, self.position[2] + 0.77), vrep.simx_opmode_oneshot) #chair 2 x, self.chair2 = vrep.simxLoadModel(clientID, address + "dining chair.ttm", 0, vrep.simx_opmode_blocking) vrep.simxSetObjectPosition(clientID, self.chair2, -1, (self.position[0], self.position[1] - 0.85, self.position[2] + 0.45), vrep.simx_opmode_oneshot) vrep.simxSetObjectOrientation(clientID, self.chair2, -1, (0, 0, math.pi), vrep.simx_opmode_oneshot) x, self.plate2 = vrep.simxLoadModel(clientID, address + "plate.ttm", 0, vrep.simx_opmode_blocking) vrep.simxSetObjectPosition(clientID, self.plate2, -1, (self.position[0], self.position[1] + 0.3, self.position[2] + 0.77), vrep.simx_opmode_oneshot) x, self.spoon2 = vrep.simxLoadModel(clientID, address + "spoon.ttm", 0, vrep.simx_opmode_blocking) vrep.simxSetObjectPosition(clientID, self.spoon2, -1, (self.position[0] - 0.2, self.position[1] + 0.3, self.position[2] + 0.77), vrep.simx_opmode_oneshot) vrep.simxSetObjectOrientation(clientID, self.spoon2, -1, (math.pi / 2, 0, math.pi / 2), vrep.simx_opmode_oneshot) x, self.fork2 = vrep.simxLoadModel(clientID, address + "fork.ttm", 0, vrep.simx_opmode_blocking) vrep.simxSetObjectPosition(clientID, self.fork2, -1,(self.position[0] + 0.2, self.position[1] + 0.3, self.position[2] + 0.77),\ vrep.simx_opmode_oneshot) vrep.simxSetObjectOrientation(clientID, self.fork2, -1, (math.pi / 2, 0, math.pi / 2), vrep.simx_opmode_oneshot) if self.size == 4: #chair 3 x, self.chair3 = vrep.simxLoadModel(clientID, address + "dining chair.ttm", 0, vrep.simx_opmode_blocking) vrep.simxSetObjectPosition(clientID, self.chair3, -1, (self.position[0] + 1, self.position[1], self.position[2] + 0.45), vrep.simx_opmode_oneshot) vrep.simxSetObjectOrientation(clientID, self.chair3, -1, (0, 0, -math.pi / 2), vrep.simx_opmode_oneshot) x, self.plate3 = vrep.simxLoadModel(clientID, address + "plate.ttm", 0, vrep.simx_opmode_blocking) vrep.simxSetObjectPosition( clientID, self.plate3, -1, (self.position[0] + 0.6, self.position[1], self.position[2] + 0.77), vrep.simx_opmode_oneshot) x, self.spoon3 = vrep.simxLoadModel(clientID, address + "spoon.ttm", 0, vrep.simx_opmode_blocking) vrep.simxSetObjectPosition( clientID, self.spoon3, -1, (self.position[0] + 0.6, self.position[1] + 0.2, self.position[2] + 0.77), vrep.simx_opmode_oneshot) vrep.simxSetObjectOrientation( clientID, self.spoon3, -1, (math.pi / 2, -math.pi / 2, math.pi / 2), vrep.simx_opmode_oneshot) x, self.fork3 = vrep.simxLoadModel(clientID, address + "fork.ttm", 0, vrep.simx_opmode_blocking) vrep.simxSetObjectPosition(clientID, self.fork3, -1, (self.position[0] +0.6, self.position[1] - 0.2, self.position[2] + 0.77), \ vrep.simx_opmode_oneshot) vrep.simxSetObjectOrientation( clientID, self.fork3, -1, (math.pi / 2, -math.pi / 2, math.pi / 2), vrep.simx_opmode_oneshot) #chair 4 x, self.chair4 = vrep.simxLoadModel(clientID, address + "dining chair.ttm", 0, vrep.simx_opmode_blocking) vrep.simxSetObjectPosition(clientID, self.chair4, -1, (self.position[0] - 1, self.position[1], self.position[2] + 0.45), vrep.simx_opmode_oneshot) vrep.simxSetObjectOrientation(clientID, self.chair4, -1, (0, 0, math.pi / 2), vrep.simx_opmode_oneshot) x, self.plate4 = vrep.simxLoadModel(clientID, address + "plate.ttm", 0, vrep.simx_opmode_blocking) vrep.simxSetObjectPosition( clientID, self.plate4, -1, (self.position[0] - 0.6, self.position[1], self.position[2] + 0.77), vrep.simx_opmode_oneshot) x, self.spoon4 = vrep.simxLoadModel(clientID, address + "spoon.ttm", 0, vrep.simx_opmode_blocking) vrep.simxSetObjectPosition( clientID, self.spoon4, -1, (self.position[0] - 0.6, self.position[1] - 0.2, self.position[2] + 0.77), vrep.simx_opmode_oneshot) vrep.simxSetObjectOrientation( clientID, self.spoon4, -1, (math.pi / 2, math.pi / 2, math.pi / 2), vrep.simx_opmode_oneshot) x, self.fork4 = vrep.simxLoadModel(clientID, address + "fork.ttm", 0, vrep.simx_opmode_blocking) vrep.simxSetObjectPosition(clientID, self.fork4, -1, (self.position[0] - 0.6, self.position[1] + 0.2, self.position[2] + 0.77), \ vrep.simx_opmode_oneshot) vrep.simxSetObjectOrientation( clientID, self.fork4, -1, (math.pi / 2, math.pi / 2, math.pi / 2), vrep.simx_opmode_oneshot) #generating people if self.people == 0: pass elif size == 2 and self.people == 1: r = random.randint(0, 1) if r == 0: x, self.person1 = vrep.simxLoadModel( clientID, address + "Sitting Bill.ttm", 0, vrep.simx_opmode_blocking) vrep.simxSetObjectPosition( clientID, self.person1, -1, (self.position[0], self.position[1] + 0.5, self.position[2]), vrep.simx_opmode_oneshot) if r == 1: x, self.person1 = vrep.simxLoadModel( clientID, address + "Sitting Bill.ttm", 0, vrep.simx_opmode_blocking) vrep.simxSetObjectPosition( clientID, self.person1, -1, (self.position[0], self.position[1] - 0.5, self.position[2]), vrep.simx_opmode_oneshot) vrep.simxSetObjectOrientation(clientID, self.person1, -1, (0, 0, math.pi / 2), vrep.simx_opmode_oneshot) elif size == 2 and self.people == 2: x, self.person2 = vrep.simxLoadModel(clientID, address + "Sitting Bill.ttm", 0, vrep.simx_opmode_blocking) vrep.simxSetObjectPosition( clientID, self.person2, -1, (self.position[0], self.position[1] - 0.5, self.position[2]), vrep.simx_opmode_oneshot) vrep.simxSetObjectOrientation(clientID, self.person2, -1, (0, 0, math.pi / 2), vrep.simx_opmode_oneshot) x, self.person1 = vrep.simxLoadModel(clientID, address + "Sitting Bill.ttm", 0, vrep.simx_opmode_blocking) vrep.simxSetObjectPosition( clientID, self.person1, -1, (self.position[0], self.position[1] + 0.5, self.position[2]), vrep.simx_opmode_oneshot) elif size == 4: choices = [1, 2, 3, 4] for i in range(self.people): r = random.choice(choices) choices.remove(r) if r == 1: x, self.person1 = vrep.simxLoadModel( clientID, address + "Sitting Bill.ttm", 0, vrep.simx_opmode_blocking) vrep.simxSetObjectPosition( clientID, self.person1, -1, (self.position[0], self.position[1] + 0.5, self.position[2]), vrep.simx_opmode_oneshot) if r == 2: x, self.person2 = vrep.simxLoadModel( clientID, address + "Sitting Bill.ttm", 0, vrep.simx_opmode_blocking) vrep.simxSetObjectPosition( clientID, self.person2, -1, (self.position[0], self.position[1] - 0.5, self.position[2]), vrep.simx_opmode_oneshot) vrep.simxSetObjectOrientation(clientID, self.person2, -1, (0, 0, math.pi / 2), vrep.simx_opmode_oneshot) if r == 3: x, self.person3 = vrep.simxLoadModel( clientID, address + "Sitting Bill.ttm", 0, vrep.simx_opmode_blocking) vrep.simxSetObjectPosition( clientID, self.person3, -1, (self.position[0] - 0.6, self.position[1], self.position[2]), vrep.simx_opmode_oneshot) vrep.simxSetObjectOrientation(clientID, self.person3, -1, (0, 0, 0), vrep.simx_opmode_oneshot) if r == 4: x, self.person4 = vrep.simxLoadModel( clientID, address + "Sitting Bill.ttm", 0, vrep.simx_opmode_blocking) vrep.simxSetObjectPosition( clientID, self.person4, -1, (self.position[0] + 0.6, self.position[1], self.position[2]), vrep.simx_opmode_oneshot) vrep.simxSetObjectOrientation(clientID, self.person4, -1, (0, 0, math.pi), vrep.simx_opmode_oneshot)
def makeFile(walls, obstacles, thermal, visual, startPos, uiWindow=None): '''Create a file data string from the positions and scales''' try: returnCode, handler = vrep.simxGetObjectHandle( clientID=clientID, objectName="ResizableFloor_5_25", operationMode=vrep.simx_opmode_blocking) vrep.simxRemoveModel(clientID=clientID, objectHandle=handler, operationMode=vrep.simx_opmode_oneshot) except: print("Can not remove the ResizableFloor_5_25") #Strings to hold the tile parts allTiles = "" #Strings to hold the boundaries for special tiles allCheckpointBounds = "" allTrapBounds = "" allGoalBounds = "" allSwampBounds = "" #Upper left corner to start placing tiles from width = len(walls[0]) height = len(walls) startX = -(len(walls[0]) * 0.25 / 2.0) startZ = -(len(walls) * 0.25 / 2.0) #Id numbers used to give a unique but interable name to tile pieces tileId = 0 checkId = 0 trapId = 0 goalId = 0 swampId = 0 #Iterate through all the tiles for x in range(0, len(walls[0])): for z in range(0, len(walls)): #Check which corners and external walls and notches are needed corners = checkForCorners([x, z], walls) externals = checkForExternalWalls([x, z], walls) notchData = checkForNotch([x, z], walls) notch = "" #Set notch string to correct value if notchData[0]: notch = "left" if notchData[1]: notch = "right" #tile # print( "V-REP add tile") isTile = False #wall if walls[z][x][1][0]: #top # print( "V-REP add top wall") resetCode, obj = vrep.simxLoadModel( clientID=clientID, modelPathAndName=model_path + '/models/walls/' + wall_color + 'top_wall.ttm', options=0, operationMode=vrep.simx_opmode_blocking) returnCode, position = vrep.simxGetObjectPosition( clientID=clientID, objectHandle=obj, relativeToObjectHandle=-1, operationMode=vrep.simx_opmode_blocking) pos_z = position[2] vrep.simxSetObjectPosition( clientID=clientID, objectHandle=obj, relativeToObjectHandle=-1, position=((x * 0.25 + startX), -1 * (z * 0.25 + startZ) + 0.125, pos_z), operationMode=vrep.simx_opmode_oneshot) if walls[z][x][1][1]: #right # print( "V-REP add right wall") resetCode, obj = vrep.simxLoadModel( clientID=clientID, modelPathAndName=model_path + '/models/walls/' + wall_color + 'right_wall.ttm', options=0, operationMode=vrep.simx_opmode_blocking) returnCode, position = vrep.simxGetObjectPosition( clientID=clientID, objectHandle=obj, relativeToObjectHandle=-1, operationMode=vrep.simx_opmode_blocking) pos_z = position[2] vrep.simxSetObjectPosition( clientID=clientID, objectHandle=obj, relativeToObjectHandle=-1, position=((x * 0.25 + startX) + 0.25, -1 * (z * 0.25 + startZ) + 0.125, pos_z), operationMode=vrep.simx_opmode_oneshot) if walls[z][x][1][2]: #bottom # print( "V-REP add bottom wall") resetCode, obj = vrep.simxLoadModel( clientID=clientID, modelPathAndName=model_path + '/models/walls/' + wall_color + 'bottom_wall.ttm', options=0, operationMode=vrep.simx_opmode_blocking) returnCode, position = vrep.simxGetObjectPosition( clientID=clientID, objectHandle=obj, relativeToObjectHandle=-1, operationMode=vrep.simx_opmode_blocking) pos_z = position[2] vrep.simxSetObjectPosition( clientID=clientID, objectHandle=obj, relativeToObjectHandle=-1, position=((x * 0.25 + startX) + 0.125, -1 * (z * 0.25 + startZ) - 0.25, pos_z), operationMode=vrep.simx_opmode_oneshot) if walls[z][x][1][3]: #left # print( "V-REP add left wall") resetCode, obj = vrep.simxLoadModel( clientID=clientID, modelPathAndName=model_path + '/models/walls/' + wall_color + 'left_wall.ttm', options=0, operationMode=vrep.simx_opmode_blocking) returnCode, position = vrep.simxGetObjectPosition( clientID=clientID, objectHandle=obj, relativeToObjectHandle=-1, operationMode=vrep.simx_opmode_blocking) pos_z = position[2] vrep.simxSetObjectPosition( clientID=clientID, objectHandle=obj, relativeToObjectHandle=-1, position=((x * 0.25 + startX), -1 * (z * 0.25 + startZ) + 0.125, pos_z), operationMode=vrep.simx_opmode_oneshot) #checkpoint if walls[z][x][2]: #Add bounds to the checkpoint boundaries print("V-REP add checkpoint") resetCode, obj = vrep.simxLoadModel( clientID=clientID, modelPathAndName=model_path + '/models/tiles/checkpoint_tile.ttm', options=0, operationMode=vrep.simx_opmode_blocking) returnCode, position = vrep.simxGetObjectPosition( clientID=clientID, objectHandle=obj, relativeToObjectHandle=-1, operationMode=vrep.simx_opmode_blocking) pos_z = position[2] vrep.simxSetObjectPosition( clientID=clientID, objectHandle=obj, relativeToObjectHandle=-1, position=((x * 0.25 + startX), -1 * (z * 0.25 + startZ), pos_z), operationMode=vrep.simx_opmode_oneshot) # allCheckpointBounds = allCheckpointBounds + boundsPart.format("checkpoint", checkId, (x * 0.25 + startX) - 0.15, (z * 0.25 + startZ) - 0.15, (x * 0.25 + startX) + 0.15, (z * 0.25 + startZ) + 0.15) #Increment id counter checkId = checkId + 1 isTile = True #trap if walls[z][x][3]: #Add bounds to the trap boundaries print("V-REP add trap / hole tile") resetCode, obj = vrep.simxLoadModel( clientID=clientID, modelPathAndName=model_path + '/models/tiles/hole_tile.ttm', options=0, operationMode=vrep.simx_opmode_blocking) returnCode, position = vrep.simxGetObjectPosition( clientID=clientID, objectHandle=obj, relativeToObjectHandle=-1, operationMode=vrep.simx_opmode_blocking) pos_z = position[2] vrep.simxSetObjectPosition( clientID=clientID, objectHandle=obj, relativeToObjectHandle=-1, position=((x * 0.25 + startX), -1 * (z * 0.25 + startZ), pos_z), operationMode=vrep.simx_opmode_oneshot) # allTrapBounds = allTrapBounds + boundsPart.format("trap", trapId, (x * 0.25 + startX) - 0.15, (z * 0.25 + startZ) - 0.15, (x * 0.25 + startX) + 0.15, (z * 0.25 + startZ) + 0.15) #Increment id counter trapId = trapId + 1 isTile = True #goal if walls[z][x][4]: #Add bounds to the goal boundaries print("V-REP add start tile") resetCode, obj = vrep.simxLoadModel( clientID=clientID, modelPathAndName=model_path + '/models/tiles/start_tile.ttm', options=0, operationMode=vrep.simx_opmode_blocking) returnCode, position = vrep.simxGetObjectPosition( clientID=clientID, objectHandle=obj, relativeToObjectHandle=-1, operationMode=vrep.simx_opmode_blocking) pos_z = position[2] vrep.simxSetObjectPosition( clientID=clientID, objectHandle=obj, relativeToObjectHandle=-1, position=((x * 0.25 + startX), -1 * (z * 0.25 + startZ), pos_z), operationMode=vrep.simx_opmode_oneshot) resetCode, obj_robot = vrep.simxLoadModel( clientID=clientID, modelPathAndName=model_path + '/models/robots/simplus_e-puck.ttm', options=0, operationMode=vrep.simx_opmode_blocking) returnCode, position = vrep.simxGetObjectPosition( clientID=clientID, objectHandle=obj_robot, relativeToObjectHandle=-1, operationMode=vrep.simx_opmode_blocking) pos_z = position[2] print("V-rep add robot") vrep.simxSetObjectPosition( clientID=clientID, objectHandle=obj_robot, relativeToObjectHandle=-1, position=((x * 0.25 + startX), -1 * (z * 0.25 + startZ), pos_z), operationMode=vrep.simx_opmode_oneshot) resetCode, obj = vrep.simxLoadModel( clientID=clientID, modelPathAndName=model_path + '/models/game_manager.ttm', options=0, operationMode=vrep.simx_opmode_blocking) returnCode, position = vrep.simxGetObjectPosition( clientID=clientID, objectHandle=obj, relativeToObjectHandle=-1, operationMode=vrep.simx_opmode_blocking) vrep.simxSetObjectPosition( clientID=clientID, objectHandle=obj, relativeToObjectHandle=-1, position=(20, 20, 0), operationMode=vrep.simx_opmode_oneshot) #Increment id counter goalId = goalId + 1 isTile = True #swamp if walls[z][x][5]: print("V-REP add swamp tile") resetCode, obj = vrep.simxLoadModel( clientID=clientID, modelPathAndName=model_path + '/models/tiles/speed_bump.ttm', options=0, operationMode=vrep.simx_opmode_blocking) returnCode, position = vrep.simxGetObjectPosition( clientID=clientID, objectHandle=obj, relativeToObjectHandle=-1, operationMode=vrep.simx_opmode_blocking) pos_z = position[2] vrep.simxSetObjectPosition( clientID=clientID, objectHandle=obj, relativeToObjectHandle=-1, position=((x * 0.25 + startX), -1 * (z * 0.25 + startZ), pos_z), operationMode=vrep.simx_opmode_oneshot) #Add bounds to the swamp boundaries # allSwampBounds = allSwampBounds + boundsPart.format("swamp", swampId, (x * 0.25 + startX) - 0.15, (z * 0.25 + startZ) - 0.15, (x * 0.25 + startX) + 0.15, (z * 0.25 + startZ) + 0.15) #Increment id counter swampId = swampId + 1 isTile = True # white tile if not isTile: resetCode, obj = vrep.simxLoadModel( clientID=clientID, modelPathAndName=model_path + '/models/tiles/tile_white.ttm', options=0, operationMode=vrep.simx_opmode_blocking) returnCode, position = vrep.simxGetObjectPosition( clientID=clientID, objectHandle=obj, relativeToObjectHandle=-1, operationMode=vrep.simx_opmode_blocking) pos_z = position[2] vrep.simxSetObjectPosition( clientID=clientID, objectHandle=obj, relativeToObjectHandle=-1, position=((x * 0.25 + startX), -1 * (z * 0.25 + startZ), pos_z), operationMode=vrep.simx_opmode_oneshot) #Increment id counter tileId = tileId + 1 #String to hold all the data for the obstacles allObstacles = "" allDebris = "" #Id to give a unique name to the obstacles obstacleId = 0 debrisId = 0 #Iterate obstalces for obstacle in obstacles: #If this is debris if obstacle[3]: #Add the debris object # allDebris = allDebris + debrisPart.format(debrisId, obstacle[0], obstacle[1], obstacle[2]) #Increment id counter debrisId = debrisId + 1 else: #Add the obstacle # allObstacles = allObstacles + obstaclePart.format(obstacleId, obstacle[0], obstacle[1], obstacle[2]) #Increment id counter obstacleId = obstacleId + 1 #Return the file data as a string return
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') # 1. First send a command to display a specific message in a dialog box: emptyBuff = bytearray() res, retInts, retFloats, retStrings, retBuffer = vrep.simxCallScriptFunction( clientID, 'remoteApiCommandServer', vrep.sim_scripttype_childscript, 'displayText_function', [], [], ['Hello world!'], emptyBuff, vrep.simx_opmode_blocking) vrep.simxLoadModel( clientID, "/home/lamy/Desktop/VREP/V-REP_PRO_EDU_V3_5_0_Linux/models/robots/non-mobile/UR5.ttm", 1, vrep.simx_opmode_blocking) if res == vrep.simx_return_ok: print('Return string: ', retStrings[0] ) # display the reply from V-REP (in this case, just a string) else: print('Remote function call failed') # 2. Now create a dummy object at coordinate 0.1,0.2,0.3 with name 'MyDummyName': res, retInts, retFloats, retStrings, retBuffer = vrep.simxCallScriptFunction( clientID, 'remoteApiCommandServer', vrep.sim_scripttype_childscript, 'createDummy_function', [], [0.1, 0.2, 0.3], ['MyDummyName'], emptyBuff, vrep.simx_opmode_blocking) if res == vrep.simx_return_ok: print( 'Dummy handle: ', retInts[0]
return cgenome 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