def braiten1b(self):
"slowly move forward and print normal vector readings"
intens = 100
ambientIntensRatio = 0.2
attVect = [0,0,pi *4]
for step in xrange(self.noSteps):
rightInput = vrep.simxReadProximitySensor(self.simulID, self.rightEye, vrep.simx_opmode_oneshot_wait)
vrep.simxSynchronousTrigger(self.simulID)
leftInput = vrep.simxReadProximitySensor(self.simulID, self.leftEye, vrep.simx_opmode_oneshot_wait)
vrep.simxSynchronousTrigger(self.simulID)
centerInput = vrep.simxReadProximitySensor(self.simulID, self.KJcenterEye, vrep.simx_opmode_oneshot_wait)
vrep.simxSynchronousTrigger(self.simulID)
angle = degrees(self.angleBetVecs([0,0,1], centerInput[2]))
lightReading = self.irradAtSensor(intens, ambientIntensRatio, centerInput[2], attVect)
print "Center-->err:%s - Detct'd: %s\tAngle:%.3f\tIrrad:%.3f\tNorm: %.3f\tVector:%s\t" % (centerInput[0],
centerInput[3],
angle,
lightReading,
norm(centerInput[2]),
centerInput[2])
eCode = vrep.simxSetJointTargetVelocity(self.simulID, self.rightMotor, 1/lightReading, vrep.simx_opmode_oneshot_wait)
eCode = vrep.simxSetJointTargetVelocity(self.simulID, self.leftMotor, 1/lightReading, vrep.simx_opmode_oneshot_wait)
vrep.simxSynchronousTrigger(self.simulID)
sleep(0)
def drive(self):
Left = self.v0
Right = self.v0
for i in range(0,len(self.ultra_sensors)-1):
res, detectionState, \
detectedPoint, detectedObjectHandle, \
detectedSurfaceNormalVector = \
vrep.simxReadProximitySensor(self.clientID,
self.ultra_sensors[i],
vrep.simx_opmode_buffer)
dist = vec_length(detectedPoint)
if (res==0) and (dist<self.noDetectionDist):
if (dist<self.maxDetectionDist):
dist=self.maxDetectionDist
self.detect[i]=1-((dist-self.maxDetectionDist)/(self.noDetectionDist-self.maxDetectionDist))
else:
self.detect[i]=0
Left=Left+self.braitenbergL[i]*self.detect[i]
Right=Right+self.braitenbergR[i]*self.detect[i]
vrep.simxSetJointTargetVelocity(self.clientID,
self.left_motor_handle,
Left,
vrep.simx_opmode_streaming)
vrep.simxSetJointTargetVelocity(self.clientID,
self.right_motor_handle,
Right,
vrep.simx_opmode_streaming)
def execute_action(clientID, RobotHandle, LMotorHandle, RMotorHandle, raw_pose_data, next_action):
linearVelo, angularVelo, goal_pose = compute_control(raw_pose_data, next_action)
l_ang_v, r_ang_v = dif_drive(linearVelo, angularVelo)
#----------
dist_bound = 0.15
ang_bound = 0.1*PI
# retDia, DialogHandle, uiHandle = vrep.simxDisplayDialog(clientID, 'Current Action', next_action, vrep.sim_dlgstyle_message, 'None', None, None, vrep.simx_opmode_blocking)
#----------
while (distance(raw_pose_data, goal_pose)>= dist_bound) or (abs(raw_pose_data[2]-goal_pose[2])>=ang_bound):
# print '--line_distance--', distance(raw_pose_data, goal_pose)
# print '--angle distance--', abs(raw_pose_data[2]-goal_pose[2])
pret, RobotPos = vrep.simxGetObjectPosition(clientID, RobotHandle, -1, vrep.simx_opmode_blocking)
#print "robot position: (x = " + str(RobotPos[0]) + ", y = " + str(RobotPos[1]) + ")"
oret, RobotOrient = vrep.simxGetObjectOrientation(clientID, RobotHandle, -1, vrep.simx_opmode_blocking)
#print "robot orientation: (a = " + str(RobotOrient[0]) + ", b = " + str(RobotOrient[1]) +", g = " + str(RobotOrient[2]) + ")"
raw_pose_data = shift_raw_pose([RobotPos[0], RobotPos[1], RobotOrient[2]])
#------------------------------
linearVelo, angularVelo = Find_Control(raw_pose_data, next_action, goal_pose)
l_ang_v, r_ang_v = dif_drive(linearVelo, angularVelo)
#print 'raw_pose_data', raw_pose_data
vrep.simxSetJointTargetVelocity(clientID, LMotorHandle, l_ang_v, vrep.simx_opmode_streaming)
vrep.simxSetJointTargetVelocity(clientID, RMotorHandle, r_ang_v, vrep.simx_opmode_streaming)
#print 'distance_x_y:%s; angle_dif:%s' %(str(distance(raw_pose_data, goal_pose)),str(abs(raw_pose_data[2]-goal_pose[2])))
print 'Goal pose reached!'
# vrep.simxSetJointTargetVelocity(clientID, LMotorHandle, 0, vrep.simx_opmode_blocking)
# vrep.simxSetJointTargetVelocity(clientID, RMotorHandle, 0, vrep.simx_opmode_blocking)
# time.sleep(0.5)
# retEnd = vrep.simxEndDialog(clientID, DialogHandle, vrep.simx_opmode_blocking)
return raw_pose_data
def moveKJuniorReadProxSensors(self):
"slowly move forward and print normal vector readings"
rightInput = vrep.simxReadProximitySensor(self.simulID, self.KJrightEye, vrep.simx_opmode_streaming)
leftInput = vrep.simxReadProximitySensor(self.simulID, self.KJleftEye, vrep.simx_opmode_streaming)
vrep.simxSynchronousTrigger(self.simulID)
print "Proximity sensor readings error codes: ", rightInput[0],leftInput[0]
for step in xrange(self.noSteps):
rightSpeed = 10
leftSpeed = rightSpeed
eCode = vrep.simxSetJointTargetVelocity(self.simulID, self.KJrightMotor, rightSpeed, vrep.simx_opmode_oneshot_wait)
eCode = vrep.simxSetJointTargetVelocity(self.simulID, self.KJleftMotor, leftSpeed, vrep.simx_opmode_oneshot_wait)
vrep.simxSynchronousTrigger(self.simulID)
rightInput = vrep.simxReadProximitySensor(self.simulID, self.KJrightEye, vrep.simx_opmode_buffer)
vrep.simxSynchronousTrigger(self.simulID)
leftInput = vrep.simxReadProximitySensor(self.simulID, self.KJleftEye, vrep.simx_opmode_buffer)
vrep.simxSynchronousTrigger(self.simulID)
print "Left-->err:%s - Detct'd: %s\t%s\t\t\tRight--> err:%s - Detct'd: %s\t%s" % (leftInput[0],
leftInput[3],
leftInput[4],
rightInput[0],
rightInput[3],
rightInput[4])
sleep(.2)
def moveAndReadProxSensors(self):
"rotate in place and print sensor distance and normal vector readings"
for step in xrange(self.noSteps):
if step>self.noSteps / 2:
rightSpeed = -1
leftSpeed = -rightSpeed
else:
rightSpeed = 1
leftSpeed = -rightSpeed
eCode = vrep.simxSetJointTargetVelocity(self.simulID, self.rightMotor, rightSpeed, vrep.simx_opmode_oneshot_wait)
eCode = vrep.simxSetJointTargetVelocity(self.simulID, self.leftMotor, leftSpeed, vrep.simx_opmode_oneshot_wait)
vrep.simxSynchronousTrigger(self.simulID)
rightInput = vrep.simxReadProximitySensor(self.simulID, self.rightEye, vrep.simx_opmode_oneshot_wait)
vrep.simxSynchronousTrigger(self.simulID)
leftInput = vrep.simxReadProximitySensor(self.simulID, self.leftEye, vrep.simx_opmode_oneshot_wait)
vrep.simxSynchronousTrigger(self.simulID)
print "Left-->err:%s - Detct'd: %s\t%s\t\tRight--> err:%s - Detct'd: %s\t\t\t%s" % (leftInput[0],
leftInput[3],
leftInput[2],
rightInput[0],
rightInput[3],
rightInput[2])
sleep(.1)
self.stopRobot(self.simulID,[self.rightMotor,self.leftMotor])
vrep.simxSynchronousTrigger(self.simulID)
def set_motors():
global linearVelocityLeft, linearVelocityRight, wheelRadius, vrep, clientID, leftJointDynamic, rightJointDynamic
t1 = linearVelocityLeft/wheelRadius
t2 = linearVelocityRight/wheelRadius
print t1, t2
vrep.simxSetJointTargetVelocity(clientID, leftJointDynamic, t1, vrep.simx_opmode_oneshot_wait)
vrep.simxSetJointTargetVelocity(clientID, rightJointDynamic, t2, vrep.simx_opmode_oneshot_wait)
def testMaxSpeed(self, maxSpeed, mode):
"""test max speed of khepera-like robot in V-Rep
revving the motors up to maxSpeed in the self.noSteps and then backward.
mode--> 1, both motors, 2: right only, 3: left only"""
if mode == 1:
rightOn = leftOn = 1
elif mode == 2:
rightOn = 1
leftOn = 0
elif mode == 3:
rightOn = 0
leftOn = 1
unitSpeed = maxSpeed /self.noSteps
for i in xrange(self.noSteps):
eCode = vrep.simxSetJointTargetVelocity(self.simulID, self.rightMotor, unitSpeed *(i+1)*rightOn, vrep.simx_opmode_oneshot_wait)
eCode = vrep.simxSetJointTargetVelocity(self.simulID, self.leftMotor, unitSpeed *(i+1)*leftOn, vrep.simx_opmode_oneshot_wait)
vrep.simxSynchronousTrigger(self.simulID)
print "Step: %s\t Speed now: %.2f" %(str(i),(unitSpeed *(i+1)))
for i in xrange(self.noSteps):
eCode = vrep.simxSetJointTargetVelocity(self.simulID, self.rightMotor, -(maxSpeed/(i+1))*rightOn, vrep.simx_opmode_oneshot_wait)
eCode = vrep.simxSetJointTargetVelocity(self.simulID, self.leftMotor, -(maxSpeed/(i+1))*leftOn, vrep.simx_opmode_oneshot_wait)
vrep.simxSynchronousTrigger(self.simulID)
print "Step: %s\t Speed now: %.2f" % (str(i), (maxSpeed/(i+1))*rightOn)
def moveRobotRaw_dr20_(leftJoint, rightJoint, clientID):
err_move_leftJ = vrep.simxSetJointTargetVelocity(clientID, leftJoint, 2, vrep.simx_opmode_oneshot)
err_move_rightJ = vrep.simxSetJointTargetVelocity(clientID, rightJoint, 2, vrep.simx_opmode_oneshot)
if err_move_leftJ != vrep.simx_error_noerror:
print "Das linke Gelenk konnte nicht bewegt werden!"
if err_move_rightJ != vrep.simx_error_noerror:
print "Das rechte Gelenk konnte nicht bewegt werden!"
def set_motor_velocity(self, control):
"""Set a target velocity on the pioneer motors, multiplied by the gain
defined in self.gain
Args:
control(list): the control [left_motor, right_motor]
"""
vrep.simxSetJointTargetVelocity(self.client_id, self.left_motor, self.gain*control[0], vrep.simx_opmode_oneshot_wait)
vrep.simxSetJointTargetVelocity(self.client_id, self.right_motor, self.gain*control[1], vrep.simx_opmode_oneshot_wait)
def handle_input(self, values):
# Set the velocity to some large number with the correct sign,
# because v-rep is weird like that
vrep.simxSetJointTargetVelocity(self.cid, self.arm_joint, values[0]*100,
vrep.simx_opmode_oneshot)
# Apply the desired torques to the joints
# V-REP is looking for just the absolute value here
vrep.simxSetJointForce(self.cid, self.arm_joint, abs(values[0]),
vrep.simx_opmode_oneshot)
def setController(self, name, typeController, val):
"""
Give an order to a controller
"""
if typeController == "motor":
if name in self.handles.keys():
pass
else:
self.initHandle(name)
motor_handle=self.handles[name]
vrep.simxSetJointTargetVelocity(self.clientID,motor_handle,val, vrep.simx_opmode_streaming)
def applyActionAbsolute(self,action):
self.desiredWheelRotSpeed=self.absSpeed[action[0]]
self.desiredSteeringAngle=self.absAngle[action[1]]
returnCode=vrep.simxSetJointTargetVelocity(self.clientID,self.ml,self.desiredWheelRotSpeed,vrep.simx_opmode_oneshot)
returnCode=vrep.simxSetJointTargetVelocity(self.clientID,self.mr,self.desiredWheelRotSpeed,vrep.simx_opmode_oneshot)
steeringAngleLeft=math.atan(self.l/(-self.d+self.l/math.tan(self.desiredSteeringAngle+0.00001)))
steeringAngleRight=math.atan(self.l/(self.d+self.l/math.tan(self.desiredSteeringAngle+0.00001)))
returnCode=vrep.simxSetJointTargetPosition(self.clientID,self.sl,steeringAngleLeft,vrep.simx_opmode_oneshot)
returnCode=vrep.simxSetJointTargetPosition(self.clientID,self.sr,steeringAngleRight,vrep.simx_opmode_oneshot)
def run360():
print "connecting to vrep with address", IPADDRESS, "on port", PORT
clientID = vrep.simxStart(IPADDRESS, PORT, True, True, 5000, 5)
print "hello vrep! ", clientID
if clientID == -1:
print "failed to connect to vrep server"
return
# get the motor joint handle
error,jointHandle = vrep.simxGetObjectHandle(clientID, "plantStandMotor", vrep.simx_opmode_oneshot_wait)
print "starting the motor..."
# set the velocity of the joint
vrep.simxSetJointTargetVelocity(clientID, jointHandle, 1.0, vrep.simx_opmode_oneshot_wait);
print "spinning 360 degrees..."
# set up to stream joint positions
vrep.simxGetJointPosition(clientID, jointHandle, vrep.simx_opmode_streaming);
passed90Degrees = False
# The control loop:
while vrep.simxGetConnectionId(clientID) != -1 : # while we are connected to the server..
(error,position) = vrep.simxGetJointPosition(
clientID,
jointHandle,
vrep.simx_opmode_buffer
)
# Fetch the newest joint value from the inbox (func. returns immediately (non-blocking)):
if error==vrep.simx_error_noerror :
# here we have the newest joint position in variable jointPosition!
# break when we've done a 360
if passed90Degrees and position >= 0 and position < math.pi/2.0:
break
elif not passed90Degrees and position > math.pi/2.0:
passed90Degrees = True
print "stoppping..."
vrep.simxSetJointTargetVelocity(clientID, jointHandle, 0.0, vrep.simx_opmode_oneshot_wait);
if clientID != -1:
vrep.simxFinish(clientID)
print "done!"
def reset_vrep():
print 'Start to connect vrep'
# Close eventual old connections
vrep.simxFinish(-1)
# Connect to V-REP remote server
clientID = vrep.simxStart('127.0.0.1', 19997, True, True, 5000, 5)
opmode = vrep.simx_opmode_oneshot_wait
# Try to retrieve motors and robot handlers
# http://www.coppeliarobotics.com/helpFiles/en/remoteApiFunctionsPython.htm#simxGetObjectHandle
ret_l, LMotorHandle = vrep.simxGetObjectHandle(clientID, "Pioneer_p3dx_leftMotor", opmode)
ret_r, RMotorHandle = vrep.simxGetObjectHandle(clientID, "Pioneer_p3dx_rightMotor", opmode)
ret_a, RobotHandle = vrep.simxGetObjectHandle(clientID, "Pioneer_p3dx", opmode)
vrep.simxSetJointTargetVelocity(clientID, LMotorHandle, 0, vrep.simx_opmode_blocking)
vrep.simxSetJointTargetVelocity(clientID, RMotorHandle, 0, vrep.simx_opmode_blocking)
time.sleep(1)
vrep.simxStopSimulation(clientID, vrep.simx_opmode_oneshot_wait)
vrep.simxFinish(clientID)
print 'Connection to vrep reset-ed!'
def moveKJuniorReadProxSensors(self):
"slowly move forward and print normal vector readings"
intens = 100
ambientIntens = 0
attVect = [0,0,pi *4]
rightInput = vrep.simxReadProximitySensor(self.simulID, self.KJrightEye, vrep.simx_opmode_streaming)
leftInput = vrep.simxReadProximitySensor(self.simulID, self.KJleftEye, vrep.simx_opmode_streaming)
centerInput = vrep.simxReadProximitySensor(self.simulID, self.KJcenterEye, vrep.simx_opmode_streaming)
vrep.simxSynchronousTrigger(self.simulID)
print "Proximity sensor readings error codes: ", rightInput[0],leftInput[0]
for step in xrange(self.noSteps):
rightSpeed = 10
leftSpeed = rightSpeed
eCode = vrep.simxSetJointTargetVelocity(self.simulID, self.KJrightMotor, rightSpeed, vrep.simx_opmode_oneshot_wait)
eCode = vrep.simxSetJointTargetVelocity(self.simulID, self.KJleftMotor, leftSpeed, vrep.simx_opmode_oneshot_wait)
vrep.simxSynchronousTrigger(self.simulID)
rightInput = vrep.simxReadProximitySensor(self.simulID, self.KJrightEye, vrep.simx_opmode_buffer)
vrep.simxSynchronousTrigger(self.simulID)
leftInput = vrep.simxReadProximitySensor(self.simulID, self.KJleftEye, vrep.simx_opmode_buffer)
vrep.simxSynchronousTrigger(self.simulID)
centerInput = vrep.simxReadProximitySensor(self.simulID, self.KJcenterEye, vrep.simx_opmode_buffer)
vrep.simxSynchronousTrigger(self.simulID)
#===================================================================
# print "Left-->err:%s - Detct'd: %s\t%s\t\t\tRight--> err:%s - Detct'd: %s\t%s" % (leftInput[0],
# leftInput[3],
# leftInput[4],
# rightInput[0],
# rightInput[3],
# rightInput[4])
#===================================================================
angle = degrees(self.angleBetVecs([0,0,1], centerInput[2]))
lightReading = self.irradAtSensor(intens, ambientIntens, centerInput[2], attVect)
print "Center-->err:%s - Detct'd: %s\tAngle:%.3f\tIrrad:%.3f\tNorm: %.3f\tVector:%s\t" % (centerInput[0],
centerInput[3],
angle,
lightReading,
norm(centerInput[2]),
centerInput[2])
sleep(0)
def move(self, direction): """ sets velocity of wheels to move the bubbleRob """ velocity = [] if direction == 0: #forward velocity = [8,8] elif direction == 1: #left velocity = [3,8] elif direction == 2: #right velocity = [8,3] elif direction == 3: #backward velocity = [-8,-8] vrep.simxSetJointTargetVelocity(self.clientID,self.leftMotorHandle,velocity[0],vrep.simx_opmode_oneshot) vrep.simxSetJointTargetVelocity(self.clientID,self.rightMotorHandle,velocity[1],vrep.simx_opmode_oneshot) time.sleep(0.1)
def moveAndReadLights(self):
"rotate in place and print light readings"
eCode, res, rightEyeRead = vrep.simxGetVisionSensorImage(self.simulID, self.rightEye, 0, vrep.simx_opmode_streaming)
ecode, res, leftEyeRead = vrep.simxGetVisionSensorImage(self.simulID, self.leftEye, 0, vrep.simx_opmode_streaming)
vrep.simxSynchronousTrigger(self.simulID)
for step in xrange(self.noSteps):
rightSpeed = 25
leftSpeed = rightSpeed
eCode = vrep.simxSetJointTargetVelocity(self.simulID, self.rightMotor, rightSpeed, vrep.simx_opmode_oneshot_wait)
eCode = vrep.simxSetJointTargetVelocity(self.simulID, self.leftMotor, leftSpeed, vrep.simx_opmode_oneshot_wait)
vrep.simxSynchronousTrigger(self.simulID)
eCodeR, res, rightEyeRead = vrep.simxGetVisionSensorImage(self.simulID, self.rightEye, 0, vrep.simx_opmode_buffer)
eCodeL, res, leftEyeRead = vrep.simxGetVisionSensorImage(self.simulID, self.leftEye, 0, vrep.simx_opmode_buffer)
vrep.simxSynchronousTrigger(self.simulID)
# print "Right eCode:\t", eCodeR,
# print "Left eCode:\t", eCodeL
# leftImg = np.array(leftEyeRead, np.uint8)
# rightImg.resize(res[0],res[1],3)
print "Right:\t%d, %d\tLeft:\t%d, %d"% (len(rightEyeRead),sum(rightEyeRead), len(leftEyeRead),sum(leftEyeRead))
def computeVelocityStraight(self, LastPosition, NextPosition ):
if self.clientID != -1:
vector_displacement = [NextPosition[0]-LastPosition[0],NextPosition[1]-LastPosition[1]]
distanceStaight = math.sqrt(vector_displacement[0]*vector_displacement[0] + vector_displacement[1]*vector_displacement[1])
Vmax = OmegaMax*RWheel
Time_needed = math.fabs(distanceStaight) / Vmax
print(Time_needed)
vrep.simxSetJointTargetVelocity(self.clientID,self.LWMotor_hdl[1],OmegaMax,vrep.simx_opmode_oneshot)
vrep.simxSetJointTargetVelocity(self.clientID,self.RWMotor_hdl[1],OmegaMax,vrep.simx_opmode_oneshot)
time.sleep(Time_needed)
vrep.simxSetJointTargetVelocity(self.clientID,self.LWMotor_hdl[1],0.0,vrep.simx_opmode_oneshot)
vrep.simxSetJointTargetVelocity(self.clientID,self.RWMotor_hdl[1],0.0,vrep.simx_opmode_oneshot)
print ('MovementDone')
def MotorDifferential(clientID, handleList, speed, diff, astern):
# Get all the handles at once
shape = len(handleList)
errorcode =[]
if shape % 2 == 0:
for x in range(1,shape+1,2):
if astern :
get_errorCode=vrep.simxSetJointTargetVelocity(clientID, handleList[x-1], -speed, vrep.simx_opmode_oneshot)
errorcode.append(get_errorCode)
get_errorCode=vrep.simxSetJointTargetVelocity(clientID, handleList[x], -(speed-diff), vrep.simx_opmode_oneshot)
errorcode.append(get_errorCode)
else:
get_errorCode=vrep.simxSetJointTargetVelocity(clientID, handleList[x-1], speed, vrep.simx_opmode_oneshot)
errorcode.append(get_errorCode)
get_errorCode=vrep.simxSetJointTargetVelocity(clientID, handleList[x], speed-diff, vrep.simx_opmode_oneshot)
errorcode.append(get_errorCode)
else:
sys.exit('Differential needs pairs of motors')
return errorcode
def braiten2a(self):
"Seek light source"
intens = 50
ambientIntensRatio = .2
attVect = [0,0,1]
rightInput = vrep.simxReadProximitySensor(self.simulID, self.KJrightEye, vrep.simx_opmode_streaming)
leftInput = vrep.simxReadProximitySensor(self.simulID, self.KJleftEye, vrep.simx_opmode_streaming)
centerInput = vrep.simxReadProximitySensor(self.simulID, self.KJcenterEye, vrep.simx_opmode_streaming)
vrep.simxSynchronousTrigger(self.simulID)
print "Proximity sensor readings error codes: ", rightInput[0],leftInput[0]
for step in xrange(self.noSteps):
rightInput = vrep.simxReadProximitySensor(self.simulID, self.KJrightEye, vrep.simx_opmode_buffer)
vrep.simxSynchronousTrigger(self.simulID)
leftInput = vrep.simxReadProximitySensor(self.simulID, self.KJleftEye, vrep.simx_opmode_buffer)
vrep.simxSynchronousTrigger(self.simulID)
centerInput = vrep.simxReadProximitySensor(self.simulID, self.KJcenterEye, vrep.simx_opmode_buffer)
vrep.simxSynchronousTrigger(self.simulID)
#===================================================================
# print "Left-->err:%s - Detct'd: %s\t%s\t\t\tRight--> err:%s - Detct'd: %s\t%s" % (leftInput[0],
# leftInput[3],
# leftInput[4],
# rightInput[0],
# rightInput[3],
# rightInput[4])
#===================================================================
angle = degrees(self.angleBetVecs([0,0,1], centerInput[2]))
lightReading = self.irradAtSensor(intens, ambientIntens, centerInput[2], attVect)
print "Center-->err:%s - Detct'd: %s\tAngle:%.3f\tIrrad:%.3f\tNorm: %.3f\tVector:%s\t" % (centerInput[0],
centerInput[3],
angle,
lightReading,
norm(centerInput[2]),
centerInput[2])
eCode = vrep.simxSetJointTargetVelocity(self.simulID, self.KJrightMotor, lightReading, vrep.simx_opmode_oneshot_wait)
eCode = vrep.simxSetJointTargetVelocity(self.simulID, self.KJleftMotor, lightReading, vrep.simx_opmode_oneshot_wait)
vrep.simxSynchronousTrigger(self.simulID)
sleep(0)
def initilize_vrep_api(self):
# initialize ePuck handles and variables
_, self.bodyelements=vrep.simxGetObjectHandle(
self.clientID, 'ePuck_bodyElements', vrep.simx_opmode_oneshot_wait)
_, self.leftMotor=vrep.simxGetObjectHandle(
self.clientID, 'ePuck_leftJoint', vrep.simx_opmode_oneshot_wait)
_, self.rightMotor=vrep.simxGetObjectHandle(
self.clientID, 'ePuck_rightJoint', vrep.simx_opmode_oneshot_wait)
_, self.ePuck=vrep.simxGetObjectHandle(
self.clientID, 'ePuck', vrep.simx_opmode_oneshot_wait)
_, self.ePuckBase=vrep.simxGetObjectHandle(
self.clientID, 'ePuck_base', vrep.simx_opmode_oneshot_wait)
# proxSens = prox_sens_initialize(self.clientID)
# initialize odom of ePuck
_, self.xyz = vrep.simxGetObjectPosition(
self.clientID, self.ePuck, -1, vrep.simx_opmode_streaming)
_, self.eulerAngles = vrep.simxGetObjectOrientation(
self.clientID, self.ePuck, -1, vrep.simx_opmode_streaming)
# initialize overhead cam
_, self.overheadCam=vrep.simxGetObjectHandle(
self.clientID, 'Global_Vision', vrep.simx_opmode_oneshot_wait)
_, self.resolution, self.image = vrep.simxGetVisionSensorImage(
self.clientID,self.overheadCam,0,vrep.simx_opmode_oneshot_wait)
# initialize goal handle + odom
_, self.goalHandle=vrep.simxGetObjectHandle(
self.clientID, 'Goal_Position', vrep.simx_opmode_oneshot_wait)
_, self.goalPose = vrep.simxGetObjectPosition(
self.clientID, self.goalHandle, -1, vrep.simx_opmode_streaming)
# STOP Motor Velocities. Not sure if this is necessary
_ = vrep.simxSetJointTargetVelocity(
self.clientID,self.leftMotor,0,vrep.simx_opmode_oneshot_wait)
_ = vrep.simxSetJointTargetVelocity(
self.clientID,self.rightMotor,0,vrep.simx_opmode_oneshot_wait)
def applyActionIncremental(self,action):
if action[0]!=0:
self.desiredWheelRotSpeed=self.desiredWheelRotSpeed+action[0]*self.wheelRotSpeedDx
if self.desiredWheelRotSpeed>10*self.wheelRotSpeedDx:
self.desiredWheelRotSpeed = 10*self.wheelRotSpeedDx
elif self.desiredWheelRotSpeed<-10*self.wheelRotSpeedDx:
self.desiredWheelRotSpeed = -10*self.wheelRotSpeedDx
returnCode=vrep.simxSetJointTargetVelocity(self.clientID,self.ml,self.desiredWheelRotSpeed,vrep.simx_opmode_oneshot)
returnCode=vrep.simxSetJointTargetVelocity(self.clientID,self.mr,self.desiredWheelRotSpeed,vrep.simx_opmode_oneshot)
if action[1]!=0:
self.desiredSteeringAngle=self.desiredSteeringAngle+action[1]*self.steeringAngleDx
if self.desiredSteeringAngle>10*self.steeringAngleDx:
self.desiredSteeringAngle = 10*self.steeringAngleDx
elif self.desiredSteeringAngle<-10*self.steeringAngleDx:
self.desiredSteeringAngle = -10*self.steeringAngleDx
steeringAngleLeft=math.atan(self.l/(-self.d+self.l/math.tan(self.desiredSteeringAngle+0.00001)))
steeringAngleRight=math.atan(self.l/(self.d+self.l/math.tan(self.desiredSteeringAngle+0.00001)))
returnCode=vrep.simxSetJointTargetPosition(self.clientID,self.sl,steeringAngleLeft,vrep.simx_opmode_oneshot)
returnCode=vrep.simxSetJointTargetPosition(self.clientID,self.sr,steeringAngleRight,vrep.simx_opmode_oneshot)
motor_torque=60;
dVel=1;
dSteer=0.1;
steer_angle=0;
motor_velocity=dVel*3;
brake_force=0;
#set the brake part
returnCode=vrep.simxSetJointForce(clientID,fl_brake,brake_force,vrep.simx_opmode_blocking)
returnCode=vrep.simxSetJointForce(clientID,fr_brake,brake_force,vrep.simx_opmode_blocking)
returnCode=vrep.simxSetJointForce(clientID,bl_brake,brake_force,vrep.simx_opmode_blocking)
returnCode=vrep.simxSetJointForce(clientID,br_brake,brake_force,vrep.simx_opmode_blocking)
#drive car
returnCode=vrep.simxSetJointForce(clientID,motor,motor_torque,vrep.simx_opmode_blocking)
returnCode=vrep.simxSetJointTargetVelocity(clientID,motor,motor_velocity,vrep.simx_opmode_blocking)
returnCode=vrep.simxSetJointTargetPosition(clientID,steer,steer_angle,vrep.simx_opmode_blocking)
#read data from vreppython image
returnCode,detectionState_l,detectedPoint_l,OH,SNV=vrep.simxReadProximitySensor(clientID,left_sensor,vrep.simx_opmode_streaming)
returnCode,detectionState_lf,detectedPoint_lf,OH,SNV=vrep.simxReadProximitySensor(clientID,leftf_sensor,vrep.simx_opmode_streaming)
returnCode,detectionState_f,detectedPoint_f,OH,SNV=vrep.simxReadProximitySensor(clientID,front_sensor,vrep.simx_opmode_streaming)
returnCode,detectionState_rf,detectedPoint_rf,OH,SNV=vrep.simxReadProximitySensor(clientID,rightf_sensor,vrep.simx_opmode_streaming)
returnCode,detectionState_r,detectedPoint_r,OH,SNV=vrep.simxReadProximitySensor(clientID,right_sensor,vrep.simx_opmode_streaming)
returnCode,rel_pos=vrep.simxGetObjectPosition(clientID,tar_pos,car_pos,vrep.simx_opmode_streaming);
returnCode,resolution,image=vrep.simxGetVisionSensorImage(clientID,camera,0,vrep.simx_opmode_streaming)
print(time.localtime( time.time() ))
for i in range(0,1000):
#read sensor data
returnCode,detectionState_f,detectedPoint_f,OH,SNV=vrep.simxReadProximitySensor(clientID,front_sensor,vrep.simx_opmode_buffer)
vrep.simxFinish(-1) # just in case, close all opened connections
'''Establishes connection to the Simulation Server'''
clientID=vrep.simxStart('127.0.0.1',19997,True,True,5000,5)
if clientID!=-1: #check if client connection successful
print ('Connected to remote API server')
else:
print ('Connection not successful')
sys.exit('Could not connect')
errorCodeLeftMotor,left_motor_handle=vrep.simxGetObjectHandle(clientID,'Pioneer_p3dx_leftMotor',vrep.simx_opmode_blocking)
errorCodeRightMotor,right_motor_handle=vrep.simxGetObjectHandle(clientID,'Pioneer_p3dx_rightMotor',vrep.simx_opmode_oneshot_wait)
vrep.simxSetJointTargetVelocity(clientID,left_motor_handle,0, vrep.simx_opmode_streaming)
vrep.simxSetJointTargetVelocity(clientID,right_motor_handle,0, vrep.simx_opmode_streaming)
'''Gets the ids of the sensors of the robot'''
sH = []
s = []
for x in range(1,16+1):
errorCodeSensor,sensor_handle=vrep.simxGetObjectHandle(clientID,'Pioneer_p3dx_ultrasonicSensor'+str(x),vrep.simx_opmode_oneshot_wait)
sH.append(sensor_handle)
s.append(sH[1])
s.append(sH[3])
s.append(sH[4])
s.append(sH[6])
s.append(sH[11])
def RotateOnSpot(desiredAngle, robotID):
allowance = 1.5
desiredAngle = desiredAngle
mytime = time.time()
runningtime = 0
if (robotID == 1):
ErrorCode, angles = vrep.simxGetObjectOrientation(
clientID, Red1_Orientation, -1, vrep.simx_opmode_buffer)
currentAngle = angles[2] * 180 / math.pi
while ((abs(currentAngle - desiredAngle)) >
allowance) and runningtime < 0.3: #adjust for allowance
runningtime = time.time() - mytime
ErrorCode, angles = vrep.simxGetObjectOrientation(
clientID, Red1_Orientation, -1, vrep.simx_opmode_buffer)
currentAngle = angles[2] * 180 / math.pi
speed = abs(currentAngle - desiredAngle) * 0.05
if currentAngle >= 179 or currentAngle <= -179:
if desiredAngle < 0:
currentAngle = -179
else:
currentAngle = 179
if desiredAngle > currentAngle:
rotationOnSpotCCW1(speed) #adjust for resolution
else:
rotationOnSpotCW1(speed) #adjust for resolution
if (abs(currentAngle - desiredAngle) < allowance):
ErrorCode = vrep.simxSetJointTargetVelocity(
clientID, LeftJointHandle1, 0, vrep.simx_opmode_oneshot_wait)
ErrorCode = vrep.simxSetJointTargetVelocity(
clientID, RightJointHandle1, 0, vrep.simx_opmode_oneshot_wait)
if (robotID == 2):
ErrorCode, angles = vrep.simxGetObjectOrientation(
clientID, Red2_Orientation, -1, vrep.simx_opmode_buffer)
currentAngle = angles[2] * 180 / math.pi
while (abs(currentAngle - desiredAngle) >
allowance) and runningtime < 0.3: #adjust for allowance
runningtime = time.time() - mytime
speed = abs(currentAngle - desiredAngle) * 0.05
ErrorCode, angles = vrep.simxGetObjectOrientation(
clientID, Red2_Orientation, -1, vrep.simx_opmode_buffer)
currentAngle = angles[2] * 180 / math.pi
if (desiredAngle > currentAngle):
rotationOnSpotCCW2(speed) #adjust for resolution
else:
rotationOnSpotCW2(speed) #adjust for resolution
if (abs(currentAngle - desiredAngle) < allowance):
ErrorCode = vrep.simxSetJointTargetVelocity(
clientID, LeftJointHandle2, 0, vrep.simx_opmode_oneshot_wait)
ErrorCode = vrep.simxSetJointTargetVelocity(
clientID, RightJointHandle2, 0, vrep.simx_opmode_oneshot_wait)
if (robotID == 3):
ErrorCode, angles = vrep.simxGetObjectOrientation(
clientID, Red3_Orientation, -1, vrep.simx_opmode_buffer)
currentAngle = angles[2] * 180 / math.pi
while (abs(currentAngle - desiredAngle) >
allowance) and runningtime < 0.3: #adjust for allowance
runningtime = time.time() - mytime
speed = abs(currentAngle - desiredAngle) * 0.05
ErrorCode, angles = vrep.simxGetObjectOrientation(
clientID, Red3_Orientation, -1, vrep.simx_opmode_buffer)
currentAngle = angles[2] * 180 / math.pi
if (desiredAngle > currentAngle):
rotationOnSpotCCW3(speed) #adjust for resolution
else:
rotationOnSpotCW3(speed) #adjust for resolution
if (abs(currentAngle - desiredAngle) < allowance):
ErrorCode = vrep.simxSetJointTargetVelocity(
clientID, LeftJointHandle3, 0, vrep.simx_opmode_oneshot_wait)
ErrorCode = vrep.simxSetJointTargetVelocity(
clientID, RightJointHandle3, 0, vrep.simx_opmode_oneshot_wait)
#retrieve motor handles
errorCode, left_motor_handle = vrep.simxGetObjectHandle(
clientID, 'wheel_left_joint', vrep.simx_opmode_oneshot_wait)
errorCode, right_motor_handle = vrep.simxGetObjectHandle(
clientID, 'wheel_right_joint', vrep.simx_opmode_oneshot_wait)
#steps=Path
#steps = [[100, 100, 0],[0, 0, 0],[0, 0, 0],[0, 0, 0],[0, 0, 0],[0, 0, 0],[0, 0, 0],[0, 0, 0],[0, 0, 0]]
#steps = [[-4, -4, 0], [-3.905, -4.0, 0.82], [-3.776, -3.862, 0.82], [-3.647, -3.724, 0.82], [-3.518, -3.586, 0.82], [-3.389, -3.448, 0.82], [-3.26, -3.31, 0.82], [-3.131, -3.172, 0.82], [-3.002, -3.034, 0.82], [-2.873, -2.896, 0.82], [-2.744, -2.758, 0.82], [-2.615, -2.62, 0.82], [-2.486, -2.482, 0.82], [-2.357, -2.344, 0.82], [-2.228, -2.206, 0.82], [-2.099, -2.068, 2.47]]
#movement = [[10,0],[20,0],[50,0],[100,100],[50,0], [20,0],[10,0]]
#movement = Velocity_matrix
count = 0
print(len(movement))
for i in movement:
errorCode = vrep.simxSetJointTargetVelocity(clientID,
left_motor_handle, i[0],
vrep.simx_opmode_streaming)
errorCode = vrep.simxSetJointTargetVelocity(clientID,
right_motor_handle, i[1],
vrep.simx_opmode_blocking)
if count == len(movement):
time.sleep(0)
else:
print(steps[count])
count = count + 1
print(count)
simTime = vrep.simxGetLastCmdTime(clientID)
print(simTime / 1000)
time.sleep(0.25)
errorCode = vrep.simxSetJointTargetVelocity(clientID, left_motor_handle, 0,
def setMotor(motor, speed):
vrep.simxSetJointTargetVelocity(clientID, motor, speed,
vrep.simx_opmode_oneshot_wait)
def step(self, action):
# Activate the motors
vrep.simxSetJointTargetVelocity(self.client_id, self.left_motor_handle,
action[0], vrep.simx_opmode_blocking)
vrep.simxSetJointTargetVelocity(self.client_id,
self.right_motor_handle, action[1],
vrep.simx_opmode_blocking)
# Get observations
observations = {}
observations['proxy_sensor'] = []
observations['light_sensor'] = []
# Fetch the vals of proxy sensors
for sensor in self.proxy_sensors:
_, _, detectedPoint, _, _ = vrep.simxReadProximitySensor(
self.client_id, sensor, vrep.simx_opmode_blocking)
# Append to list of values
observations['proxy_sensor'].append(np.linalg.norm(detectedPoint))
# Fetch the vals of light sensors
for sensor in self.light_sensors:
# Fetch the initial value in the suggested mode
_, _, image = vrep.simxGetVisionSensorImage(
self.client_id, sensor, 1, vrep.simx_opmode_blocking)
# extract image from list
image = image[0] if len(image) else -1
# Append to the list of values
observations['light_sensor'].append(image)
# vrep gives a positive value for the black strip and negative for the
# floor so convert it into 0 and 1
observations['light_sensor'] = np.asarray(observations['light_sensor'])
observations['light_sensor'] = np.sign(observations['light_sensor'])
# When nothing is detected a very small value is retured -> changing it to 2
observations['proxy_sensor'] = np.asarray(observations['proxy_sensor'])
observations['proxy_sensor'][observations['proxy_sensor'] < 0.001] = 2
# Assign reward
reward = {}
# For light sensors
# If any of the center 2 sensors is 1 give high reward
if (observations['light_sensor'][[3, 4]] > 0).any():
reward['light_sensor'] = 5
# If any of second, third, sixth or seventh is 1
elif (observations['light_sensor'][[1, 2, 5, 6]] > 0).any():
reward['light_sensor'] = 2
# If first or last are high
elif (observations['light_sensor'][[0, 7]] > 0).any():
reward['light_sensor'] = 0
# Bot is completly out of line
else:
reward['light_sensor'] = -5
# For proximity sensors
reward['proxy_sensor'] = (observations['proxy_sensor'] <
0.7).sum() * -2
reward['proxy_sensor'] += (observations['proxy_sensor']
== 2).sum() * 10
# Should be rewarded for movement
r = np.clip(np.sum(np.absolute(action)) * 2, 0, 2)
reward['light_sensor'] += r
reward['proxy_sensor'] += r
# reward['combined'] += r
return observations, reward
def act(
self, turn_discr, accel_discr
): # entradas discretas, que hay que pasar a salidas continuas y no normalizadas
# turn_discr: string con estas posibilidades: {"very_left", "left", "slight_left", "front", "slight_right", "right", "very_right"}
# accel_discr: string con estas posibilidades: {"hard_break", "medium_break", "slight_break", "inertia", "slight_accel", "medium_accel", "full_gas"}
turn_dict = {
"[1, 0, 0, 0, 0, 0, 0]": 0, # "very_left"
"[0, 1, 0, 0, 0, 0, 0]": 0.2, # "left"
"[0, 0, 1, 0, 0, 0, 0]": 0.35, # "slight_left"
"[0, 0, 0, 1, 0, 0, 0]": 0.5, # "front"
"[0, 0, 0, 0, 1, 0, 0]": 0.65, # "slight_right"
"[0, 0, 0, 0, 0, 1, 0]": 0.8, # "right"
"[0, 0, 0, 0, 0, 0, 1]": 1 # "very_right"
}
steer_denorm = self.denorm(turn_dict[str(turn_discr)],
-self.max_steer_angle, self.max_steer_angle)
vrep.simxSetJointTargetPosition(self.clientID, self.steer_handle,
steer_denorm,
vrep.simx_opmode_streaming)
# accel_dict = { # (brake_force,motor_torque,motor_velocity)
# "[1, 0, 0, 0, 0, 0, 0]": (1, 0, 0), #"hard_break"
# "[0, 1, 0, 0, 0, 0, 0]": (0.5, 0, 0), #"medium_break"
# "[0, 0, 1, 0, 0, 0, 0]": (0 , 0, 0), #inertia
# "[0, 0, 0, 1, 0, 0, 0]": (0, 1, 0.25), #slight_accel
# "[0, 0, 0, 0, 1, 0, 0]": (0, 1, 0.5), #medium_accel
# "[0, 0, 0, 0, 0, 1, 0]": (0, 1, 0.75), #high_accel
# "[0, 0, 0, 0, 0, 0, 1]": (0, 1, 1) #"full_gas"
# }
accel_dict = { # (brake_force,motor_torque,motor_velocity)
"[1, 0, 0, 0, 0, 0, 0]": (0, 1, 0), #"hard_break"
"[0, 1, 0, 0, 0, 0, 0]": (0, 1, 0.17), #"medium_break"
"[0, 0, 1, 0, 0, 0, 0]": (0, 1, 0.33), #inertia
"[0, 0, 0, 1, 0, 0, 0]": (0, 1, 0.5), #slight_accel
"[0, 0, 0, 0, 1, 0, 0]": (0, 1, 0.67), #medium_accel
"[0, 0, 0, 0, 0, 1, 0]": (0, 1, 0.83), #high_accel
"[0, 0, 0, 0, 0, 0, 1]": (0, 1, 1) #"full_gas"
}
brake_force_norm, motor_torque_norm, motor_velocity_norm = accel_dict[
str(accel_discr)]
brake_force = self.denorm(brake_force_norm, self.min_brake,
self.max_brake)
motor_torque = self.denorm(motor_torque_norm, self.min_torque,
self.max_torque)
motor_velocity = self.denorm(motor_velocity_norm, self.min_accel,
self.max_accel)
vrep.simxSetJointForce(self.clientID, self.motor_handle, motor_torque,
vrep.simx_opmode_streaming)
vrep.simxSetJointTargetVelocity(self.clientID, self.motor_handle,
motor_velocity,
vrep.simx_opmode_streaming)
vrep.simxSetJointForce(self.clientID, self.fr_brake_handle,
brake_force, vrep.simx_opmode_streaming)
vrep.simxSetJointForce(self.clientID, self.fl_brake_handle,
brake_force, vrep.simx_opmode_streaming)
vrep.simxSetJointForce(self.clientID, self.bl_brake_handle,
brake_force, vrep.simx_opmode_streaming)
vrep.simxSetJointForce(self.clientID, self.br_brake_handle,
brake_force, vrep.simx_opmode_streaming)
##****************************************************************
location = int(inches / 12)
for cnt in contours:
(x, y, w, h) = cv2.boundingRect(cnt)
x_medium = int((x + x + w) / 2)
break
#Folloing Line of code detects the Red blob location from the centre pixel
#If the blob is located less than -30 pixel and distance greater than 1 feet away
#it it ldiplay 'left' and turn on right side motor forward
if x_medium < center - 30 and location > 1:
print('left')
errorCode = vrep.simxSetJointTargetVelocity(
clientID, left_motor_handle, 0.3,
vrep.simx_opmode_streaming)
errorCode = vrep.simxSetJointTargetVelocity(
clientID, right_motor_handle, 0.1,
vrep.simx_opmode_streaming)
errorCode = vrep.simxSetJointTargetVelocity(
clientID, left_front_motor_handle, 0.3,
vrep.simx_opmode_streaming)
errorCode = vrep.simxSetJointTargetVelocity(
clientID, right_front_motor_handle, 0.1,
vrep.simx_opmode_streaming)
#Folloing Line of code detects the Red blob location from the centre pixel
#If the blob is located greater than -30 pixel and distance greater than 1 feet away
#it it ldiplay 'right' and turn on Left side motor forward
import vrep
import cv2
import array
import numpy as np
import time
from PIL import Image as I
print('program started')
vrep.simxFinish(-1)
clientID=vrep.simxStart('127.0.0.1',19997,True,True,5000,5)
print ('Connected to remote API server')
r, colorCam = vrep.simxGetObjectHandle(clientID, "kinect_rgb", vrep.simx_opmode_oneshot_wait);
r, leftmotor = vrep.simxGetObjectHandle(clientID, "Pioneer_p3dx_leftMotor", vrep.simx_opmode_oneshot_wait);
r, rightmotor = vrep.simxGetObjectHandle(clientID, "Pioneer_p3dx_rightMotor", vrep.simx_opmode_oneshot_wait);
vrep.simxSetJointTargetVelocity(clientID, leftmotor, 0, vrep.simx_opmode_streaming);
vrep.simxSetJointTargetVelocity(clientID, rightmotor, 0, vrep.simx_opmode_streaming);
r, resolution, image = vrep.simxGetVisionSensorImage(clientID, colorCam, 1, vrep.simx_opmode_streaming);
time.sleep(0.5)
###
lm = 1
rm = -1
cx = 0
cnt = 0
###
while True:
r, resolution, image = vrep.simxGetVisionSensorImage(clientID, colorCam, 1, vrep.simx_opmode_buffer);
mat = np.asarray(image, dtype=np.uint8)
ec1, motor1_handle = vrep.simxGetObjectHandle(clientID,
'PhantomXPincher_joint1',
vrep.simx_opmode_oneshot_wait)
ec2, motor2_handle = vrep.simxGetObjectHandle(clientID,
'PhantomXPincher_joint2',
vrep.simx_opmode_oneshot_wait)
ec3, motor3_handle = vrep.simxGetObjectHandle(clientID,
'PhantomXPincher_joint3',
vrep.simx_opmode_oneshot_wait)
ec4, motor4_handle = vrep.simxGetObjectHandle(clientID,
'PhantomXPincher_joint4',
vrep.simx_opmode_oneshot_wait)
ec5, auxMotor2_handle = vrep.simxGetObjectHandle(clientID, 'uarm_auxMotor2',
vrep.simx_opmode_oneshot_wait)
ec6, gripperMotor2_handle = vrep.simxGetObjectHandle(
clientID, 'uarmGripper_motor2Method2', vrep.simx_opmode_oneshot_wait)
#vrep.simxSetJointTargetPosition(clientID, motor1_handle, pi, vrep.simx_opmode_oneshot) #rotates motor1, takes radians, default is at pi/2
#vrep.simxSetJointTargetPosition(clientID, motor2_handle, 1, vrep.simx_opmode_blocking)# motor2 and 3 work simultaneously
#vrep.simxSetJointPosition(clientID, motor2_handle, 0.5, vrep.simx_opmode_oneshot)
vrep.simxSetJointPosition(clientID, motor3_handle, 1, vrep.simx_opmode_oneshot)
"""
#If not sure about commands being received, add a 'pioneer p3dx' to the scene and run this.
_, left_motor_handle = vrep.simxGetObjectHandle(clientID, 'Pioneer_p3dx_leftMotor', vrep.simx_opmode_oneshot_wait)
vrep.simxSetJointTargetVelocity(clientID, left_motor_handle, 0.2, vrep.simx_opmode_streaming)
"""
time.sleep(1)
vrep.simxFinish(clientID)
def send_forces(self, u):
""" Apply the specified torque to the robot joints
Apply the specified torque to the robot joints, move the simulation
one time step forward, and update the position of the hand object.
Parameters
----------
u : np.array
the torques to apply to the robot
"""
# invert because VREP torque directions are opposite of expected
u *= -1
for ii, joint_handle in enumerate(self.joint_handles):
# get the current joint torque
_, torque = vrep.simxGetJointForce(
self.clientID,
joint_handle,
vrep.simx_opmode_blocking)
if _ != 0:
raise Exception('Error retrieving joint torque, ' +
'return code ', _)
# if force has changed signs,
# we need to change the target velocity sign
if np.sign(torque) * np.sign(u[ii]) <= 0:
self.joint_target_velocities[ii] *= -1
_ = vrep.simxSetJointTargetVelocity(
self.clientID,
joint_handle,
self.joint_target_velocities[ii],
vrep.simx_opmode_blocking)
if _ != 0:
raise Exception('Error setting joint target velocity, ' +
'return code ', _)
# and now modulate the force
_ = vrep.simxSetJointForce(self.clientID,
joint_handle,
abs(u[ii]), # force to apply
vrep.simx_opmode_blocking)
if _ != 0:
raise Exception('Error setting max joint force, ' +
'return code ', _)
# Update position of hand object
hand_xyz = self.robot_config.Tx(name='EE', q=self.q)
self.set_xyz('hand', hand_xyz)
# Update orientation of hand object
quaternion = self.robot_config.orientation('EE', q=self.q)
angles = transformations.euler_from_quaternion(
quaternion, axes='rxyz')
self.set_orientation('hand', angles)
# move simulation ahead one time step
vrep.simxSynchronousTrigger(self.clientID)
self.count += self.dt
def mover_para(x, y):
print("Movendo para ", x, ",", y)
print("Angulo: ", get_ang_atual())
time.sleep(1)
vel = 2
ang_alvo = get_angulo_alvo(get_pos_atual()[0],
get_pos_atual()[1], get_ang_atual(), x, y)
virar(ang_alvo)
chegou = False
while (not chegou):
dist = ler_distancias(handle_sensores)
if (dist):
salva_dados(dist,
get_pos_atual()[0],
get_pos_atual()[1], get_ang_atual())
if (dist[3] < 0.1):
vrep.simxSetJointTargetVelocity(clientID, handle_motor_dir,
-vel,
vrep.simx_opmode_streaming)
vrep.simxSetJointTargetVelocity(clientID, handle_motor_esq,
vel,
vrep.simx_opmode_streaming)
elif (dist[4] < 0.1):
vrep.simxSetJointTargetVelocity(clientID, handle_motor_dir,
vel,
vrep.simx_opmode_streaming)
vrep.simxSetJointTargetVelocity(clientID, handle_motor_esq,
-vel,
vrep.simx_opmode_streaming)
elif (dist[2] < 0.2):
vrep.simxSetJointTargetVelocity(clientID, handle_motor_dir,
-vel,
vrep.simx_opmode_streaming)
vrep.simxSetJointTargetVelocity(clientID, handle_motor_esq,
vel,
vrep.simx_opmode_streaming)
elif (dist[5] < 0.2):
vrep.simxSetJointTargetVelocity(clientID, handle_motor_dir,
vel,
vrep.simx_opmode_streaming)
vrep.simxSetJointTargetVelocity(clientID, handle_motor_esq,
-vel,
vrep.simx_opmode_streaming)
else:
ang_alvo = get_angulo_alvo(get_pos_atual()[0],
get_pos_atual()[1], get_ang_atual(),
x, y)
if (abs(get_ang_atual() - ang_alvo) > 0.1):
#print(get_ang_atual())
if ((ang_alvo > 0 and dist[7] > 1.0)
or (ang_alvo < 0 and dist[0] > 1.0)):
virar(ang_alvo)
vrep.simxSetJointTargetVelocity(clientID, handle_motor_dir,
vel,
vrep.simx_opmode_streaming)
vrep.simxSetJointTargetVelocity(clientID, handle_motor_esq,
vel,
vrep.simx_opmode_streaming)
if (abs(get_pos_atual()[0] - x) < 0.1
and abs(get_pos_atual()[1] - y) < 0.1):
chegou = True
print "chegou"
def makeMove(self,state,action):
factor = 0.8
if action == 0:
steerSpeed = factor/min(state[0],state[1])
vrep.simxSetJointTargetVelocity(clientID,left_motor_handle,0, vrep.simx_opmode_streaming)
vrep.simxSetJointTargetVelocity(clientID,right_motor_handle,steerSpeed, vrep.simx_opmode_streaming)
time.sleep(0.2)
vrep.simxSetJointTargetVelocity(clientID,left_motor_handle,vl, vrep.simx_opmode_streaming)
vrep.simxSetJointTargetVelocity(clientID,right_motor_handle,vr, vrep.simx_opmode_streaming)
time.sleep(0.1)
elif action == 1:
steerSpeed = factor/min(state[2],state[3])
vrep.simxSetJointTargetVelocity(clientID,left_motor_handle,steerSpeed, vrep.simx_opmode_streaming)
vrep.simxSetJointTargetVelocity(clientID,right_motor_handle,0, vrep.simx_opmode_streaming)
time.sleep(0.2)
vrep.simxSetJointTargetVelocity(clientID,left_motor_handle,vl, vrep.simx_opmode_streaming)
vrep.simxSetJointTargetVelocity(clientID,right_motor_handle,vr, vrep.simx_opmode_streaming)
time.sleep(0.1)
elif action == 2:
steerSpeed = 0.12/vr
vrep.simxSetJointTargetVelocity(clientID,left_motor_handle,steerSpeed, vrep.simx_opmode_streaming)
vrep.simxSetJointTargetVelocity(clientID,right_motor_handle,steerSpeed, vrep.simx_opmode_streaming)
sI = self.sensorInformation(s, clientID)
return sI
def moveLeftJoint(velocity):
print "prille"
err_left = vrep.simxSetJointTargetVelocity(Variables.clientID, Variables.leftJoint, velocity, vrep.simx_opmode_oneshot)
if err_left != vrep.simx_error_noerror and err_left == vrep.simx_error_novalue_flag:
print "Fehler beim Bewegen des linken Gelenks des Roboters dr20"
def moveSonicJoint(velocity):
err_sonic = vrep.simxSetJointTargetVelocity(Variables.clientID, Variables.sonicJoint, velocity, vrep.simx_opmode_oneshot)
if err_sonic != vrep.simx_error_noerror and err_sonic == vrep.simx_error_novalue_flag:
print "Fehler beim Bewegen des sonic Gelenks des Roboters dr20"
def _vrep_set_joint_target_velocity(self, handle, speed, opmode):
return vrep.unwrap_vrep(
vrep.simxSetJointTargetVelocity(self._clientID, handle, speed,
opmode))
def getballposition():
errorCode, position_BR = vrep.simxGetObjectPosition(
clientID, BRod_handle, -1, vrep.simx_opmode_oneshot)
errorCode, position_S = vrep.simxGetObjectPosition(
clientID, Sphere_handle, -1, vrep.simx_opmode_oneshot)
errorCode, position_RR = vrep.simxGetObjectPosition(
clientID, RRod_handle, -1, vrep.simx_opmode_oneshot)
Bv = position_S[1] - position_BR[1]
BBv = position_S[2] - position_BR[2]
Rv = position_S[1] - position_RR[1]
RRv = position_S[2] - position_RR[2]
while True:
try:
if keyboard.is_pressed('c'):
vrep.simxSetJointTargetVelocity(clientID, BRev_handle,
B_KickBallVel,
vrep.simx_opmode_oneshot_wait)
elif keyboard.is_pressed('v'):
vrep.simxSetJointTargetVelocity(clientID, BRev_handle,
R_KickBallVel,
vrep.simx_opmode_oneshot_wait)
if keyboard.is_pressed('z'):
vrep.simxSetJointTargetVelocity(clientID, BMo_handle, 0.2,
vrep.simx_opmode_oneshot_wait)
elif keyboard.is_pressed('x'):
vrep.simxSetJointTargetVelocity(clientID, BMo_handle, -0.2,
vrep.simx_opmode_oneshot_wait)
else:
vrep.simxSetJointTargetVelocity(clientID, BMo_handle, 0,
vrep.simx_opmode_oneshot_wait)
except:
break
try:
if keyboard.is_pressed('o'):
vrep.simxSetJointTargetVelocity(clientID, RRev_handle,
R_KickBallVel,
vrep.simx_opmode_oneshot_wait)
elif keyboard.is_pressed('p'):
vrep.simxSetJointTargetVelocity(clientID, RRev_handle,
B_KickBallVel,
vrep.simx_opmode_oneshot_wait)
if keyboard.is_pressed('u'):
vrep.simxSetJointTargetVelocity(clientID, RMo_handle, 0.1,
vrep.simx_opmode_oneshot_wait)
elif keyboard.is_pressed('i'):
vrep.simxSetJointTargetVelocity(clientID, RMo_handle, -0.1,
vrep.simx_opmode_oneshot_wait)
else:
vrep.simxSetJointTargetVelocity(clientID, RMo_handle, 0,
vrep.simx_opmode_oneshot_wait)
except:
break
MMMB = Bv * 2
MMMR = Rv * 2
vrep.simxSetJointTargetVelocity(clientID, BMo_handle, MMMB,
vrep.simx_opmode_oneshot_wait)
vrep.simxSetJointTargetVelocity(clientID, RMo_handle, MMMR,
vrep.simx_opmode_oneshot_wait)
def speed(handle,speed):
vrep.simxSetJointTargetVelocity(clientID,handle,speed,vrep.simx_opmode_oneshot_wait)
# if(eulerAngles[1]>3*3.1416/180):
# orientation_control = PID_fast.control(0,eulerAngles[1],1)
# if(eulerAngles[1]<-3*3.1416/180):
# orientation_control = PID_fast.control(0,eulerAngles[1],1)
#if(eulerAngles[1]>(1*3.1416/180) and eulerAngles[1]<(-1*3.1416/180)):
# orientation_control = 0
#Set body Orientation
vrep.simxSetJointTargetPosition(clientID, jointHandles[2],
-3.3 * eulerAngles[1],
vrep.simx_opmode_oneshot)
#Set Motor Velocity
vrep.simxSetJointTargetVelocity(clientID, jointHandles[0],
orientation_control,
vrep.simx_opmode_oneshot)
vrep.simxSetJointTargetVelocity(clientID, jointHandles[1],
orientation_control,
vrep.simx_opmode_oneshot)
vrep.simxPauseCommunication(clientID, 0)
vrep.simxSynchronousTrigger(clientID)
except KeyboardInterrupt:
returnCode = vrep.simxStopSimulation(clientID,
vrep.simx_opmode_blocking)
vrep.simxFinish(clientID)
print "simulation stopped!"
exit(-1)
dist = get_distance(path_transformed, np.array([0, 0]))
#loop to determine which point will be the carrot
for i in range(dist.argmin(), dist.shape[0]):
if dist[i] < lad and indx <= i:
indx = i
#mark the carrot with the sphere
returnCode = vrep.simxSetObjectPosition(
clientID, look_ahead_sphere, -1,
(path_to_track[indx, 0], path_to_track[indx, 1], 0.005),
vrep.simx_opmode_oneshot)
orient_error = np.arctan2(path_transformed[indx, 1],
path_transformed[indx, 0])
#the controllers
v_sp = d_controller.update(dist[indx])
om_sp = omega_controller.update(orient_error)
vr, vl = pioneer_robot_model(v_sp, om_sp)
errorCode_leftM = vrep.simxSetJointTargetVelocity(
clientID, left_motor_handle, vr, vrep.simx_opmode_oneshot)
errorCode_rightM = vrep.simxSetJointTargetVelocity(
clientID, right_motor_handle, vl, vrep.simx_opmode_oneshot)
count += 1
tock = time.time()
dt = tock - tick
print(dt)
else:
print("GOAAAAAAALL !!")
finally:
time.sleep(0.1)
vrep.simxStopSimulation(clientID, vrep.simx_opmode_blocking)
vrep.simxFinish(-1)
def main():
print('Program started')
vrep.simxFinish(-1) # just in case, close all opened connections
clientID = vrep.simxStart('127.0.0.1', 19997, True, True, 2000, 5)
if clientID != -1:
print('Connected to remote API server')
emptyBuff = bytearray()
# Start the simulation:
vrep.simxStartSimulation(clientID, vrep.simx_opmode_oneshot_wait)
# initiaize robot
# Retrieve wheel joint handles:
wheelJoints = np.empty(4, dtype=np.int)
wheelJoints.fill(-1) #front left, rear left, rear right, front right
res, wheelJoints[0] = vrep.simxGetObjectHandle(
clientID, 'rollingJoint_fl', vrep.simx_opmode_oneshot_wait)
res, wheelJoints[1] = vrep.simxGetObjectHandle(
clientID, 'rollingJoint_rl', vrep.simx_opmode_oneshot_wait)
res, wheelJoints[2] = vrep.simxGetObjectHandle(
clientID, 'rollingJoint_rr', vrep.simx_opmode_oneshot_wait)
res, wheelJoints[3] = vrep.simxGetObjectHandle(
clientID, 'rollingJoint_fr', vrep.simx_opmode_oneshot_wait)
# set wheel velocity to 0
for i in range(0, 4):
vrep.simxSetJointTargetVelocity(clientID, wheelJoints[i], 0,
vrep.simx_opmode_oneshot)
#ste first wheel velocities to 5 m/s for 2 s
r = 50.0 #radius of wheels
l1 = 300.46 / 2.0 #D/2
l2 = 471.0 / 2.0 #C/2
"""
vx = 0
vy = 0
w0 = 1
vel1 = 1/r * (1*vx + 1 * vy -(l1+l2)*w0)
vel2 = 1/r * (1*vx -1 * vy +(l1+l2)*w0)
vel3 = 1/r * (1*vx -1 * vy -(l1+l2)*w0)
vel4 = 1/r * (1*vx + 1 * vy +(l1+l2)*w0)
"""
res, base = vrep.simxGetObjectHandle(clientID, 'youBot_center',
vrep.simx_opmode_oneshot_wait)
res, base_pos = vrep.simxGetObjectPosition(
clientID, base, -1, vrep.simx_opmode_oneshot_wait)
res, base_orient = vrep.simxGetObjectOrientation(
clientID, base, -1, vrep.simx_opmode_oneshot_wait)
vrep.simxGetPingTime(clientID)
#print(base,"\n",base_pos,"\n",base_orient,"\n")
res = vrep.simxSetIntegerSignal(clientID, 'handle_xy_sensor', 2,
vrep.simx_opmode_oneshot)
vrep.simxSetIntegerSignal(clientID, 'displaylasers', 1,
vrep.simx_opmode_oneshot)
res, hokuyo1 = vrep.simxGetObjectHandle(clientID, 'fastHokuyo_sensor1',
vrep.simx_opmode_oneshot_wait)
res, hokuyo2 = vrep.simxGetObjectHandle(clientID, 'fastHokuyo_sensor2',
vrep.simx_opmode_oneshot_wait)
res, goalBase = vrep.simxGetObjectHandle(clientID, 'Goal',
vrep.simx_opmode_oneshot_wait)
res, goal = vrep.simxGetObjectPosition(clientID, goalBase, -1,
vrep.simx_opmode_oneshot_wait)
#print(goal)
#turn around in staart pos
#wheelVelocities = vel(0,0,1.0,r,l1,l2)
#drive(wheelVelocities, math.pi, clientID,wheelJoints)
#aim for goal
angle = aimGoal(base_pos, goal, base_orient)
wheelVelocities = vel(0, 0, 1.0, r, l1, l2)
drive(wheelVelocities, angle[2], clientID, wheelJoints)
res, aux, auxD = vrep.simxReadVisionSensor(clientID, hokuyo1,
vrep.simx_opmode_streaming)
res, aux, auxD = vrep.simxReadVisionSensor(clientID, hokuyo1,
vrep.simx_opmode_buffer)
print(auxD)
#print(math.atan2(-3,3))
#print(angle[2])
"""
#7 straight
wheelVelocities = vel(-0.5,0,0,r,l1,l2)
drive(wheelVelocities, 14.0,clientID, wheelJoints)
#turn
wheelVelocities = vel(0,0,1.0,r,l1,l2)
drive(wheelVelocities, math.pi, clientID,wheelJoints)
#7 straight
wheelVelocities = vel(-0.5,0,0,r,l1,l2)
drive(wheelVelocities, 14, clientID,wheelJoints)
#turn
wheelVelocities = vel(0,0,1.0,r,l1,l2)
drive(wheelVelocities, math.pi,clientID,wheelJoints)
#7 straight
wheelVelocities = vel(-0.5,0,0,r,l1,l2)
drive(wheelVelocities, 14, clientID,wheelJoints)
#turn
wheelVelocities = vel(0,0,1.0,r,l1,l2)
drive(wheelVelocities, math.pi, clientID,wheelJoints)
#7 straight
wheelVelocities = vel(-0.5,0,0,r,l1,l2)
drive(wheelVelocities, 14, clientID,wheelJoints)
#turn
wheelVelocities = vel(0,0,1.0,r,l1,l2)
drive(wheelVelocities, math.pi, clientID,wheelJoints)
"""
res, base = vrep.simxGetObjectHandle(clientID, 'youBot_center',
vrep.simx_opmode_oneshot_wait)
base_pos = vrep.simxGetObjectPosition(clientID, base, -1,
vrep.simx_opmode_oneshot_wait)
base_orient = vrep.simxGetObjectOrientation(
clientID, base, -1, vrep.simx_opmode_oneshot_wait)
vrep.simxGetPingTime(clientID)
# print(base,"\n",base_pos,"\n",base_orient,"\n")
# set wheel velocity to 0
for i in range(0, 4):
vrep.simxSetJointTargetVelocity(clientID, wheelJoints[i], 0,
vrep.simx_opmode_oneshot)
# Stop simulation:
vrep.simxStopSimulation(clientID, vrep.simx_opmode_oneshot_wait)
# Now close the connection to V-REP:
vrep.simxFinish(clientID)
else:
print('Failed connecting to remote API server')
print('Program ended')
#Initialize the sensor class
sensor = gtg.Sensor()
sensor.u1 = u1_val
sensor.u2 = u3_val
sensor.u3 = u2_val
#Initialize a start position
movil.setPose(0,0,0)
#Initialize the desired position
d_position = [1,1]
error = [0,0]
old_state = 1
current_state = 1
#time.sleep(3)
#Se asigna una vleocidad inicial
vrep.simxSetJointTargetVelocity(clientID,left_motor_handle,0, vrep.simx_opmode_streaming)
vrep.simxSetJointTargetVelocity(clientID,right_motor_handle,0, vrep.simx_opmode_streaming)
while np.sqrt((movil.x - d_position[0])**2 + (movil.y - d_position[1])**2) > 0.1:
errorCode,detectionState,detectedPoint1,detectedObjectHandle,detectedSurfaceNormalVector=vrep.simxReadProximitySensor(clientID,ult_handle1,vrep.simx_opmode_buffer)
sensor.u1 = 1.1*np.linalg.norm(detectedPoint1)
errorCode,detectionState,detectedPoint2,detectedObjectHandle,detectedSurfaceNormalVector=vrep.simxReadProximitySensor(clientID,ult_handle2,vrep.simx_opmode_buffer)
sensor.u2 = 1.1*np.linalg.norm(detectedPoint2)
errorCode,detectionState,detectedPoint3,detectedObjectHandle,detectedSurfaceNormalVector=vrep.simxReadProximitySensor(clientID,ult_handle3,vrep.simx_opmode_buffer)
sensor.u3 = 1.1*np.linalg.norm(detectedPoint3)
#Se crea la condicion de cambio de estado,
if ((sensor.u1 <= 0.05) or (sensor.u2 <= 0.05) or (sensor.u3 <= 0.05)):
old_state = current_state
current_state = 2
omega = 0
# if omega > 2.5:
# omega = 2.5
# elif omega < -2.5:
# omega = -2.5
if estadosDeteccion[2] == True and puntosDetectados[2] < 0.8:
print('Imminent collision at ' + str(puntosDetectados[4]))
omega = -1.5 - 0.1 * random.random()
v = 0.1 + 0.1 * random.random()
if estadosDeteccion[5] == True and puntosDetectados[5] < 0.8:
print('Imminent collision at ' + str(puntosDetectados[4]))
omega = 1.5 + 0.1 * random.random()
v = 0.1 + 0.1 * random.random()
ul = v / r - L * omega / (2 * r)
ur = v / r + L * omega / (2 * r)
errf = vrep.simxSetJointTargetVelocity(clientID, motorL, ul,
vrep.simx_opmode_streaming)
errf = vrep.simxSetJointTargetVelocity(clientID, motorR, ur,
vrep.simx_opmode_streaming)
time.sleep(0.005)
plt.figure(1)
plt.imshow(tocc + matrizDeOcupacion)
plt.show()
np.savetxt('map.txt', tocc + matrizDeOcupacion)
vrep.simxStopSimulation(clientID, vrep.simx_opmode_oneshot)
# controlling the max torque allowed (yeah, i know)
# get the current joint torque
_, torque = \
vrep.simxGetJointForce(clientID,
joint_handle,
vrep.simx_opmode_blocking)
if _ !=0 : raise Exception()
# if force has changed signs,
# we need to change the target velocity sign
if np.sign(torque) * np.sign(u[ii]) <= 0:
joint_target_velocities[ii] = \
joint_target_velocities[ii] * -1
vrep.simxSetJointTargetVelocity(clientID,
joint_handle,
joint_target_velocities[ii], # target velocity
vrep.simx_opmode_blocking)
if _ !=0 : raise Exception()
# and now modulate the force
vrep.simxSetJointForce(clientID,
joint_handle,
abs(u[ii]), # force to apply
vrep.simx_opmode_blocking)
if _ !=0 : raise Exception()
# move simulation ahead one time step
vrep.simxSynchronousTrigger(clientID)
count += dt
else:
raise Exception('Failed connecting to remote API server')
def set_target_velocity(self, vel):
# Control joint by velocity - VREP applies approprite torque to reach the velocity
rc = vrep.simxSetJointTargetVelocity(clientID, self.handle, vel,
oneshot)
def followpath(path, objectHandle, port):
vrep.simxFinish(-1)
clientID = vrep.simxStart('127.0.0.1', port, True, True, 5000, 5)
if clientID != -1:
prev_time = 0
pos_on_path = 1
dis = 0
path_pos = {}
flag = 0
x = 0
check = 0
emptyBuff = bytearray()
ultrasonic = []
_, robotHandle = vrep.simxGetObjectHandle(
clientID, 'Start', vrep.simx_opmode_oneshot_wait)
_, targetHandle = vrep.simxGetObjectHandle(
clientID, 'End', vrep.simx_opmode_oneshot_wait)
_, lm = vrep.simxGetObjectHandle(clientID, 'LeftJoint',
vrep.simx_opmode_oneshot_wait)
_, rm = vrep.simxGetObjectHandle(clientID, 'RightJoint',
vrep.simx_opmode_oneshot_wait)
_, ebot = vrep.simxGetObjectHandle(clientID, 'eBot',
vrep.simx_opmode_oneshot_wait)
_, picksubs = vrep.simxGetObjectHandle(clientID, 'Cuboid14',
vrep.simx_opmode_oneshot_wait)
res, retInts, retFloats, retStrings, retBuffer = vrep.simxCallScriptFunction(
clientID, 'Dummy', vrep.sim_scripttype_childscript,
'threadFunction', [], path, [], emptyBuff,
vrep.simx_opmode_oneshot_wait)
#print(len(retFloats))
while (1):
ultrasonic = fun.stop_function(19999)
check = 0
if (ultrasonic[0] < 0.5 or ultrasonic[1] < 0.5
or ultrasonic[2] < 0.5):
check = 1
print("danger")
if (check == 0):
if (time.time() - prev_time) > 100:
if (not flag):
retFloats = path_5_sec(clientID, path)
sleep(0.1)
pos_on_path = 1
prev_time = time.time()
emptyBuff = bytearray()
_, _, dis, _, _ = vrep.simxCallScriptFunction(
clientID, 'Dummy', vrep.sim_scripttype_childscript,
'follow', [pos_on_path], retFloats, [], emptyBuff,
vrep.simx_opmode_blocking)
v_des = -0.02
om_des = -0.8 * dis[1]
d = 0.06
v_r = (v_des + d * om_des)
v_l = (v_des - d * om_des)
r_w = 0.0275
omega_right = (v_r / r_w)
omega_left = (v_l / r_w)
_, _, _, _, _ = vrep.simxCallScriptFunction(
clientID, 'Dummy', vrep.sim_scripttype_childscript,
'veloctiyRos', [], [omega_right, omega_right], [],
emptyBuff, vrep.simx_opmode_blocking)
_ = vrep.simxSetJointTargetVelocity(
clientID, lm, (-1 * omega_left),
vrep.simx_opmode_oneshot_wait)
_ = vrep.simxSetJointTargetVelocity(
clientID, rm, (-1 * omega_right),
vrep.simx_opmode_oneshot_wait)
if (dis[0] < 0.1):
pos_on_path += 3
if (flag == 1):
if (math.sqrt((path[0] - retFloats[pos_on_path - 1])**2 +
(path[1] - retFloats[pos_on_path])**2) <
0.01):
_ = vrep.simxSetJointTargetVelocity(
clientID, lm, 0, vrep.simx_opmode_oneshot_wait)
_ = vrep.simxSetJointTargetVelocity(
clientID, rm, 0, vrep.simx_opmode_oneshot_wait)
print("exit")
x = 1
break
if (x == 1):
break
print(
math.sqrt((path[0] - retFloats[pos_on_path - 1])**2 +
(path[1] - retFloats[pos_on_path])**2))
if (math.sqrt((path[0] - retFloats[pos_on_path - 1])**2 +
(path[1] - retFloats[pos_on_path])**2) < 0.3):
flag = 1
if (objectHandle):
_ = vrep.simxSetObjectPosition(
clientID, picksubs, ebot, [0, 0, 0.052],
vrep.simx_opmode_oneshot_wait)
else:
_ = vrep.simxSetJointTargetVelocity(
clientID, lm, 0, vrep.simx_opmode_oneshot_wait)
_ = vrep.simxSetJointTargetVelocity(
clientID, rm, 0, vrep.simx_opmode_oneshot_wait)
def setWheelVelocity(clientID, velocity):
wheelJoints = getWheelJoints(clientID)
for i in range(0, 4):
vrep.simxSetJointTargetVelocity(clientID, wheelJoints[i], velocity,
vrep.simx_opmode_oneshot)
def run_onlyimu_training(sim_number, sim_time):
print("start running onlyIMU model")
prefs.codegen.target = "numpy"
start_scope()
# number of sensor in robot
N_sensor = 8
N_vision = 2
# robot radius
r = 0.0586
# CD neurons number
N_CD = 36
# vision detection_neurons number
N_VD = 10
# HD neuron number
N_HD = 72
# coordinate neuron
N_x_axis, N_y_axis = 32, 32
N_PI = N_x_axis * N_y_axis
# Simulation time
# Speed cell number
N_speed = 6
m = 5 # 60
iter = 0
# width and length of the arena x=width/ y=lenght
# 1 square in vrep = 0.5*0.5
x_scale = 2
y_scale = 2
# distance unit per neuron
N = 200 / (N_x_axis * N_y_axis)
# define the initial mismatch encounter
last_mismatch = 0
##--------------------------------------------------------------------------------------------------------------------##
##--------------------Collision detection Neural Architecture--------------------------------------------##
##--------------------------------------------------------------------------------------------------------------------##
Notred_red_inh, spikemon_red, spikemon_notred, spikemon_estimatedlandmark, spikemon_nonlandmark, wall_landmark_synapse, landmark_wall_synapse, Poisson_group, Poisson_vision, Collision_neuron, Collision_or_not, Color, Wall, Landmark, Poisson_synapse, PoissonVision_synapse, Self_ex_synapse, Self_ex_synapse_color, Self_in_synapse, Self_in_synapse_color, Collide_or_not_synapse, Red_landmark_ex, Red_wall_inh, Notred_landmark_inh, spikemon_CD, spikemon_collision, spikemon_landmark, spikemon_poisson, spikemon_wall, Poisson_non_collision, Non_collision_neuron, Poisson_non_synapse, CON_noncollision_synapse, Non_collision_color_synapses, spikemon_noncollison, Estimated_landmark_neuron, Non_landmark_neuron, Poisson_nonlandmark_synapse, Landmark_nonlandmark_synapse, CON_landmark_ex, CON_wall_ex, Notred_wall_ex, Red, Notred, color_notred_synapses, color_red_synapses = CD_net(
N_CD, N_vision, N_VD)
# gaussian input for 8 collison sensors
stimuli = np.array([
gaussian_spike(N_CD, j * N_CD / N_sensor, 10, 0.2)
for j in range(N_sensor)
])
# gaussion distribution for 2 vision input(wall or landmark)
stimuli_vision = np.array([
gaussian_spike(N_VD, j * N_VD / N_vision, 10, 0.2)
for j in range(N_vision)
])
##--------------------------------------------------------------------------------------------------------------------##
##--------------------Head Direction and position Neural Architecture----------------------------------------------------------##
##--------------------------------------------------------------------------------------------------------------------##
IHD_in_synapse, str_inh_synapse, Poisson_straight, Go_straight_neuron, Go_straight, HD_str_synapse, str_IHD_synapse, sti_PI, sti, Poisson_Left, Poisson_Right, Poisson_Compass, Left_drive, Right_drive, HD_Neuron, IHD_Neuron, Left_shift_neuron, Left_shift_speed_neuron, Right_shift_neuron, Right_shift_speed_neuron, HD_Left_synapse, Left_IHD_synapse, HD_Right_synapse, Left_drive_synapse, Right_drive_synapse, Right_IHD_synapse, HD_IHD_synapse, Reset_synapse, HD_ex_synapse, HD_in_synapse, spikemon_HD, spikemon_IHD, Poisson_PI, PI_Neurons, IPI_Neurons, Directional_Neurons, HD_directional_synapse, directional_PI_synapse, PI_shifting_neurons, North_shifting_neurons, South_shifting_neurons, East_shifting_neurons, West_shifting_neurons, PI_ex_synapse, PI_in_synapse, PI_N_synapse, PI_S_synapse, PI_E_synapse, PI_W_synapse, IPI_N_synapse, IPI_S_synapse, IPI_E_synapse, IPI_W_synapse, IPI_PI_synapse, PI_Reset_synapse, spikemon_PI, spikemon_IPI, NE_shifting_neurons, SE_shifting_neurons, WS_shifting_neurons, WN_shifting_neurons, PI_NE_synapse, PI_SE_synapse, PI_WS_synapse, PI_WN_synapse, IPI_NE_synapse, IPI_SE_synapse, IPI_WS_synapse, IPI_WN_synapse, Left_inh_synapse, Right_IHD_synapse, IPI_in_synapse, IPI_stay_synapse, Stay_stay_layer, Stay_layer, Stay, PI_stay_synapse = HD_PI_integrated_net(
N_HD, N_speed, N_x_axis, N_y_axis, sim_time)
##-----------------head direction correcting matrix network-------------#
# Poisson_IMU,IMU_Neuron,HD_errormatrix_neurons,HD_positive_error,HD_negative_error,IMU_poi_synapses,IMU_errors_connecting_synapses,HD_error_connect_synapses,Error_negative_synapses,Error_positive_synapses,Ex_speed_pool_Neurons,Inh_speed_pool_Neurons,Positive_ex_pool_synapses,Negative_inh_pool_synapses,statemon_positiveHD_error,statemon_negativeHD_error,spikemon_positiveHD_error,spikemon_negativeHD_error,spiketrain_pos=HD_error_correcting(N_HD,HD_Neuron)
##--------------------------------------------------------------------------------------------------------------------##
##--------------------Fusi Synapse between Position - Landmark Neuron----------------------------------------------------------##
##--------------------------------------------------------------------------------------------------------------------##
'''measure collision weight by fusi synapse landmark'''
landmark_PI_plastic = fusi_landmark(PI_Neurons, Landmark)
w_landmark_plas_shape = np.shape(landmark_PI_plastic.w_fusi_landmark)[0]
w_plastic_landmark = landmark_PI_plastic.w_fusi_landmark
all_fusi_weights_landmark = landmark_PI_plastic.w_fusi_landmark
##--------------------------------------------------------------------------------------------------------------------##
##--------------------Fusi Synapse between Position - Wall Neuron----------------------------------------------------------##
##--------------------------------------------------------------------------------------------------------------------##
'''measure collision weight by fusi synapse landmark'''
wall_PI_plastic = fusi_wall(PI_Neurons, Wall)
w_plas_shape_wall = np.shape(wall_PI_plastic.w_fusi_wall)[0]
w_plastic_wall = wall_PI_plastic.w_fusi_wall
all_fusi_weights_wall = wall_PI_plastic.w_fusi_wall
##--------------------------------------------------------------------------------------------------------------------##
##-------------------------------------------Mismatch Network--------------------------------------------------------------##
##--------------------------------------------------------------------------------------------------------------------##
'''store previously estimated landmark position by fusi synapse estimatedlandmark'''
preeq = '''
v_post += 1*(w_piest >= 0.5)
'''
PI_Neurons_est_synapse = Synapses(PI_Neurons,
Estimated_landmark_neuron,
'w_piest : 1',
on_pre=preeq)
PI_Neurons_est_synapse.connect()
Mismatch_landmark_inh_synpase, landmark_mismatch_inh_synapse, spikemon_nonestimatedlandmark, NonEst_landmark_poisson, Non_estimatedlandmark_neuron, Mismatch_neuron, Non_Mismatch_neuron, Nonestimatedlandmark_poi_synapse, Est_nonest_inh_synapse, NonCol_mismatch_synapse, NonCol_nonmismatch_synapse, NonEst_mistmatch_inh_synapse, Est_mismatch_ex_synapse, Est_nonmismatch_inh_synapses, Mismatch_est_inh_synpase, spikemon_Mismatch = mismatch_net(
Non_landmark_neuron, Estimated_landmark_neuron, Landmark)
##--------------------------------------------------------------------------------------------------------------------##
##----------------------------------------------Network--------------------------------------------------------------##
##--------------------------------------------------------------------------------------------------------------------##
print("adding network")
# Network
PINet = Network()
# add collision network
PINet.add(Notred_red_inh, spikemon_red, spikemon_notred,
spikemon_estimatedlandmark, spikemon_nonlandmark,
wall_landmark_synapse, landmark_wall_synapse, Poisson_group,
Poisson_vision, Collision_neuron, Collision_or_not, Color, Wall,
Landmark, Poisson_synapse, PoissonVision_synapse,
Self_ex_synapse, Self_ex_synapse_color, Self_in_synapse,
Self_in_synapse_color, Collide_or_not_synapse, Red_landmark_ex,
Red_wall_inh, Notred_landmark_inh, spikemon_CD,
spikemon_collision, spikemon_landmark, spikemon_poisson,
spikemon_wall, Poisson_non_collision, Non_collision_neuron,
Poisson_non_synapse, CON_noncollision_synapse,
Non_collision_color_synapses, spikemon_noncollison,
Estimated_landmark_neuron, Non_landmark_neuron,
Poisson_nonlandmark_synapse, Landmark_nonlandmark_synapse,
CON_landmark_ex, CON_wall_ex, Notred_wall_ex, Red, Notred,
color_notred_synapses, color_red_synapses)
# add position network
PINet.add(
IHD_in_synapse, str_inh_synapse, Poisson_straight, Go_straight_neuron,
Go_straight, HD_str_synapse, str_IHD_synapse, Poisson_Left,
Poisson_Right, Poisson_Compass, Left_drive, Right_drive, HD_Neuron,
IHD_Neuron, Left_shift_neuron, Left_shift_speed_neuron,
Right_shift_neuron, Right_shift_speed_neuron, HD_Left_synapse,
Left_IHD_synapse, HD_Right_synapse, Left_drive_synapse,
Right_drive_synapse, Right_IHD_synapse, HD_IHD_synapse, Reset_synapse,
HD_ex_synapse, HD_in_synapse, spikemon_HD, spikemon_IHD, Poisson_PI,
PI_Neurons, IPI_Neurons, Directional_Neurons, HD_directional_synapse,
directional_PI_synapse, PI_shifting_neurons, North_shifting_neurons,
South_shifting_neurons, East_shifting_neurons, West_shifting_neurons,
PI_ex_synapse, PI_in_synapse, PI_N_synapse, PI_S_synapse, PI_E_synapse,
PI_W_synapse, IPI_N_synapse, IPI_S_synapse, IPI_E_synapse,
IPI_W_synapse, IPI_PI_synapse, PI_Reset_synapse, spikemon_PI,
spikemon_IPI, NE_shifting_neurons, SE_shifting_neurons,
WS_shifting_neurons, WN_shifting_neurons, PI_NE_synapse, PI_SE_synapse,
PI_WS_synapse, PI_WN_synapse, IPI_NE_synapse, IPI_SE_synapse,
IPI_WS_synapse, IPI_WN_synapse, Left_inh_synapse, Right_IHD_synapse,
IPI_in_synapse, IPI_stay_synapse, Stay_stay_layer, Stay_layer, Stay,
PI_stay_synapse)
# add fusi plastic synapses for landmark,wall neurons with PI neurons
PINet.add(landmark_PI_plastic)
PINet.add(wall_PI_plastic)
# add mismatch network
PINet.add(PI_Neurons_est_synapse, Mismatch_landmark_inh_synpase,
landmark_mismatch_inh_synapse, spikemon_nonestimatedlandmark,
NonEst_landmark_poisson, Non_estimatedlandmark_neuron,
Mismatch_neuron, Non_Mismatch_neuron,
Nonestimatedlandmark_poi_synapse, Est_nonest_inh_synapse,
NonCol_mismatch_synapse, NonCol_nonmismatch_synapse,
NonEst_mistmatch_inh_synapse, Est_mismatch_ex_synapse,
Est_nonmismatch_inh_synapses, Mismatch_est_inh_synpase,
spikemon_Mismatch)
##-----------------------------------------------------------------------------------------##
##---------------------------------Robot controller----------------------------------------##
##-----------------------------------------------------------------------------------------##
# Connect to the server
print('finished adding network')
vrep.simxFinish(-1) # just in case, close all opened connections
clientID = vrep.simxStart('127.0.0.1', 19999, True, True, 5000, 5)
if clientID != -1:
print("Connected to remote API server")
else:
print("Not connected to remote API server")
sys.exit("Could not connect")
vrep.simxSynchronous(clientID, 1)
##----------------------------------Controller initialized------------------------------------------##
# set motor
err_code, l_motor_handle = vrep.simxGetObjectHandle(
clientID, "KJunior_motorLeft", vrep.simx_opmode_blocking)
err_code, r_motor_handle = vrep.simxGetObjectHandle(
clientID, "KJunior_motorRight", vrep.simx_opmode_blocking)
# Compass output=orientation
# define robot
err_code, robot = vrep.simxGetObjectHandle(clientID, 'KJunior',
vrep.simx_opmode_blocking)
# define Angles
err_code, Angles = vrep.simxGetObjectOrientation(
clientID, robot, -1, vrep.simx_opmode_streaming)
# define object position
err_code, Position = vrep.simxGetObjectPosition(clientID, robot, -1,
vrep.simx_opmode_streaming)
# get sensor
sensor1, sensor2, sensor3, sensor4, sensor5, sensor6, sensor7, sensor8 = getSensor(
clientID)
# read point
detectedPoint1, detectedPoint2, detectedPoint3, detectedPoint4, detectedPoint5, detectedPoint6, detectedPoint7, detectedPoint8 = getDetectedpoint(
sensor1, sensor2, sensor3, sensor4, sensor5, sensor6, sensor7, sensor8,
clientID)
# Distance from sensor to obstacle
sensor_val1, sensor_val2, sensor_val3, sensor_val4, sensor_val5, sensor_val6, sensor_val7, sensor_val8 = getSensorDistance(
detectedPoint1, detectedPoint2, detectedPoint3, detectedPoint4,
detectedPoint5, detectedPoint6, detectedPoint7, detectedPoint8)
# get sensor for mismatch landmark detection
err_code, sensorMismatch = vrep.simxGetObjectHandle(
clientID, "Proximity_sensor", vrep.simx_opmode_blocking)
err_code, detectionStateMismatch, detectedPointMismatch, detectedObjectHandleMismatch, detectedSurfaceNormalVectorMismatch = vrep.simxReadProximitySensor(
clientID, sensorMismatch, vrep.simx_opmode_streaming)
mismatch_sensordistance = np.linalg.norm(detectedPointMismatch)
# get vision sensor
err_code, camera = vrep.simxGetObjectHandle(clientID, "Vision_sensor",
vrep.simx_opmode_blocking)
err_code, resolution, image = vrep.simxGetVisionSensorImage(
clientID, camera, 1, vrep.simx_opmode_streaming)
##---------------------------------Sensor initial condition (for CD)------------------------------------##
# sensor value of every sensor for every neuron
# Use inv_filter to filter out noise
# sensor_val = np.array([np.repeat(inv_filter(sensor_val1),N_CD), np.repeat(inv_filter(sensor_val2),N_CD),np.repeat(inv_filter(sensor_val3),N_CD),np.repeat(inv_filter(sensor_val7),N_CD),np.repeat(inv_filter(sensor_val8),N_CD)])
sensor_val = np.array([
np.repeat(inv_filter(sensor_val1), N_CD),
np.repeat(inv_filter(sensor_val2), N_CD),
np.repeat(inv_filter(sensor_val3), N_CD),
np.repeat(inv_filter(sensor_val4), N_CD),
np.repeat(inv_filter(sensor_val5), N_CD),
np.repeat(inv_filter(sensor_val6), N_CD),
np.repeat(inv_filter(sensor_val7), N_CD),
np.repeat(inv_filter(sensor_val8), N_CD)
])
sensor_val_vision = np.zeros([N_vision, N_VD])
# sum of each sensor value * its gaussian distribution --> sum to find all activity for each neurons --> WTA
All_stimuli = np.sum(sensor_val * stimuli, axis=0)
All_stimuli_vision = np.sum(sensor_val_vision * stimuli_vision, axis=0)
# find the winner
winner = WTA(All_stimuli)
for w in winner:
Poisson_synapse.w_poi[w] = All_stimuli[w]
winner_vision = WTA(All_stimuli_vision)
for w in winner_vision:
PoissonVision_synapse.w_vision_poi[w] = All_stimuli_vision[w]
##------------------------------------------------------------------------------------------------##
# initial speed of wheel
l_steer = 0.24
r_steer = 0.24
# record the initial time
t_int = time.time()
# record compass angle
compass_angle = np.array([])
# record x and y axis at each time
x_axis = np.array([])
y_axis = np.array([])
# path that is passing
r = np.array([])
# all_time
all_time = np.array([])
# collision index that the robot collided
collision_index = np.array([])
collision_index_during_run = []
# record weight parameter
# record every m sec
weight_record_time = 1
weight_during_run_landmark = np.zeros(
[np.shape(w_plastic_landmark)[0], 3 * int(sim_time / 5)])
weight_during_run_wall = np.zeros(
[np.shape(w_plastic_wall)[0], 3 * int(sim_time / 5)])
time_step_list = np.array([])
# start simulation
while (time.time() - t_int) < sim_time:
t1 = time.time() - t_int
# record proximity sensor at each time step
sensor_val1, sensor_val2, sensor_val3, sensor_val4, sensor_val5, sensor_val6, sensor_val7, sensor_val8 = getSensorDistance(
detectedPoint1, detectedPoint2, detectedPoint3, detectedPoint4,
detectedPoint5, detectedPoint6, detectedPoint7, detectedPoint8)
all_sensor = np.array(
[sensor_val1, sensor_val2, sensor_val3, sensor_val4, sensor_val5])
all_sensor[all_sensor < 4.1e-20] = np.infty
activated_sensor = np.argmin(all_sensor)
# obtain vision sensor values
pixelimage = set(image)
if all_sensor[activated_sensor] < 0.1:
if activated_sensor == 3:
r_steer, l_steer, zeta = TurnRight(r_steer, l_steer, delta_t)
elif activated_sensor == 4:
r_steer, l_steer, zeta = TurnRight(r_steer, l_steer, delta_t)
elif activated_sensor == 0:
r_steer, l_steer, zeta = TurnLeft(r_steer, l_steer, delta_t)
elif activated_sensor == 1:
r_steer, l_steer, zeta = TurnLeft(r_steer, l_steer, delta_t)
elif activated_sensor == 2:
r_steer, l_steer, zeta = TurnLeft(r_steer, l_steer, delta_t)
else:
l_steer = 0.24
r_steer = 0.24
zeta = 0
else:
l_steer = 0.24
r_steer = 0.24
zeta = 0
####-------------------- Record weight------------------------####
weight_during_run_landmark[:, iter] = w_plastic_landmark
weight_during_run_wall[:, iter] = w_plastic_wall
collision_index_during_run.append(collision_index)
####-------------------- Start recording spike (CD) ------------------------####
# cleaning noises for the collision distance sensor value
sensor_val = np.array([
np.repeat(inv_filter(sensor_val1), N_CD),
np.repeat(inv_filter(sensor_val2), N_CD),
np.repeat(inv_filter(sensor_val3), N_CD),
np.repeat(inv_filter(sensor_val4), N_CD),
np.repeat(inv_filter(sensor_val5), N_CD),
np.repeat(inv_filter(sensor_val6), N_CD),
np.repeat(inv_filter(sensor_val7), N_CD),
np.repeat(inv_filter(sensor_val8), N_CD)
])
# get the vision sensor value
sensor_val_vision = generate_vision_val(pixelimage, N_VD)
# reset weight to 0 again
Poisson_synapse.w_poi = 0
PoissonVision_synapse.w_vision_poi = 0
err_code = vrep.simxSetJointTargetVelocity(clientID, l_motor_handle,
l_steer,
vrep.simx_opmode_streaming)
err_code = vrep.simxSetJointTargetVelocity(clientID, r_motor_handle,
r_steer,
vrep.simx_opmode_streaming)
# All stimuli and WTA
All_stimuli = (np.sum(sensor_val * stimuli, axis=0))
winner = WTA(All_stimuli)
for w in winner:
Poisson_synapse.w_poi[w] = All_stimuli[w]
All_stimuli_vision = np.sum(sensor_val_vision * stimuli_vision, axis=0)
winner_vision = WTA(All_stimuli_vision)
for w in winner_vision:
PoissonVision_synapse.w_vision_poi[w] = All_stimuli_vision[w] / 10
# --------mismatching detecting---------##
PI_Neurons_est_synapse.w_piest = landmark_PI_plastic.w_fusi_landmark
####-------------------- End recording spike ----------------------------####
####-------------------- Start recording Head direction ------------------------####
# Choose neuron that is the nearest to the turning speed/direction
# if turn left
if r_steer == 0:
neuron_index = nearest_neuron_speed(zeta, N_speed)
for syn in range(N_speed):
Left_drive_synapse[syn].w_left_drive = 0
Right_drive_synapse[syn].w_right_drive = 0
Left_drive_synapse[neuron_index].w_left_drive = 5
Right_drive_synapse[neuron_index].w_right_drive = 0
Go_straight.w_str = -3
directional_PI_synapse.w_dir_PI = 0
Stay_stay_layer.w_stay_stay = 2
# if turn right
elif l_steer == 0:
neuron_index = nearest_neuron_speed(zeta, N_speed)
for syn in range(N_speed):
Left_drive_synapse[syn].w_left_drive = 0
Right_drive_synapse[syn].w_right_drive = 0
Left_drive_synapse[neuron_index].w_left_drive = 0
Right_drive_synapse[neuron_index].w_right_drive = 5
Go_straight.w_str = -3
directional_PI_synapse.w_dir_PI = 0 # if turn position PI stay
Stay_stay_layer.w_stay_stay = 2
# if go straight
else:
for syn in range(N_speed):
Left_drive_synapse[syn].w_left_drive = 0
Right_drive_synapse[syn].w_right_drive = 0
Go_straight.w_str = 10
directional_PI_synapse.w_dir_PI = 4 # if move position PI run
Stay_stay_layer.w_stay_stay = -4
##-----------------reset IHD (Compass) -----------------##
# Get actual heading direction
err_code, Angles = vrep.simxGetObjectOrientation(
clientID, robot, -1, vrep.simx_opmode_streaming)
heading_dir = getHeadingdirection(Angles)
compass_angle = np.append(compass_angle, heading_dir)
# recalibrate head direction to nearest neuron
recal = nearest_neuron_head(heading_dir, N_HD)
# set reset by compass weight upon angle atm
Reset_synapse.w_reset = np.array(gaussian_spike(N_HD, recal, 30, 0.03))
# print("reset synapse",Reset_synapse.w_reset)
##-----------------head direction error correction -----------------##
if len(spikemon_Mismatch) > last_mismatch:
print('encounter mismatch')
err_code, detectionStateMismatch, detectedPointMismatch, detectedObjectHandleMismatch, detectedSurfaceNormalVectorMismatch = vrep.simxReadProximitySensor(
clientID, sensorMismatch, vrep.simx_opmode_streaming)
mismatch_sensordistance = np.linalg.norm(detectedPointMismatch)
while mismatch_sensordistance <= 0.01:
l_steer = 0
r_steer = 0.5
err_code = vrep.simxSetJointTargetVelocity(
clientID, l_motor_handle, l_steer,
vrep.simx_opmode_streaming)
err_code = vrep.simxSetJointTargetVelocity(
clientID, r_motor_handle, r_steer,
vrep.simx_opmode_streaming)
err_code, detectionStateMismatch, detectedPointMismatch, detectedObjectHandleMismatch, detectedSurfaceNormalVectorMismatch = vrep.simxReadProximitySensor(
clientID, sensorMismatch, vrep.simx_opmode_streaming)
mismatch_sensordistance = np.linalg.norm(detectedPointMismatch)
current_error_distance = mismatch_sensordistance
print("mismatch distance", mismatch_sensordistance)
l_steer = 0.24
r_steer = 0.24
err_code = vrep.simxSetJointTargetVelocity(
clientID, l_motor_handle, l_steer, vrep.simx_opmode_streaming)
err_code = vrep.simxSetJointTargetVelocity(
clientID, r_motor_handle, r_steer, vrep.simx_opmode_streaming)
last_mismatch = len(spikemon_Mismatch)
##----------------- Read position -----------------##
err_code, Position = vrep.simxGetObjectPosition(
clientID, robot, -1, vrep.simx_opmode_streaming)
x_pos = Position[0]
y_pos = Position[1]
# recalibrate head direction to nearest neuron
x_axis = np.append(x_axis, x_pos)
y_axis = np.append(y_axis, y_pos)
# recalibrate
recal_x_axis = nearest_neuron_x_axis(x_pos, N_x_axis, x_scale)
recal_y_axis = nearest_neuron_y_axis(y_pos, N_y_axis, y_scale)
recal_index = N_x_axis * recal_y_axis + recal_x_axis
r = np.append(
r, recal_index
) # is an array keeping all index that neuron fire during the run
# set reset weight
##----------------- Recording Position Index when collision -----------------##
if l_steer == 0 or r_steer == 0:
collision_index = np.append(collision_index, recal_index)
##----------------- Collect time-----------------##
all_time = np.append(all_time, time.time() - t_int)
# run
PINet.run(15 * ms)
print("current plas landmark synapese index",
np.where(w_plastic_landmark >= 0.5))
print('wall synpases values index bigger',
np.where(w_plastic_wall >= 0.5))
print(
'####-------------------- Read sensor (new round) ----------------------------####'
)
# slow down the controller for more stability
# time.sleep(4.2)
# Start new measurement in the next time step
detectedPoint1, detectedPoint2, detectedPoint3, detectedPoint4, detectedPoint5, detectedPoint6, detectedPoint7, detectedPoint8 = getDetectedpoint(
sensor1, sensor2, sensor3, sensor4, sensor5, sensor6, sensor7,
sensor8, clientID)
err_code, detectionStateMismatch, detectedPointMismatch, detectedObjectHandleMismatch, detectedSurfaceNormalVectorMismatch = vrep.simxReadProximitySensor(
clientID, sensorMismatch, vrep.simx_opmode_streaming)
err_code, resolution, image = vrep.simxGetVisionSensorImage(
clientID, camera, 1, vrep.simx_opmode_streaming)
# record time step and final time
t2 = time.time() - t_int
delta_t = t2 - t1
final_time = time.time()
time_step_list = np.append(time_step_list, delta_t)
print("delta-t", delta_t, iter)
print("final time t2", int(t2))
iter += 1
##---------------end of simulation ---------------##
print("Done")
# saving data
# setting pathway
pathway = "/Users/jieyab/SNN/for_plotting/"
exp_number = str(sim_number)
np.save(pathway + "r" + exp_number, r)
np.save(pathway + "spikemon_CD_i" + exp_number, spikemon_CD.i)
np.save(pathway + "spikemon_CD_t" + exp_number, spikemon_CD.t / ms)
np.save(pathway + "spikemon_HD_i" + exp_number, spikemon_HD.i)
np.save(pathway + "spikemon_HD_t" + exp_number, spikemon_HD.t / ms)
np.save(pathway + "spikemon_PI_i" + exp_number, spikemon_PI.i)
np.save(pathway + "spikemon_PI_t" + exp_number, spikemon_PI.t / ms)
np.save(pathway + "spikemon_noncollision_i" + exp_number,
spikemon_noncollison.i)
np.save(pathway + "spikemon_noncollision_t" + exp_number,
spikemon_noncollison.t / ms)
np.save(pathway + "spikemon_wall_i" + exp_number, spikemon_wall.i)
np.save(pathway + "spikemon_wall_t" + exp_number, spikemon_wall.t / ms)
np.save(pathway + "spikemon_nonlandmark_i" + exp_number,
spikemon_nonlandmark.i)
np.save(pathway + "spikemon_nonlandmark_t" + exp_number,
spikemon_nonlandmark.t / ms)
np.save(pathway + "spikemon_landmark_i" + exp_number, spikemon_landmark.i)
np.save(pathway + "spikemon_landmark_t" + exp_number,
spikemon_landmark.t / ms)
np.save(pathway + "weight_during_run_landmark" + exp_number,
weight_during_run_landmark)
np.save(pathway + "weight_during_run_wall" + exp_number,
weight_during_run_wall)
np.save(pathway + "weight_landmark" + exp_number, w_plastic_landmark)
np.save(pathway + "weight__wall" + exp_number, w_plastic_wall)
np.save(pathway + "spikemon_mismatch_t" + exp_number,
spikemon_Mismatch.t / ms)
np.save(pathway + "spikemon_mismatch_v" + exp_number, spikemon_Mismatch.v)
np.save(pathway + "collision_index" + exp_number, collision_index)
np.save(pathway + "collision_index_during_run" + exp_number,
collision_index_during_run)
np.save(pathway + "compass_angle" + exp_number, compass_angle)
np.save(pathway + "all_time" + exp_number, all_time)
np.save(pathway + "spikemon_estimatedlandmark_i" + exp_number,
spikemon_estimatedlandmark.i)
np.save(pathway + "spikemon_estimatedlandmark_t" + exp_number,
spikemon_estimatedlandmark.t / ms)
np.save(pathway + "spikemon_nonestimatedlandmark_i" + exp_number,
spikemon_nonestimatedlandmark.i)
np.save(pathway + "spikemon_nonestimatedlandmark_t" + exp_number,
spikemon_nonestimatedlandmark.t / ms)
np.save(pathway + "spikemon_red_i" + exp_number, spikemon_red.i)
np.save(pathway + "spikemon_red_t" + exp_number, spikemon_red.t / ms)
np.save(pathway + "spikemon_notred_i" + exp_number, spikemon_notred.i)
np.save(pathway + "spikemon_notred_t" + exp_number, spikemon_notred.t / ms)
np.save(pathway + "step_time" + exp_number, time_step_list)
return clientID
errorCode,usensor10=vrep.simxGetObjectHandle(clientID,'Pioneer_p3dx_ultrasonicSensor10',vrep.simx_opmode_oneshot_wait) errorCode,usensor11=vrep.simxGetObjectHandle(clientID,'Pioneer_p3dx_ultrasonicSensor11',vrep.simx_opmode_oneshot_wait) errorCode,usensor12=vrep.simxGetObjectHandle(clientID,'Pioneer_p3dx_ultrasonicSensor12',vrep.simx_opmode_oneshot_wait) errorCode,usensor13=vrep.simxGetObjectHandle(clientID,'Pioneer_p3dx_ultrasonicSensor13',vrep.simx_opmode_oneshot_wait) errorCode,usensor14=vrep.simxGetObjectHandle(clientID,'Pioneer_p3dx_ultrasonicSensor14',vrep.simx_opmode_oneshot_wait) errorCode,usensor15=vrep.simxGetObjectHandle(clientID,'Pioneer_p3dx_ultrasonicSensor15',vrep.simx_opmode_oneshot_wait) errorCode,usensor16=vrep.simxGetObjectHandle(clientID,'Pioneer_p3dx_ultrasonicSensor16',vrep.simx_opmode_oneshot_wait) #for i in range(1,100): # # vrep.simxSetJointTargetVelocity(clientID,left_motor,2,vrep.simx_opmode_oneshot_wait) # vrep.simxSetJointTargetVelocity(clientID,right_motor,2,vrep.simx_opmode_oneshot_wait) for i in range(1,10000): vrep.simxSetJointTargetVelocity(clientID,left_motor,0,vrep.simx_opmode_oneshot_wait) vrep.simxSetJointTargetVelocity(clientID,right_motor,0,vrep.simx_opmode_oneshot_wait) #Sensor4 true (Lurus) error_code, det_state, det_point, det_handle, det_vec = vrep.simxReadProximitySensor(clientID, usensor4,vrep.simx_opmode_oneshot_wait) print 'Sensor : 4 det state: ', det_state,' det point : ', det_point[2] if det_state == True: if det_point[2]<0.3: #Sensor8 #Sensor4 true error_code, det_state, det_point, det_handle, det_vec = vrep.simxReadProximitySensor(clientID, usensor8,vrep.simx_opmode_oneshot_wait) print 'Sensor : 8 det state: ', det_state,' det point : ', det_point[2] if det_state == True: if det_point[2]<0.3: vrep.simxSetJointTargetVelocity(clientID,left_motor,-2,vrep.simx_opmode_oneshot_wait) vrep.simxSetJointTargetVelocity(clientID,right_motor,2,vrep.simx_opmode_oneshot_wait) time.sleep(0.5); else:
while vrep.simxGetConnectionId(clientID) != -1:
if (sala_atual == 1):
for pos in sala_1:
mover_para(pos[0], pos[1])
elif (sala_atual == 2):
for pos in sala_2:
mover_para(pos[0], pos[1])
elif (sala_atual == 3):
for pos in sala_3:
mover_para(pos[0], pos[1])
elif (sala_atual == 4):
for pos in sala_4:
mover_para(pos[0], pos[1])
vrep.simxSetJointTargetVelocity(clientID, handle_motor_dir, 0,
vrep.simx_opmode_streaming)
vrep.simxSetJointTargetVelocity(clientID, handle_motor_esq, 0,
vrep.simx_opmode_streaming)
print "Fim"
for p in pontos:
pontos_x.append(p[0])
pontos_y.append(p[1])
plotar_mapa()
while (1):
vrep.simxSetJointTargetVelocity(clientID, handle_motor_dir, 0,
vrep.simx_opmode_streaming)
vrep.simxSetJointTargetVelocity(clientID, handle_motor_esq, 0,
vrep.simx_opmode_streaming)
sala_atual += 1
def execute(motorSpeed):
vrep.simxSetJointTargetVelocity(clientID, leftMotorHandle, motorSpeed['speedLeft'], vrep.simx_opmode_oneshot )
vrep.simxSetJointTargetVelocity(clientID, rightMotorHandle, motorSpeed['speedRight'], vrep.simx_opmode_oneshot )
raise Exception('could not get handle 2')
result, j2 = vrep.simxGetObjectHandle(clientID, 'JacoHand_finger3_motor1',
vrep.simx_opmode_blocking)
if result != vrep.simx_return_ok:
raise Exception('could not get handle 3')
result, j3 = vrep.simxGetObjectHandle(clientID, 'JacoHand_finger3_motor2',
vrep.simx_opmode_blocking)
if result != vrep.simx_return_ok:
raise Exception('could not get handle 4')
closingVel = -0.04
for i in range(5):
# Close the gripper
result = vrep.simxSetJointTargetVelocity(clientID, j0, closingVel,
vrep.simx_opmode_blocking)
if result != vrep.simx_return_ok:
raise Exception('could not set joint velocity')
result = vrep.simxSetJointTargetVelocity(clientID, j1, closingVel,
vrep.simx_opmode_blocking)
if result != vrep.simx_return_ok:
raise Exception('could not set joint velocity')
result = vrep.simxSetJointTargetVelocity(clientID, j2, closingVel,
vrep.simx_opmode_blocking)
if result != vrep.simx_return_ok:
raise Exception('could not set joint velocity')
result = vrep.simxSetJointTargetVelocity(clientID, j3, closingVel,
vrep.simx_opmode_blocking)
if result != vrep.simx_return_ok:
raise Exception('could not set joint velocity')
angleModel[2] = abs(angleModel[1])
while clientID!=-1:
joystick = data.find_one({"_id": 0})
x = float(joystick['x'])
y = float(joystick['y'])
lockMode = str(joystick['mode'])
(err, position) = vrep.simxGetObjectPosition(clientID,robot,-1,vrep.simx_opmode_oneshot)
(err, orientation) = vrep.simxGetObjectOrientation(clientID,robot,-1,vrep.simx_opmode_oneshot)
(err, speed, angularSpeed) = vrep.simxGetObjectVelocity(clientID,robot,vrep.simx_opmode_oneshot)
angleModel[0] = orientation[1]
(velizq, velder, targetReached) = setSpeeds(x, y, angleModel, lockMode)
if (joystick != oldJoystick) or targetReached:
oldJoystick = joystick
#print('x: '+str(x)+' y: '+str(y)+' vel: izq '+str(velizq)+' der '+str(velder)+' Lock Mode: '+str(lockMode))
vrep.simxSetJointTargetVelocity(clientID,lwmotor,velizq,vrep.simx_opmode_oneshot)
vrep.simxSetJointTargetVelocity(clientID,rwmotor,velder,vrep.simx_opmode_oneshot)
if (position != oldPosition):
data.update({"item": "robotData"}, {"$set":{"position": {"x": round(position[0],4), "y": round(position[1],4), "z": round(position[2],4)}}})
oldPosition = position
if (orientation!= oldOrientation):
data.update({"item": "robotData"}, {"$set":{"orientation": {"alpha": round(math.degrees(orientation[0]),2), "beta": round(math.degrees(orientation[1]),2), "gamma": round(math.degrees(orientation[2]),2)}}})
oldOrientation = orientation
if (speed!= oldSpeed):
data.update({"item": "robotData"}, {"$set":{"speed": {"vx": round(speed[0],2), "vy": round(speed[1],2), "vz": round(speed[2],2)}}})
oldSpeed = speed
if (angularSpeed!= oldAngularSpeed):
data.update({"item": "robotData"}, {"$set":{"angularSpeed": {"dAlpha": round(angularSpeed[0],2), "dBeta": round(angularSpeed[1],2), "dGamma": round(angularSpeed[2],2)}}})
oldAngularSpeed = angularSpeed
clientID, leftSensor, vrep.simx_opmode_oneshot)[1] == 1)
sensorReading[1] = (vrep.simxReadVisionSensor(
clientID, middleSensor, vrep.simx_opmode_oneshot)[1] == 1)
sensorReading[2] = (vrep.simxReadVisionSensor(
clientID, rightSensor, vrep.simx_opmode_oneshot)[1] == 1)
isRed = False
if vrep.simxReadVisionSensor(clientID, middleSensor,
vrep.simx_opmode_oneshot)[2]:
isRed = (vrep.simxReadVisionSensor(
clientID, middleSensor, vrep.simx_opmode_oneshot)[2][0][6] > 0.9)
if (isRed == True):
print("STOP")
linearVelocityRight = 0
linearVelocityLeft = 0
vrep.simxSetJointTargetVelocity(clientID, leftJointDynamic,
linearVelocityLeft / (wheelRadius),
vrep.simx_opmode_oneshot)
vrep.simxSetJointTargetVelocity(
clientID, rightJointDynamic,
linearVelocityRight / (wheelRadius), vrep.simx_opmode_oneshot)
break
if dist[0] == 0:
while dist[1] < 0.15:
linearVelocityRight = 0
linearVelocityLeft = 0
vrep.simxSetJointTargetVelocity(clientID, leftJointDynamic,
linearVelocityLeft / (wheelRadius),
vrep.simx_opmode_oneshot)
vrep.simxSetJointTargetVelocity(
clientID, rightJointDynamic,
linearVelocityRight / (wheelRadius), vrep.simx_opmode_oneshot)
def moveRightJoint(velocity):
err_right = vrep.simxSetJointTargetVelocity(Variables.clientID, Variables.rightJoint, velocity, vrep.simx_opmode_oneshot)
if err_right != vrep.simx_error_noerror and err_right == vrep.simx_error_novalue_flag:
print "Fehler beim Bewegen des rechten Gelenks des Roboters dr20"
import vrep
import sys
vrep.simxFinish(-1)
clientID = vrep.simxStart('127.0.0.1', 19999, True, True, 5000, 5)
if clientID != -1:
print("Connected to remote 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)
if errorCode == -1:
print('Can not find left or right motor')
sys.exit()
errorCode = vrep.simxSetJointTargetVelocity(clientID, left_motor_handle, 0.2, vrep.simx_opmode_oneshot_wait)
errorCode = vrep.simxSetJointTargetVelocity(clientID, right_motor_handle, 2.2, vrep.simx_opmode_oneshot_wait)
vrep.simxSetJointTargetVelocity(clientID, RMo_handle, -0.1,
vrep.simx_opmode_oneshot_wait)
else:
vrep.simxSetJointTargetVelocity(clientID, RMo_handle, 0,
vrep.simx_opmode_oneshot_wait)
except:
break
MMMB = Bv * 2
MMMR = Rv * 2
vrep.simxSetJointTargetVelocity(clientID, BMo_handle, MMMB,
vrep.simx_opmode_oneshot_wait)
vrep.simxSetJointTargetVelocity(clientID, RMo_handle, MMMR,
vrep.simx_opmode_oneshot_wait)
vrep.simxSetJointTargetVelocity(clientID, BRev_handle, 0,
vrep.simx_opmode_oneshot_wait)
vrep.simxSetJointTargetVelocity(clientID, RRev_handle, 0,
vrep.simx_opmode_oneshot_wait)
vrep.simxSetJointTargetVelocity(clientID, RMo_handle, 0,
vrep.simx_opmode_oneshot_wait)
start()
getballposition()
stop()
#vrep.simxSetJointTargetVelocity(clientID,BRev_handle,B_KickBallVel,vrep.simx_opmode_oneshot_wait)
#vrep.simxSetJointTargetVelocity(clientID,BMo_handle,Move,vrep.simx_opmode_oneshot_wait)
#vrep.simxSetJointTargetVelocity(clientID,RRev_handle,R_KickBallVel,vrep.simx_opmode_oneshot_wait)
#vrep.simxSetJointTargetVelocity(clientID,RMo_handle,Move,vrep.simx_opmode_oneshot_wait)
import numpy as np
import matplotlib as plt
import sys
vrep.simxFinish(-1) #clean up the previous stuff
clientID = vrep.simxStart('127.0.0.1', 19999, True, True, 5000, 5)
if clientID != -1:
print('Connected to remote API server')
else:
print('Connection unsuccessful')
sys.exit('Error: Could not connect to API server')
frontMotorHandles = [-1, -1, -1, -1]
error, frontMotorHandles[0] = vrep.simxGetObjectHandle(
clientID, 'joint_front_left_wheel#0', vrep.simx_opmode_oneshot_wait)
error, frontMotorHandles[1] = vrep.simxGetObjectHandle(
clientID, 'joint_front_right_wheel#0', vrep.simx_opmode_oneshot_wait)
error, frontMotorHandles[2] = vrep.simxGetObjectHandle(
clientID, 'joint_back_right_wheel#0', vrep.simx_opmode_oneshot_wait)
error, frontMotorHandles[3] = vrep.simxGetObjectHandle(
clientID, 'joint_back_left_wheel#0', vrep.simx_opmode_oneshot_wait)
vrep.simxSetJointTargetVelocity(clientID, frontMotorHandles[0], 3,
vrep.simx_opmode_streaming)
vrep.simxSetJointTargetVelocity(clientID, frontMotorHandles[1], -2,
vrep.simx_opmode_streaming)
vrep.simxSetJointTargetVelocity(clientID, frontMotorHandles[2], -2,
vrep.simx_opmode_streaming)
vrep.simxSetJointTargetVelocity(clientID, frontMotorHandles[3], 3,
vrep.simx_opmode_streaming)
