selection action based on current perception using python

[kushi91221]

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

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'   

[coppelia]

Hello,

your question is too broad and you are basically only giving us some code to look at. We usually only help with questions directly related to V-REP or how to use it. Algorithms is a too broad matter for this forum.

Cheers