how to simulate by using python remote api

Typically: "How do I... ", "How can I... " questions
Post Reply
vrep_arrow_forest
Posts: 2
Joined: 16 Jan 2019, 07:27

how to simulate by using python remote api

Post by vrep_arrow_forest » 17 Jan 2019, 17:15

Hi.

I open scene and set robot which is server in vrep .

It is shown in the following code.

Code: Select all

simExtRemoteApiStart(19999)
I prepared python file to run robot.
It is shown in the following code.

Code: Select all

# -*- coding: utf-8 -*-
import time
import os, sys
import ctypes
import math
import numpy

sys.path.append("/desktop/vrep")

############################# 
#############################
try:
    import vrep
except:
    print ('--------------------------------------------------------------')
    print ('"vrep.py" could not be imported. This means very probably that')
    print ('either "vrep.py" or the remoteApi library could not be found.')
    print ('Make sure both are in the same folder as this file,')
    print ('or appropriately adjust the file "vrep.py"')
    print ('--------------------------------------------------------------')
    print ('')

    
############################# robot #############################        
vrep.simxFinish(-1)
clientID=vrep.simxStart('127.0.0.1',19999,True,True,5000,5)

if clientID!=-1:
    print ('Connected to remote API server')
else:
    print ('Failed connecting to remote API server')
    sys.exit('Program Ended')
print "Connection succeeded"

    
res,objs=vrep.simxGetObjects(clientID, vrep.sim_handle_all,vrep.simx_opmode_blocking)
if res==vrep.simx_return_ok:
    print ('Number of objects in the scene: ',len(objs))
else:
    print ('Remote API function call returned with error code: ',res)

    
############################# set up robot #############################        
time.sleep(0.5)

startTime = time.time()
vrep.simxGetIntegerParameter(clientID,vrep.sim_intparam_mouse_x, vrep.simx_opmode_streaming)

usensors = 16*[-1]
for i in range(16):
    res, usensors[i] = vrep.simxGetObjectHandle(clientID, "Pioneer_p3dx_ultrasonicSensor"+str(i+1), vrep.simx_opmode_blocking)
res, leftWheel  = vrep.simxGetObjectHandle(clientID, "Pioneer_p3dx_leftMotor" , vrep.simx_opmode_blocking)
res, rightWheel = vrep.simxGetObjectHandle(clientID, "Pioneer_p3dx_rightMotor", vrep.simx_opmode_blocking)

if res != vrep.simx_return_ok:
    print ('Failed to get sensor Handler')
    vrep.simxFinish(clientID)
    sys.exit('Program ended')

wheel_radius = 0.195/2
wheel_tread  = 0.381
dt = 0.05
    
print "Setting up succeeded"


############################# definition of functions and variables for robot

vel_left  = 3               
vel_right = 3              
odom = [0.0, 0.0, 0.0]         
last_left_rad, last_right_rad = [0, 0], [0, 0]

def CalcRadDiff(rad, last_rad):
    if rad*last_rad >= 0 or (last_rad < 0 and rad < math.pi/2) or (last_rad > 0 and rad > -math.pi/2):
        return rad - last_rad
    return rad + (2*math.pi - last_rad) if rad < 0 else rad - (2*math.pi + last_rad)

# t-1 to t
def CalcOdometry(left_diff, right_diff, odom, vel, wvel):
    odom[2] = odom[2] + wvel*dt
    odom[0] = odom[0] + vel*dt*math.cos(odom[2])
    odom[1] = odom[1] + vel*dt*math.sin(odom[2])
    return odom

#getting  datas from sensors
def GetSonicData(n=-1):
    detect = 16*[0] #quering whether sensor detected propery
    data   = 16*[0]
    for i in range(16):
        # return, TorF, (x,y,z), 
        ret, detect[i], a, b, c = vrep.simxReadProximitySensor(clientID, usensors[i], vrep.simx_opmode_streaming)
        data[i] = a[2]
    #print map(lambda x:x[2], data))
    #
    return (detect, data) if n == -1 else (detect[n], data[n])

# strange phenomonan
def MoveOnce(vel, wvel):
 
    past_left_wheel_rad  = vrep.simxGetJointPosition(clientID, leftWheel,  vrep.simx_opmode_streaming)
    past_right_wheel_rad = vrep.simxGetJointPosition(clientID, rightWheel, vrep.simx_opmode_streaming)
    # commads
    global wheel_tread, wheel_radius
    vel_left  = (vel - wheel_tread*wvel*0.5)/wheel_radius
    vel_right = (vel + wheel_tread*wvel*0.5)/wheel_radius
    vrep.simxSetJointTargetVelocity(clientID, leftWheel,  vel_left/2,  vrep.simx_opmode_oneshot)
    vrep.simxSetJointTargetVelocity(clientID, rightWheel, vel_right/2, vrep.simx_opmode_oneshot)
    #
    time.sleep(dt)
    #
    # 
    left_wheel_rad  = vrep.simxGetJointPosition(clientID, leftWheel,  vrep.simx_opmode_streaming)
    right_wheel_rad = vrep.simxGetJointPosition(clientID, rightWheel, vrep.simx_opmode_streaming)
  
    global odom 
    left_diff  = CalcRadDiff(left_wheel_rad[1]/2,  past_left_wheel_rad[1])
    right_diff = CalcRadDiff(right_wheel_rad[1]/2, past_right_wheel_rad[1])
    odom = CalcOdometry(left_diff, right_diff, odom, vel, wvel)
    #
    return odom

