def get_maze_segments(wallHandles):
## Get handles to:
# 1. The Maze collection:
res, mazeHandle = vrep.simxGetCollectionHandle(clientID, "Maze",
vrep.simx_opmode_blocking)
# Get the handles associated with each wall and the absolute position of the center of each wall:
res, wallHandles, intData, absPositions, stringData = vrep.simxGetObjectGroupData(
clientID, mazeHandle, 3, vrep.simx_opmode_blocking)
mazeSegments = []
if res == vrep.simx_return_ok:
count = 1
for wall in wallHandles:
res, wallCenterAbsPos = vrep.simxGetObjectPosition(
clientID, wall, -1, vrep.simx_opmode_oneshot_wait)
res, wallMinY = vrep.simxGetObjectFloatParameter(
clientID, wall, vrep.sim_objfloatparam_objbbox_min_y,
vrep.simx_opmode_oneshot_wait)
res, wallMaxY = vrep.simxGetObjectFloatParameter(
clientID, wall, vrep.sim_objfloatparam_objbbox_max_y,
vrep.simx_opmode_oneshot_wait)
wallLength = abs(wallMaxY - wallMinY)
# Get the orientation of the wall: Third euler angle is the angle around z-axis:
res, wallOrient = vrep.simxGetObjectOrientation(
clientID, wall, -1, vrep.simx_opmode_oneshot_wait)
# Get the end points of the maze wall: A list containing two tuples: [ (xs,ys) , (xe,ye)]
wallSeg = get_wall_seg(
wallCenterAbsPos, wallLength, wallOrient[2]
) # Assuming all walls are on the ground and nearly flat:
print("Wall #", count, " -> ", wallSeg)
mazeSegments.append(wallSeg)
count += 1
else:
print(" Failed to get individual wall handles!")
return mazeSegments
def _get_handle(self):
"""Retrieve collection handle."""
if not self._name:
raise RuntimeError("Could not retrieve handle to {}: missing name."
"".format(EMPTY_NAME))
client_id = self.client_id
if client_id is None:
raise ConnectionError(
"Could not retrieve handle to {}: not connected to V-REP "
"remote API server.".format(self._name))
res, handle = vrep.simxGetCollectionHandle(client_id, self._name,
vrep.simx_opmode_blocking)
if res != vrep.simx_return_ok:
raise ServerError("Could not retrieve handle to {}.".format(
self._name))
return handle
for i in range(0,100):
print("i = " + str(i))
_, dhandle = vrep.simxGetObjectHandle(clientID,"Dummy"+str(100+i),vrep.simx_opmode_blocking)
_ = vrep.simxSetObjectPosition(clientID,dhandle,-1,[0,i*0.5,0],vrep.simx_opmode_streaming)
_ = vrep.simxSetObjectOrientation(clientID,dhandle,-1,[0,0,math.pi/2],vrep.simx_opmode_oneshot)
'''
'''
for theta in np.arange(0,2*math.pi,0.5/R):
_, dhandle = vrep.simxCreateDummy(clientID, 0.1, None, vrep.simx_opmode_blocking)
_ = vrep.simxSetObjectParent(clientID, dhandle, dsethandle, True, vrep.simx_opmode_blocking)
x = R*math.cos(theta)
y = R*math.sin(theta)
_ = vrep.simxSetObjectPosition(clientID, dhandle, -1, [x, y, 0], vrep.simx_opmode_oneshot)
'''
_, colHandle = vrep.simxGetCollectionHandle(clientID, "Ref",
vrep.simx_opmode_blocking)
_, refHandle, intData, doubleData, strData = vrep.simxGetObjectGroupData(
clientID, colHandle, 3, vrep.simx_opmode_blocking)
refPos = np.reshape(doubleData, (len(refHandle), 3))
for index in range(0, len(refHandle) - 1):
curPos = refPos[index]
nextPos = refPos[index + 1]
relPos = nextPos - curPos
ang = math.atan2(relPos[1], relPos[0])
_ = vrep.simxSetObjectPosition(clientID, refHandle[index], -1,
[curPos[0], curPos[1], 0.1],
# else:
# print("Indicate following arguments: 'portNumber jointHandles'")
# time.sleep(5.0)
# sys.exit(0)
# get Handles
jointHandles = [0] * 6
for i in range(0, 6):
returnCode, jointHandles[i] = vrep.simxGetObjectHandle(
clientID, 'Mico_joint' + str(i + 1), vrep.simx_opmode_blocking)
if i == 0: printlog('simxGetObjectHandle', returnCode)
returnCode, fingersH1 = vrep.simxGetObjectHandle(
clientID, 'MicoHand_fingers12_motor1', vrep.simx_opmode_blocking)
returnCode, fingersH2 = vrep.simxGetObjectHandle(
clientID, 'MicoHand_fingers12_motor2', vrep.simx_opmode_blocking)
returnCode, jointsCollectionHandle = vrep.simxGetCollectionHandle(
clientID, "sixJoints#", vrep.simx_opmode_blocking)
returnCode, distToGoalHandle = vrep.simxGetDistanceHandle(
clientID, "distanceToGoal#", vrep.simx_opmode_blocking)
returnCode, distanceToGoal = vrep.simxReadDistance(
clientID, distToGoalHandle,
vrep.simx_opmode_streaming) #start streaming
print('jointHandles', jointHandles)
#Arm
pi = np.pi
vel = 1 # looks fast in simulation!
#some test target positions for the 6 joints, in radians
targetPos0 = np.zeros(6)
targetPosPi = np.array(
[pi] * 6) #default initial state: pi or 180 degrees for all joints
#Destino
codErro, target = vrep.simxGetObjectHandle(
clientID, 'target', operationMode=vrep.simx_opmode_oneshot)
#Medidores de colisão
codErro, pioneerColDetect = vrep.simxGetCollisionHandle(
clientID, 'pioneerColDetect', operationMode=vrep.simx_opmode_blocking)
#Medidores de distância
codErro, roboObstDist = vrep.simxGetDistanceHandle(
clientID, 'roboObstDistDetect', operationMode=vrep.simx_opmode_blocking)
codErro, roboTargetDist = vrep.simxGetDistanceHandle(
clientID, 'roboTargetDistDetect', operationMode=vrep.simx_opmode_blocking)
codErro, obstaculos = vrep.simxGetCollectionHandle(
clientID, 'obstaculos#', operationMode=vrep.simx_opmode_oneshot)
codErro, robo = vrep.simxGetObjectHandle(
clientID, 'Pionee_p3dx', operationMode=vrep.simx_opmode_oneshot)
codErro, motorE = vrep.simxGetObjectHandle(clientID, 'Pioneer_p3dx_leftMotor',
vrep.simx_opmode_blocking)
codErro, motorD = vrep.simxGetObjectHandle(clientID, 'Pioneer_p3dx_rightMotor',
vrep.simx_opmode_blocking)
codErro, colisao = vrep.simxReadCollision(
clientID, pioneerColDetect, operationMode=vrep.simx_opmode_streaming)
posXR = vrep.simxGetFloatSignal(clientID, 'posXR', vrep.simx_opmode_streaming)
posYR = vrep.simxGetFloatSignal(clientID, 'posYR', vrep.simx_opmode_streaming)
posX1 = vrep.simxGetFloatSignal(clientID, 'posX1', vrep.simx_opmode_streaming)
def __enter__(self):
print('[ROBOTENV] Starting environment...')
sync_mode_str = 'TRUE' if self.sync_mode else 'FALSE'
portNb_str = str(self.portNb)
# launch v-rep
vrep_cmd = [
self.vrepPath, '-gREMOTEAPISERVERSERVICE_' + portNb_str +
'_FALSE_' + sync_mode_str
]
if not self.showGUI:
vrep_cmd.append('-h') # headless mode
vrep_cmd.append(self.scenePath)
# headless mode via ssh
# vrep_cmd = "xvfb-run --auto-servernum --server-num=1 /homes/dam416/V-REP_PRO_EDU_V3_4_0_Linux/vrep.sh -h -s -q MicoRobot.ttt"
# vrep_cmd = ['xvfb-run', '--auto-servernum', '--server-num=1', self.vrepPath, '-h', '-s', '-q', self.scenePath]
# vrep_cmd = ['xvfb-run', '--auto-servernum', '--server-num=1', self.vrepPath, '-h', self.scenePath]
print('[ROBOTENV] Launching V-REP...')
# NOTE: do not use "stdout=subprocess.PIPE" below to buffer logs, causes deadlock at episode 464! (flushing the buffer may work... but buffering is not needed)
self.vrepProcess = subprocess.Popen(vrep_cmd,
shell=False,
preexec_fn=os.setsid)
# connect to V-Rep Remote Api Server
vrep.simxFinish(-1) # close all opened connections
# Connect to V-REP
print('[ROBOTENV] Connecting to V-REP...')
counter = 0
while True:
self.clientID = vrep.simxStart('127.0.0.1', self.portNb, True,
False, 5000, 0)
if self.clientID != -1:
break
else:
print("[ROBOTENV] Connection failed, retrying")
counter += 1
if counter == 10:
raise RuntimeError(
'[ROBOTENV] Connection to V-REP failed.')
if self.clientID == -1:
print('[ROBOTENV] Failed connecting to remote API server')
else:
print('[ROBOTENV] Connected to remote API server')
# close model browser and hierarchy window
vrep.simxSetBooleanParameter(self.clientID,
vrep.sim_boolparam_browser_visible,
False, vrep.simx_opmode_blocking)
vrep.simxSetBooleanParameter(self.clientID,
vrep.sim_boolparam_hierarchy_visible,
False, vrep.simx_opmode_blocking)
vrep.simxSetBooleanParameter(self.clientID,
vrep.sim_boolparam_console_visible,
False, vrep.simx_opmode_blocking)
# load scene
# time.sleep(5) # to avoid errors
# returnCode = vrep.simxLoadScene(self.clientID, self.scenePath, 1, vrep.simx_opmode_oneshot_wait) # vrep.simx_opmode_blocking is recommended
# # Start simulation
# # vrep.simxSetIntegerSignal(self.clientID, 'dummy', 1, vrep.simx_opmode_blocking)
# # time.sleep(5) #to center window for recordings
# if self.initial_joint_positions is not None:
# self.initializeJointPositions(self.initial_joint_positions)
# self.startSimulation()
# # returnCode = vrep.simxStartSimulation(self.clientID, vrep.simx_opmode_blocking)
# # printlog('simxStartSimulation', returnCode)
# get handles and start streaming distance to goal
for i in range(0, self.nSJoints):
returnCode, self.jointHandles[i] = vrep.simxGetObjectHandle(
self.clientID, 'Mico_joint' + str(i + 1),
vrep.simx_opmode_blocking)
printlog('simxGetObjectHandle', returnCode)
returnCode, self.fingersH1 = vrep.simxGetObjectHandle(
self.clientID, 'MicoHand_fingers12_motor1',
vrep.simx_opmode_blocking)
returnCode, self.fingersH2 = vrep.simxGetObjectHandle(
self.clientID, 'MicoHand_fingers12_motor2',
vrep.simx_opmode_blocking)
returnCode, self.goalCubeH = vrep.simxGetObjectHandle(
self.clientID, 'GoalCube', vrep.simx_opmode_blocking)
returnCode, self.targetCubePositionH = vrep.simxGetObjectHandle(
self.clientID, 'GoalPosition', vrep.simx_opmode_blocking)
returnCode, self.robotBaseH = vrep.simxGetObjectHandle(
self.clientID, 'Mico_link1_visible', vrep.simx_opmode_blocking)
returnCode, self.jointsCollectionHandle = vrep.simxGetCollectionHandle(
self.clientID, "sixJoints#", vrep.simx_opmode_blocking)
returnCode, self.distToGoalHandle = vrep.simxGetDistanceHandle(
self.clientID, "distanceToGoal#", vrep.simx_opmode_blocking)
returnCode, self.endEffectorH = vrep.simxGetObjectHandle(
self.clientID, "DummyFinger#", vrep.simx_opmode_blocking)
# returnCode, self.distanceToGoal = vrep.simxReadDistance(self.clientID, self.distToGoalHandle, vrep.simx_opmode_streaming) #start streaming
# returnCode, _, _, floatData, _ = vrep.simxGetObjectGroupData(self.clientID, self.jointsCollectionHandle, 15, vrep.simx_opmode_streaming) #start streaming
if self.targetCubePosition is not None:
# hide goal position plane
visibility_layer_param_ID = 10
visible_layer = 1
returnCode = vrep.simxSetObjectIntParameter(
self.clientID, self.targetCubePositionH,
visibility_layer_param_ID, visible_layer,
vrep.simx_opmode_blocking)
# print('simxSetObjectIntParameter return Code: ', returnCode)
self.initializeGoalPosition()
# Start simulation
if self.initial_joint_positions is not None:
self.initializeJointPositions()
self.reset()
# self.startSimulation()
# # returnCode = vrep.simxStartSimulation(self.clientID, vrep.simx_opmode_blocking)
# # printlog('simxStartSimulation', returnCode)
# self.updateState() # default initial state: 180 degrees (=pi radians) for all angles
# check the state is valid
# while True:
# self.updateState()
# print("\nstate: ", self.state)
# print("\nreward: ", self.reward)
# # wait for a state
# if (self.state.shape == (1,self.observation_space_size) and abs(self.reward) < 1E+5):
# print("sent reward3=", self.reward)
# break
print(
'[ROBOTENV] Environment succesfully initialised, ready for simulations'
)
# while vrep.simxGetConnectionId(self.clientID) != -1:
# ##########################EXECUTE ACTIONS AND UPDATE STATE HERE #################################
# time.sleep(0.1)
# #end of execution loop
return self