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
