selection action based on current perception using python
Posted: 14 Nov 2015, 22:08
HI,since i am new to vrep and python and to the starters I am able to avoid collision to obstacles but unable to implement an agent that has memory (sensor readings and action taken in the past) and using the position of the nearest block (from ‘getNearestConcretBlockDist(...)’ ) to approach the block and collect it. so can someone help me with the implementation
And here is my code in python
And here is my code in python
Code: Select all
import vrep, math, time, env, numpy as np #array library
# get distance to obstacle detected by a given sensor
def getObstacleDist(sensorHandler_):
dist2Obstacle_LR = [0.0, 0.0]
# Get raw sensor readings using API
rawSR = vrep.simxReadProximitySensor(clientID, sensorHandler_, vrep.simx_opmode_oneshot_wait)
print(rawSR)
# Calculate Euclidean distance
if rawSR[1]: # if true, obstacle is within detection range, return distance to obstacle
return math.sqrt(rawSR[2][0]*rawSR[2][0] + rawSR[2][1]*rawSR[2][1] + rawSR[2][2]*rawSR[2][2])
else: # if false, obstacle out of detection range, return inf.
return float('inf')
# agent's reasoning procedure, using IF-THEN rules
def getMotorSpeed(dist):
# You can implement your navigation strategy here
return motorSpeed
# execute agent's action
def execute(motorSpeed):
vrep.simxSetJointTargetVelocity(clientID, leftMotorHandle, motorSpeed['speedLeft'], vrep.simx_opmode_oneshot )
vrep.simxSetJointTargetVelocity(clientID, rightMotorHandle, motorSpeed['speedRight'], vrep.simx_opmode_oneshot )
print 'Program started'
vrep.simxFinish(-1) # just in case, close all opened connections
int_portNb = 19999 # define port_nr
clientID = vrep.simxStart( '127.0.0.1', int_portNb, True, True, 5000, 5) # connect to server
if clientID != -1:
print 'Connected to remote API server'
res,objs = vrep.simxGetObjects(clientID,vrep.sim_handle_all,vrep.simx_opmode_oneshot_wait) # get all objects in the scene
if res == vrep.simx_return_ok: # Remote function call succeeded
print 'Number of objects in the scene: ',len(objs)# print number of object in the scene
ret_lm, leftMotorHandle = vrep.simxGetObjectHandle(clientID, 'Pioneer_p3dx_leftMotor', vrep.simx_opmode_oneshot_wait)
ret_rm, rightMotorHandle = vrep.simxGetObjectHandle(clientID, 'Pioneer_p3dx_rightMotor', vrep.simx_opmode_oneshot_wait)
ret_pr, pioneerRobotHandle = vrep.simxGetObjectHandle(clientID, 'Pioneer_p3dx', vrep.simx_opmode_oneshot_wait)
ret_sl, ultraSonicSensorLeft = vrep.simxGetObjectHandle(clientID, 'Pioneer_p3dx_ultrasonicSensor' + str(3),vrep.simx_opmode_oneshot_wait)
ret_sr, ultraSonicSensorRight = vrep.simxGetObjectHandle(clientID, 'Pioneer_p3dx_ultrasonicSensor' + str(5),vrep.simx_opmode_oneshot_wait)
blocksArray = env.getConcretBlockHandle(clientID)
env.initConcretBlockPosition(clientID, blocksArray) # initialize position of the blocks
while True: # main Control loop
#######################################################
# Perception Phase: Get information about environment #
#######################################################
# get distance to nearest block
dist2NearestBlock, blockId, pioneer2BlockPos = env.getNearestConcretBlockDist(clientID, blocksArray, pioneerRobotHandle)
print('dist2nearestBlock', blockId, dist2NearestBlock)
print('Pos2nearestBlock', pioneer2BlockPos)
# get distance to obstacle, return [distLeft, distRight]
dist2Obstacle_LR = [getObstacleDist(ultraSonicSensorLeft), getObstacleDist(ultraSonicSensorRight)]
print('dist2obstacle', dist2Obstacle_LR)
##############################################
# Reasoning: figure out which action to take #
##############################################
# get motor speed using IF-THEN rule
if getObstacleDist(ultraSonicSensorLeft) < 1:
motorSpeed = dict(speedLeft=-2, speedRight=-1)
elif getObstacleDist(ultraSonicSensorRight) < 0.7:
motorSpeed = dict(speedLeft=-2, speedRight=-1)
else:
motorSpeed = dict(speedLeft=2, speedRight=2)
#motorSpeed = getMotorSpeed(dist2Obstacle_LR)
########################################
# Action Phase: Assign speed to wheels #
########################################
# assign speed to the wheels
execute(motorSpeed)
# collect nearest block within a short range
env.collectNearestBlock(clientID, blocksArray, pioneerRobotHandle)
else:
print 'Remote API function call returned with error code: ',res
vrep.simxFinish(clientID) # close all opened connections
else:
print 'Failed connecting to remote API server'
print 'Program finished'