############################### user level's functions
#################################
def Stop():
    for i in range(3):
        vrep.simxSetJointTargetVelocity(clientID, leftWheel,  0, vrep.simx_opmode_oneshot)
        vrep.simxSetJointTargetVelocity(clientID, rightWheel, 0, vrep.simx_opmode_oneshot)

# run robots in several times
def Move(T, vel=0.3, wvel=0.0, finally_stop=True):
    #
    global odom
    N = int(T/dt)
    for n in range(N):
        odom = MoveOnce(vel, wvel)
        print "odometory(x, y, theta): ",
        print odom
    #
    if finally_stop:
        Stop()

def Detect():

    sensor_dirs = [ 1.57,  1.05,  0.52,  0.00, -0.00, -0.52, -1.05, -1.57, #front
                   -1.57, -2.09, -2.62, -3.14,  3.14,  2.62,  2.09,  1.57] #forward
    # getting datas 
    detection = []
    detect, data = GetSonicData()
    for i in range(16):
        if detect[i]:
            x, y = data[i]*math.cos(sensor_dirs[i]), data[2]*math.sin(sensor_dirs[i])
            detection.append([x, y, data[i], sensor_dirs[i]]) # (x, y, distance, direction)
    return detection

def Detect_():
 
    sensor_dirs = [ 1.57,  1.05,  0.52,  0.00, -0.00, -0.52, -1.05, -1.57, # front
                   -1.57, -2.09, -2.62, -3.14,  3.14,  2.62,  2.09,  1.57] # forward
    # getting datas
    detection = []
    detect, data = GetSonicData()
    for i in range(16):
        if detect[i]:
            x, y = data[i]*math.cos(sensor_dirs[i]), data[2]*math.sin(sensor_dirs[i])
            if i%8 == 0:
                sensor_dirs[i]-=0.01
            detection.append([x, y, data[i], sensor_dirs[i]]) # (x, y, distance, direction)
    return detection

def Line(line, goal, radius=0.1, vel=0.3):
    global odom
    k1 = 1
    k2 = 5
    k3 = 20
    wvel  = 0
    while True:
        eta = math.fabs(math.cos(line[2]))*(math.fabs(math.tan(line[2])*(odom[0]-line[0]) - (odom[1]-line[1])))
        dtheta = odom[2] - line[2]
        dwvel = -k1*wvel - k2*dtheta - k3*eta
        #print eta, dtheta, vel, dwvel, wvel 
        wvel += dwvel
        odom = MoveOnce(vel, wvel)
        print "odometory(x, y, theta): ",
        print odom, wvel
        if numpy.hypot(goal[0] - odom[0], goal[1] - odom[1]) < radius:
            break
    #
    Stop()

############################# user level's main programs
#############################
def demo():
    while True:
        Move(0.5, 0.3, 0.2, finally_stop=False)
        ddata = Detect()
        if len(ddata) != 0 and min(map(lambda x: x[2], ddata)) < 0.7:
            print min(map(lambda x: x[2], ddata)) 
            break
    #
    Stop()
    print "finish"

#avoid obstacle
def demo01(time):
    total_time=0
    while total_time<time:
        ddata=Detect()
        if len(ddata) != 0 and min(map(lambda x: x[2],  ddata)) < 0.3:
            min_dist=0.9
            print min(map(lambda x: x[2],  ddata))
            print "evecuation"
            for i in range(len(ddata)):
                if min_dist > ddata[i][2]:
                    min_dist=ddata[i][2]
                    min_data=i
            if 0.00 <= ddata[min_data][3] <1.57:
                Move(0.3,-0.3,0.0,finally_stop=False)
                Move(0.4,-0.0,-4.0,finally_stop=False)
                total_time+=0.7
            elif -1.57 < ddata[min_data][3] <= -0.00:
                Move(0.3,-0.4,0.0,finally_stop=False)
                Move(0.4,-0.0,3.0,finally_stop=False)
                total_time+=0.7
            elif -3.14 <= ddata[min_data][3] <=-1.57:
                Move(0.3,0.4,0.2,finally_stop=False)
                #Move(0.6,0,-5,finally_stop=False)
                total_time+=0.3
            else:
                Move(0.3,0.3,-0.25,finally_stop=False)
                #Move(0.6,0.5,5,finally_stop=False)
                total_time+=0.3
            Move(0.4,0.75,0,finally_stop=False)
            total_time+=0.4
            
        else:
            Move(0.3,0.4,0.0,finally_stop=False)
            total_time+=0.3
            

    Stop()
    print "finish"


In cygwin,it was displayed on the screen in the following

Code: Select all


python demo_01.py
Connected to remote API server
Connection succeeded
('Number of objects in the scene: ', 129)
Setting up succeeded

Next, i want to run robot in vrep.
How should I do operation in vrep?

Post Reply