def __init__(self, clientID, name):
self.clientID = clientID
self.name = name
self.noDetectionDist = 0.5
self.maxDetectionDist = 0.2
self.detect = [0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0]
self.braitenbergL = [-0.2,-0.4,-0.6,-0.8,-1,-1.2,-1.4,-1.6, 0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]
self.braitenbergR = [-1.6,-1.4,-1.2,-1,-0.8,-0.6,-0.4,-0.2, 0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]
self.v0 = 2
errorCode, self.left_motor_handle = vrep.simxGetObjectHandle(clientID,
'Pioneer_p3dx_leftMotor'+name,
vrep.simx_opmode_oneshot_wait)
errorCode, self.right_motor_handle = vrep.simxGetObjectHandle(clientID,
'Pioneer_p3dx_rightMotor'+name,
vrep.simx_opmode_oneshot_wait)
self.ultra_sensors = []
for i in range(1,16):
errorCode, sensor_handle = vrep.simxGetObjectHandle(clientID,
'Pioneer_p3dx_ultrasonicSensor'+str(i)+name,
vrep.simx_opmode_oneshot_wait)
if not errorCode:
self.ultra_sensors.append(sensor_handle)
returnCode, detectionState, \
detectedPoint, detectedObjectHandle, \
detectedSurfaceNormalVector = \
vrep.simxReadProximitySensor(clientID,
self.ultra_sensors[i-1],
vrep.simx_opmode_streaming)
else:
print 'Failed to connect to ultrasonic sensor '+str(i)
def __init__(self):
self.ip = '127.0.0.1'
self.port = 19997
self.scene = './simu.ttt'
self.gain = 2
self.initial_position = [3,3,to_rad(45)]
self.r = 0.096 # wheel radius
self.R = 0.267 # demi-distance entre les r
print('New pioneer simulation started')
vrep.simxFinish(-1)
self.client_id = vrep.simxStart(self.ip, self.port, True, True, 5000, 5)
if self.client_id!=-1:
print ('Connected to remote API server on %s:%s' % (self.ip, self.port))
res = vrep.simxLoadScene(self.client_id, self.scene, 1, vrep.simx_opmode_oneshot_wait)
res, self.pioneer = vrep.simxGetObjectHandle(self.client_id, 'Pioneer_p3dx', vrep.simx_opmode_oneshot_wait)
res, self.left_motor = vrep.simxGetObjectHandle(self.client_id, 'Pioneer_p3dx_leftMotor', vrep.simx_opmode_oneshot_wait)
res, self.right_motor = vrep.simxGetObjectHandle(self.client_id, 'Pioneer_p3dx_rightMotor', vrep.simx_opmode_oneshot_wait)
self.set_position(self.initial_position)
vrep.simxStartSimulation(self.client_id, vrep.simx_opmode_oneshot_wait)
else:
print('Unable to connect to %s:%s' % (self.ip, self.port))
def __init__(self, clientID):
self.clientID = clientID
err,leftMotorHandle=vrep.simxGetObjectHandle(clientID,"remoteApiControlledBubbleRobLeftMotor",vrep.simx_opmode_oneshot_wait)
if err==vrep.simx_error_noerror:
self.leftMotorHandle = leftMotorHandle
print('Get handle for left motor: {}'.format(leftMotorHandle))
else:
print('Error by getting handle for left motor: {}'.format(err))
err,rightMotorHandle=vrep.simxGetObjectHandle(clientID,"remoteApiControlledBubbleRobRightMotor",vrep.simx_opmode_oneshot_wait)
if err==vrep.simx_error_noerror:
self.rightMotorHandle = rightMotorHandle
print('Get handle for right motor: {}'.format(rightMotorHandle))
else:
print('Error by getting handle for right motor: {}'.format(err))
err,visionSensorHandle=vrep.simxGetObjectHandle(clientID,"Vision_sensor",vrep.simx_opmode_oneshot_wait)
if err==vrep.simx_error_noerror:
self.visionSensorHandle = visionSensorHandle
print('Get handle for vision sensor: {}'.format(visionSensorHandle))
else:
print('Error by getting handle for vision sensor: {}'.format(err))
err,bubbleRobHandle=vrep.simxGetObjectHandle(clientID,"remoteApiControlledBubbleRob",vrep.simx_opmode_oneshot_wait) #getHandle
if err==vrep.simx_error_noerror:
self.bubbleRobHandle = bubbleRobHandle
print('Get handle for bubbleRob: {}'.format(bubbleRobHandle))
else:
print('Error by getting handle for bubbleRob: {}'.format(err))
err,self.bubbleRobStartPosition = vrep.simxGetObjectPosition(clientID, bubbleRobHandle, -1, vrep.simx_opmode_oneshot_wait) #getStartPosition
def restart(self,earlyStop = False):
if (self.cid != None):
vrep.simxStopSimulation(self.cid, self.mode())
vrep.simxSynchronousTrigger(self.cid)
vrep.simxFinish(-1) # just in case, close all opened connections
time.sleep(1)
self.connect()
time.sleep(1)
vrep.simxLoadScene(self.cid, '/home/elias/[email protected]/_Winter2015/CSC494/Scenes/Base_Quad.ttt', 0, self.mode())
if earlyStop:
vrep.simxStopSimulation(self.cid, self.mode())
vrep.simxSynchronousTrigger(self.cid)
vrep.simxFinish(-1) # just in case, close all opened connections
return
vrep.simxStartSimulation(self.cid, self.mode())
self.runtime = 0
err, self.copter = vrep.simxGetObjectHandle(self.cid, "Quadricopter_base",
vrep.simx_opmode_oneshot_wait)
err, self.target = vrep.simxGetObjectHandle(self.cid, "Quadricopter_target",
vrep.simx_opmode_oneshot_wait)
err, self.front_camera = vrep.simxGetObjectHandle(self.cid, 'Quadricopter_frontCamera', vrep.simx_opmode_oneshot)
err, lin, ang = vrep.simxGetObjectVelocity(self.cid, self.copter, vrep.simx_opmode_streaming)
self.getTargetStart()
for i in range(4):
self.propellerScripts[i] = vrep.simxGetObjectHandle(self.cid,
'Quadricopter_propeller_respondable' + str(i) + str(1),
self.mode())
def __init__(self,clientID):
self.desiredSteeringAngle=0
self.desiredWheelRotSpeed=0
self.steeringAngleDx=2*math.pi/180
self.wheelRotSpeedDx=1
self.d=0.755
self.l=2.5772
self.clientID=clientID
self.absSpeed={-1:-3,0:0,1:3}
self.absAngle={-1:-8*math.pi/180,0:0,1:8*math.pi/180}
errorCodeML,ml = vrep.simxGetObjectHandle(clientID,'nakedCar_motorLeft',vrep.simx_opmode_oneshot_wait)
errorCodeMR,mr = vrep.simxGetObjectHandle(clientID,'nakedCar_motorRight',vrep.simx_opmode_oneshot_wait)
errorCodeSL,sl = vrep.simxGetObjectHandle(clientID,'nakedCar_steeringLeft',vrep.simx_opmode_oneshot_wait)
errorCodeSR,sr = vrep.simxGetObjectHandle(clientID,'nakedCar_steeringRight',vrep.simx_opmode_oneshot_wait)
self.ml = ml
self.mr = mr
self.sl = sl
self.sr = sr
def __init__(self, **kwargs):
self.id = str(kwargs.get("id", ""))
self.generation = kwargs.get("generation", 0)
self.model = kwargs.get("model")
self.DNA = kwargs.get("DNA")
self.opmode = kwargs.get("opmode", vrep.simx_opmode_oneshot_wait)
print "bot number ", self.id, " args : ", self.DNA
self.name = "2W1A"
if self.id != "0":
self.name += "#" + str(self.id)
else:
self.id = ""
print "Bot [" + self.name + "] created"
print "Internal caracteristique : A DEFINIR ASAP!"
self.clientID = kwargs.get("clientID")
vrep.simxCopyPasteObjects(self.clientID, self.model, self.opmode)
ret1, self.wristHandle = vrep.simxGetObjectHandle(self.clientID, "WristMotor" + self.id, kwargs.get("opmode"))
ret2, self.elbowHandle = vrep.simxGetObjectHandle(self.clientID, "ElbowMotor" + self.id, kwargs.get("opmode"))
ret3, self.shoulderHandle = vrep.simxGetObjectHandle(
self.clientID, "ShoulderMotor" + self.id, kwargs.get("opmode")
)
ret4, self.robotHandle = vrep.simxGetObjectHandle(self.clientID, self.id, kwargs.get("opmode"))
print ret1, ret2, ret3
if ret1 != 0 or ret2 != 0 or ret3 != 0:
sys.exit("handlers problem")
def __init__( self, sim_dt=0.01, max_target_distance=3, noise=False,
noise_std=[0,0,0,0,0,0],
target_func=None,
):
super(Quadcopter, self).__init__(sim_dt)
err, self.copter = vrep.simxGetObjectHandle(self.cid, "Quadricopter_base",
vrep.simx_opmode_oneshot_wait )
err, self.target = vrep.simxGetObjectHandle(self.cid, "Quadricopter_target",
vrep.simx_opmode_oneshot_wait )
# Reset the motor commands to zero
packedData=vrep.simxPackFloats([0,0,0,0])
raw_bytes = (ctypes.c_ubyte * len(packedData)).from_buffer_copy(packedData)
err = vrep.simxSetStringSignal(self.cid, "rotorTargetVelocities",
raw_bytes,
vrep_mode)
self.pos = [0,0,0]
self.pos_err = [0,0,0]
self.t_pos = [0,0,0]
self.lin = [0,0,0]
self.ori = [0,0,0]
self.ori_err = [0,0,0]
self.t_ori = [0,0,0]
self.ang = [0,0,0]
self.vert_prox_dist = 0
self.left_prox_dist = 0
self.right_prox_dist = 0
# Distance reading recorded when nothing is in range
self.max_vert_dist = 1.5
self.max_left_dist = 1.0
self.max_right_dist = 1.0
# Maximum target distance error that can be returned
self.max_target_distance = max_target_distance
# If noise is being modelled
self.noise = noise
# Standard Deviation of the noise for the 4 state variables
self.noise_std = noise_std
# Overwrite the get_target method if the target is to be controlled by a
# function instead of by V-REP
if target_func is not None:
self.step = 0
self.target_func = target_func
def get_target():
self.t_pos, self.t_ori = self.target_func( self.step )
self.step += 1
self.get_target = get_target
def getSensorHandles_dr20_(clientID):
err_h_infra6, infra6 = vrep.simxGetObjectHandle(clientID,'dr20_infraredSensor6_', vrep.simx_opmode_oneshot_wait)
err_h_infra2, infra2 = vrep.simxGetObjectHandle(clientID,'dr20_infraredSensor2_', vrep.simx_opmode_oneshot_wait)
err_h_infra5, infra5 = vrep.simxGetObjectHandle(clientID,'dr20_infraredSensor5_', vrep.simx_opmode_oneshot_wait)
err_h_infra1, infra1 = vrep.simxGetObjectHandle(clientID,'dr20_infraredSensor1_', vrep.simx_opmode_oneshot_wait)
err_h_sonic, sonic = vrep.simxGetObjectHandle(clientID,'dr20_ultrasonicSensor_', vrep.simx_opmode_oneshot_wait)
return (err_h_infra6, err_h_infra2, err_h_infra5, err_h_infra1, err_h_sonic,
infra6, infra2, infra5, infra1, sonic)
def __init__(self, simulatorModel=None):
self.robotOrientationFirstTime = True
self.getDistanceToGoalFirstTime = True
self.getUpDistanceFirstTime = True
self.getObjectPositionFirstTime = True
self.sysConf = LoadSystemConfiguration()
#this data structure is like a cache for the joint handles
self.jointHandleMapping = {}
robotConf = LoadRobotConfiguration()
self.model = simulatorModel
self.LucyJoints = robotConf.getJointsName()
for joint in self.LucyJoints:
self.jointHandleMapping[joint]=0
self.clientId = self.connectVREP()
RETRY_ATTEMPTS = 50
if simulatorModel:
for i in range(RETRY_ATTEMPTS):
#TODO try to reutilize the same scene for the sake of performance
error = self.loadscn(self.clientId, simulatorModel)
if not error:
break
print "retrying connection to vrep"
if error:
raise VrepException("error loading Vrep robot model", -1)
if int(self.sysConf.getProperty("synchronous mode?"))==1:
self.synchronous = True
vrep.simxSynchronousTrigger(self.clientId)
else:
self.synchronous = False
self.speedmodifier = int(self.sysConf.getProperty("speedmodifier"))
#setting the simulation time step
self.simulationTimeStepDT = float(self.sysConf.getProperty("simulation time step"))
'''#testing printing in vrep
error, consoleHandler = vrep.simxAuxiliaryConsoleOpen(self.clientId, "lucy debugging", 8, 22, None, None, None, None, vrep.simx_opmode_oneshot_wait)
print "console handler: ", consoleHandler
vrep.simxAuxiliaryConsoleShow(self.clientId, consoleHandler, 1, vrep.simx_opmode_oneshot_wait)
error = vrep.simxAuxiliaryConsolePrint(self.clientId, consoleHandler, "Hello World", vrep.simx_opmode_oneshot_wait)'''
error, self.upDistanceHandle = vrep.simxGetDistanceHandle(self.clientId, "upDistance#", vrep.simx_opmode_blocking)
error, self.distLFootToGoalHandle = vrep.simxGetDistanceHandle(self.clientId, "distanceLFootGoal#", vrep.simx_opmode_blocking)
error, self.distRFootToGoalHandle = vrep.simxGetDistanceHandle(self.clientId, "distanceRFootGoal#", vrep.simx_opmode_blocking)
error, self.bioloidHandle = vrep.simxGetObjectHandle(self.clientId,"Bioloid", vrep.simx_opmode_oneshot_wait)
error, self.cuboidHandle = vrep.simxGetObjectHandle(self.clientId,"Cuboid", vrep.simx_opmode_oneshot_wait)
error, self.armHandler = vrep.simxGetObjectHandle(self.clientId, "Pivot", vrep.simx_opmode_oneshot_wait)
self.populateJointHandleCache(self.clientId)
self.isRobotUpFirstCall = True
self.getDistanceToSceneGoal() #to fix the first invocation
self.getUpDistance()
self.isRobotUp(self.clientId)
self.armPositionXAxis = -9.0000e-02
def config_handles(self):
# Object-handle-configuration
errorFlag, self.Quadbase = vrep.simxGetObjectHandle(self.clientID,
'Quadricopter_base',
vrep.simx_opmode_oneshot_wait)
errorFlag, self.Quadobj = vrep.simxGetObjectHandle(self.clientID,
'Quadricopter',
vrep.simx_opmode_oneshot_wait)
self.getState(initial=True)
time.sleep(0.05)
def __init__( self, *args, **kwargs ):
super(SensorQuadcopter, self).__init__(*args, **kwargs)
err, self.vert_prox = vrep.simxGetObjectHandle(self.cid, "vert_prox",
vrep.simx_opmode_oneshot_wait )
err, self.left_prox = vrep.simxGetObjectHandle(self.cid, "left_prox",
vrep.simx_opmode_oneshot_wait )
err, self.right_prox = vrep.simxGetObjectHandle(self.cid, "right_prox",
vrep.simx_opmode_oneshot_wait )
def __init__(self, sim_dt=0.05):
super(Arm, self).__init__(sim_dt)
err, self.hand = vrep.simxGetObjectHandle(self.cid, "hand_end",
vrep.simx_opmode_oneshot_wait )
err, self.target = vrep.simxGetObjectHandle(self.cid, "target",
vrep.simx_opmode_oneshot_wait )
err, self.hand_joint = vrep.simxGetObjectHandle(self.cid, "hand_joint",
vrep.simx_opmode_oneshot_wait )
err, self.arm_joint = vrep.simxGetObjectHandle(self.cid, "arm_joint",
vrep.simx_opmode_oneshot_wait )
def __init__(self, address, port):
print ('Program started')
vrep.simxFinish(-1)
self.clientID=vrep.simxStart(address,port,True,True,5000,5)
if self.clientID!=-1:
print "Conexion establecida, todo en orden"
else:
print "Conexion con el simulador fallida, reinicia el simulador"
error,self.motorFL=vrep.simxGetObjectHandle(self.clientID,'Pioneer_p3dx_rightMotor',vrep.simx_opmode_oneshot_wait)
error,self.motorFR=vrep.simxGetObjectHandle(self.clientID,'Pioneer_p3dx_leftMotor',vrep.simx_opmode_oneshot_wait)
error,self.camera=vrep.simxGetObjectHandle(self.clientID,'Vision_sensor',vrep.simx_opmode_oneshot_wait)
error,self.robot=vrep.simxGetObjectHandle(self.clientID,'Pioneer_p3dx',vrep.simx_opmode_oneshot_wait)
def start(self):
print ('Program started')
vrep.simxFinish(-1) # just in case, close all opened connections
self.clientID = vrep.simxStart('127.0.0.1',19999,True,True,5000,5) # Connect to V-REP
if self.clientID != -1:
print ('Connected to remote API server')
self.LWMotor_hdl = vrep.simxGetObjectHandle(self.clientID,'LeftWheel_Motor', vrep.simx_opmode_oneshot_wait) # LeftWheel Motor handle
self.RWMotor_hdl = vrep .simxGetObjectHandle(self.clientID,'RightWheel_Motor', vrep.simx_opmode_oneshot_wait) # RightWheel Motor handle
self.Robot_hdl = vrep.simxGetObjectHandle(self.clientID, 'Cubot', vrep.simx_opmode_oneshot_wait)
print ('Handle acquired !!')
else:
print ('Error : connection to vrep failed')
def getDistanceBetwObjects(self, objectNameA, objectNameB):
"""Get the distance between two named objects in V-REP.
Raise exception if either does not exist"""
eCode, handleA = vrep.simxGetObjectHandle(self._VREP_clientId, objectNameA, vrep.simx_opmode_oneshot_wait)
if eCode != 0:
raise Exception("Could not get handle of object", objectNameA)
eCode, poseA = vrep.simxGetObjectPosition(self._VREP_clientId, handleA, -1, vrep.simx_opmode_oneshot_wait)
eCode, handleB = vrep.simxGetObjectHandle(self._VREP_clientId, objectNameB, vrep.simx_opmode_oneshot_wait)
if eCode != 0:
raise Exception("Could not get handle of object", objectNameB)
eCode, poseB = vrep.simxGetObjectPosition(self._VREP_clientId, handleB, -1, vrep.simx_opmode_oneshot_wait)
return distance(poseA,poseB)
def fetchKinect(depthSTR,rgbSTR):
errorCodeKinectRGB,kinectRGB=vrep.simxGetObjectHandle(clientID,rgbSTR,vrep.simx_opmode_oneshot_wait)
errorCodeKinectDepth,kinectDepth=vrep.simxGetObjectHandle(clientID,depthSTR,vrep.simx_opmode_oneshot_wait)
errorHere,resolution,image=vrep.simxGetVisionSensorImage(clientID,kinectRGB,0,vrep.simx_opmode_oneshot_wait)
img,imgArr=Raftaar.ProcessImage(image,resolution)
rgbArr=np.array(imgArr)
errorHere,resol,depth=vrep.simxGetVisionSensorDepthBuffer(clientID,kinectDepth,vrep.simx_opmode_oneshot_wait)
depthArr=np.array(depth)
return depthArr,rgbArr
def setJointPositionNonBlock(self, clientID, joint, angle):
handle = self.jointHandleMapping[joint]
error = False
if (handle == 0):
error, handle=vrep.simxGetObjectHandle(clientID,joint,vrep.simx_opmode_oneshot)
self.jointHandleMapping[joint]=handle
vrep.simxSetJointPosition(clientID,handle,angle,vrep.simx_opmode_streaming)
def __init__( self, sim_dt=0.01, max_target_distance=3, noise=False,
noise_std=[0,0,0,0,0,0],
):
# Call the superclass of Quadcopter, which is Robot
super(Quadcopter, self).__init__(sim_dt)
err, self.copter = vrep.simxGetObjectHandle(self.cid, "Quadricopter_base",
vrep.simx_opmode_oneshot_wait )
# Reset the motor commands to zero
packedData=vrep.simxPackFloats([0,0,0,0])
raw_bytes = (ctypes.c_ubyte * len(packedData)).from_buffer_copy(packedData)
err = vrep.simxSetStringSignal(self.cid, "rotorTargetVelocities",
raw_bytes,
vrep_mode)
self.pos = [0,0,0]
self.pos_err = [0,0,0]
self.t_pos = [0,0,0]
self.lin = [0,0,0]
self.ori = [0,0,0]
self.ori_err = [0,0,0]
self.t_ori = [0,0,0]
self.ang = [0,0,0]
# Maximum target distance error that can be returned
self.max_target_distance = max_target_distance
# If noise is being modelled
self.noise = noise
# Standard Deviation of the noise for the 4 state variables
self.noise_std = noise_std
def prox_sens_initialize(clientID):
""" Initialize proximity sensor. Maybe helpful later """
proxSens=[]
for i in range(8):
_, oneProxSens = vrep.simxGetObjectHandle(clientID, 'ePuck_proxSensor%d' % (i+1), vrep.simx_opmode_streaming)
proxSens.append(oneProxSens)
return proxSens
def get_object_handle(object_name,all_switch): #if all_switch == 1: # res,objs = vrep.simxGetObjects(clientID,vrep.sim_handle_all,vrep.simx_opmode_oneshow_wait) #else: res,objs = vrep.simxGetObjectHandle(clientID,object_name,vrep.simx_opmode_oneshot_wait) return objs
def getPosition(clientID,goal_new):
errorcode,newgoal_handle=vrep.simxGetObjectHandle(clientID,goal_new,vrep.simx_opmode_oneshot_wait)
#time.sleep(1)
errorCode,newgoal_position=vrep.simxGetObjectPosition(clientID,newgoal_handle,-1,vrep.simx_opmode_streaming)
time.sleep(1)
errorCode,newgoal_position=vrep.simxGetObjectPosition(clientID,newgoal_handle,-1,vrep.simx_opmode_buffer)
return newgoal_position
def getVisionSensor(visionSensorName,clientID,sample_time):
#Get the handle of the vision sensor
res1,visionSensorHandle=vrep.simxGetObjectHandle(clientID,visionSensorName,vrep.simx_opmode_oneshot_wait)
#Get the image
res2,resolution,image=vrep.simxGetVisionSensorImage(clientID,visionSensorHandle,0,vrep.simx_opmode_streaming)
time.sleep(0.5)
res2,resolution,image=vrep.simxGetVisionSensorImage(clientID,visionSensorHandle,0,vrep.simx_opmode_buffer)
print resolution
img = I.new("RGB", (resolution[0], resolution[1]))
while (vrep.simxGetConnectionId(clientID)!=-1):
start = time.time()
#Get the image of the vision sensor
res,resolution,image = vrep.simxGetVisionSensorImage(clientID,visionSensorHandle,0,vrep.simx_opmode_buffer)
image_byte_array = array.array('b',image)
im = I.frombuffer("RGB", (resolution[0],resolution[1]), image_byte_array, "raw", "RGB", 0, 1)
# #Set the image for naoqi
# imagepixel = []#*(resolution[0]*resolution[1])
# for i in xrange(resolution[0] * resolution[1]):
# r = i*3
# g = r+1
# b = r+2
# imagepixel.append((image[b], image[g], image[r]))
# img.putdata(imagepixel)
# print image
# print imagepixel
videoDeviceProxy.putImage(vision_definitions.kTopCamera, resolution[0], resolution[1], im.rotate(180).tostring())
end = time.time()
dt = end-start
# if dt < sample_time:
# time.sleep(sample_time - dt)
# else:
# print "Can't keep a period (%gs>%gs)" % (dt,sample_time)
print 'End of Simulation'
def setJointPosition(clientID, joint, angle):
if (jointHandleMapping[joint] > 0):
jhandle=jointHandleMapping[joint]
else:
error, jhandle=vrep.simxGetObjectHandle(clientID,joint,vrep.simx_opmode_oneshot_wait)
jointHandleMapping[joint]=jhandle
vrep.simxSetJointPosition(clientID,jhandle,angle,vrep.simx_opmode_oneshot_wait)
def configure_handles(self):
# Handles of body and joints
rc, body = vrep.simxGetObjectHandle(self.client_id, 'body',
vrep.simx_opmode_oneshot_wait)
assert rc == 0, rc
rc, joint_0 = vrep.simxGetObjectHandle(self.client_id, 'joint_0',
vrep.simx_opmode_oneshot_wait)
assert rc == 0, rc
rc, joint_1 = vrep.simxGetObjectHandle(self.client_id, 'joint_1',
vrep.simx_opmode_oneshot_wait)
assert rc == 0, rc
self.body = body
self.joints = [joint_0, joint_1]
self.get_body_position(initial=True)
self.get_joint_angles(initial=True)
time.sleep(0.5)
def readDummyPath(self):
fp = open(self.dummyPath, 'r')
lines = fp.readlines() # 1行毎にファイル終端まで全て読む(改行文字も含まれる)
fp.close()
for line in lines:
buf = line.split(",")
for x in buf:
params = x.split(":")
if len(params) >= 2:
name = params[0]
try:
portNb = 19998 # int(params[1])
self.dummyID = vrep.simxStart("127.0.0.1", portNb, True, True, 2000, 5)
if self.dummyID == -1:
print >> sys.stderr, "Fatal: No client ID while creating Dummy Communicator."
else:
self.setClientID(self.dummyID)
except ValueError:
print >> sys.stderr, "Fatal: non integer value while creating Dummy Communicator."
time.sleep(1)
exit()
else:
name = params[0]
returnCode, handle = vrep.simxGetObjectHandle(self.dummyID, name, vrep.simx_opmode_oneshot_wait)
if returnCode != vrep.simx_return_ok:
print >> sys.stderr, "Fatal: Error obtaining a handle for " + name + "!"
else:
print name, handle
item = VRepItem(name, self.dummyID, handle)
self.addItem(item)
def streamVisionSensor(visionSensorName,clientID,pause=0.0001):
#Get the handle of the vision sensor
res1,visionSensorHandle=vrep.simxGetObjectHandle(clientID,visionSensorName,vrep.simx_opmode_oneshot_wait)
#Get the image
res2,resolution,image=vrep.simxGetVisionSensorImage(clientID,visionSensorHandle,0,vrep.simx_opmode_streaming)
#Allow the display to be refreshed
plt.ion()
#Initialiazation of the figure
time.sleep(0.5)
res,resolution,image=vrep.simxGetVisionSensorImage(clientID,visionSensorHandle,0,vrep.simx_opmode_buffer)
im = I.new("RGB", (resolution[0], resolution[1]), "white")
#Give a title to the figure
fig = plt.figure(1)
fig.canvas.set_window_title(visionSensorName)
#inverse the picture
plotimg = plt.imshow(im,origin='lower')
#Let some time to Vrep in order to let him send the first image, otherwise the loop will start with an empty image and will crash
time.sleep(1)
while (vrep.simxGetConnectionId(clientID)!=-1):
#Get the image of the vision sensor
res,resolution,image=vrep.simxGetVisionSensorImage(clientID,visionSensorHandle,0,vrep.simx_opmode_buffer)
#Transform the image so it can be displayed using pyplot
image_byte_array = array.array('b',image)
im = I.frombuffer("RGB", (resolution[0],resolution[1]), image_byte_array, "raw", "RGB", 0, 1)
#Update the image
plotimg.set_data(im)
#Refresh the display
plt.draw()
#The mandatory pause ! (or it'll not work)
plt.pause(pause)
print 'End of Simulation'
def add_actuator(self, name, func, dim=1):
if name is None:
handle = None
else:
err, handle = vrep.simxGetObjectHandle(self.cid, name,
vrep.simx_opmode_oneshot_wait )
self.actuators.append([handle, func, dim])
self.size_in += dim
def __init__(self, ip, port):
"""Initializes connectivity to V-REP environment, initializes environmental variables.
"""
vrep.simxFinish(-1)
self.clientID = vrep.simxStart(ip, port, True, True, 5000, 5)
if self.clientID != -1:
print('Successfully connected to V-REP')
else:
raise RuntimeError('Could not connect to V-REP')
# Some artificial delays to let V-REP stabilize after an action
self.sleep_sec = config.SLEEP_VAL
self.sleep_sec_min = config.SLEEP_VAL_MIN
# id of the Robot Arm in V-REP environment
self.main_object = 'uarm'
# Initial state of the Gripper
self.gripper_enabled = False
# Get hands to various objects in the scene
err, self.cylinder_handle = vrep.simxGetObjectHandle(self.clientID, 'uarm_pickupCylinder2',
vrep.simx_opmode_blocking)
if err != vrep.simx_return_ok:
raise RuntimeError("Could not get handle to the cylinder object. Halting program.")
err, self.bin_handle = vrep.simxGetObjectHandle(self.clientID, 'uarm_bin',
vrep.simx_opmode_blocking)
if err != vrep.simx_return_ok:
raise RuntimeError("Could not get handle to the Bin object. Halting program.")
err, self.gripper_handle = vrep.simxGetObjectHandle(self.clientID, 'uarmGripper_motor1Method2',
vrep.simx_opmode_blocking)
if err != vrep.simx_return_ok:
raise RuntimeError("Could not get handle to the Gripper object. Halting program.")
# Distance between cylinder and bin when former is inside later
# This was measured after putting the cylinder in the bin
self.cylinder_bin_distance = config.CYLINDER_BIN_DISTANCE
# Various values, to be set in the start_sim() function, because their values can be obtained
# only after the simulation starts
self.cylinder_height = None
self.cylinder_z_locus = None
self.bin_position = None
self.objects = None
self.object_positions = None
def isRobotUp(clientID):
error, LSP_Handle=vrep.simxGetObjectHandle(clientID,"Bioloid", vrep.simx_opmode_oneshot_wait)
error, bioloid_position = vrep.simxGetObjectPosition(clientID, LSP_Handle, -1, vrep.simx_opmode_streaming)
#in case of problems consulting the robot position, consider that the robot is up
if error:
return error, True
else:
return error, bioloid_position[2]>FALL_THRESHOLD
def getHandles(self):
"Get handles for KJunior, motors, and sensors"
ecodeE, self.KJuniorHandle = vrep.simxGetObjectHandle(self.simulID, "KJunior", vrep.simx_opmode_oneshot_wait)
vrep.simxSynchronousTrigger(self.simulID)
eCodeR, self.KJrightMotor = vrep.simxGetObjectHandle(self.simulID, "KJunior_motorRight#0", vrep.simx_opmode_oneshot_wait)
vrep.simxSynchronousTrigger(self.simulID)
eCodeL, self.KJleftMotor = vrep.simxGetObjectHandle(self.simulID, "KJunior_motorLeft#0", vrep.simx_opmode_oneshot_wait)
vrep.simxSynchronousTrigger(self.simulID)
eCodeR, self.KJrightEye = vrep.simxGetObjectHandle(self.simulID, "KJunior_proxSensor4#0", vrep.simx_opmode_oneshot_wait)
vrep.simxSynchronousTrigger(self.simulID)
eCodeL, self.KJleftEye = vrep.simxGetObjectHandle(self.simulID, "KJunior_proxSensor2#0", vrep.simx_opmode_oneshot_wait)
vrep.simxSynchronousTrigger(self.simulID)
eCodeL, self.KJcenterEye = vrep.simxGetObjectHandle(self.simulID, "KJunior_proxSensor3#0", vrep.simx_opmode_oneshot_wait)
vrep.simxSynchronousTrigger(self.simulID)
eCode,self.targetID = vrep.simxGetObjectHandle(self.simulID,"Sphere", vrep.simx_opmode_oneshot_wait)
vrep.simxSynchronousTrigger(self.simulID)
if (self.KJrightMotor == 0 or self.KJleftMotor == 0 or self.KJrightEye == 0 or self.KJleftEye == 0):
self.cleanUp()
sys.exit("Exiting: Could not connect to motors or sensors")
else:
print "Connected to Right Motor: %d, leftMotor: %d, Right eye: %d, Left eye: %d, with target ID:%d" % (self.KJrightMotor,
self.KJleftMotor,
self.KJrightEye,
self.KJleftEye,
self.targetID)
def run_benchmark_training(sim_number, sim_time):
print("start running benchmark 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(get sensor point and distance)
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)
# print("IMU head direction is", heading_dir, "angle from IMU is", 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)
##----------------Active searching for landmark when encountering mismatch -----------------##
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
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)
# print("position value is",Position)
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
##----------------- reset position neuron with IMU recalibration ---------------------##
'''coment this line to disable IMU'''
#PI_Reset_synapse.w_poi_PI = np.array(gaussian_spike(N_PI, recal_index, 20, 0.01))
##----------------- 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('count mismatch', len(spikemon_Mismatch))
print('count landmark', len(spikemon_landmark))
print('####-------------------- Read sensor (new round) ----------------------------####')
# 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
def main():
# Start V-REP connection
try:
vrep.simxFinish(-1)
print("Connecting to simulator...")
clientID = vrep.simxStart('127.0.0.1', 19999, True, True, 5000, 5)
if clientID == -1:
print("Failed to connect to remote API Server")
vrep_exit(clientID)
except KeyboardInterrupt:
vrep_exit(clientID)
return
# Setup V-REP simulation
print("Setting simulator to async mode...")
vrep.simxSynchronous(clientID, True)
dt = .001
vrep.simxSetFloatingParameter(clientID,
vrep.sim_floatparam_simulation_time_step,
dt, # specify a simulation time step
vrep.simx_opmode_oneshot)
# Load V-REP scene
print("Loading scene...")
vrep.simxLoadScene(clientID, scene_name, 0xFF, vrep.simx_opmode_blocking)
# Get quadrotor handle
err, quad_handle = vrep.simxGetObjectHandle(clientID, quad_name, vrep.simx_opmode_blocking)
print(err,quad_handle)
# Initialize quadrotor position and orientation
vrep.simxGetObjectPosition(clientID, quad_handle, -1, vrep.simx_opmode_streaming)
vrep.simxGetObjectOrientation(clientID, quad_handle, -1, vrep.simx_opmode_streaming)
vrep.simxGetObjectVelocity(clientID, quad_handle, vrep.simx_opmode_streaming)
# Start simulation
vrep.simxStartSimulation(clientID, vrep.simx_opmode_blocking)
# Initialize rotors
print("Initializing propellers...")
for i in range(len(propellers)):
vrep.simxClearFloatSignal(clientID, propellers[i], vrep.simx_opmode_oneshot)
# Get quadrotor initial position and orientation
err, pos_start = vrep.simxGetObjectPosition(clientID, quad_handle, -1, vrep.simx_opmode_buffer)
err, euler_start = vrep.simxGetObjectOrientation(clientID, quad_handle, -1, vrep.simx_opmode_buffer)
err, vel_start, angvel_start = vrep.simxGetObjectVelocity(clientID,
quad_handle, vrep.simx_opmode_buffer)
pos_start = np.asarray(pos_start)
euler_start = np.asarray(euler_start)*10.
vel_start = np.asarray(vel_start)
angvel_start = np.asarray(angvel_start)
pos_old = pos_start
euler_old = euler_start
vel_old = vel_start
angvel_old = angvel_start
pos_new = pos_old
euler_new = euler_old
vel_new = vel_old
angvel_new = angvel_old
pos_error = (pos_start + setpoint_delta[0:3]) - pos_new
euler_error = (euler_start + setpoint_delta[3:6]) - euler_new
vel_error = (vel_start + setpoint_delta[6:9]) - vel_new
angvel_error = (angvel_start + setpoint_delta[9:12]) - angvel_new
pos_error_all = pos_error
euler_error_all = euler_error
n_input = 6
n_forces=4
init_f=7.
state = [0 for i in range(n_input)]
state = torch.from_numpy(np.asarray(state, dtype=np.float32)).view(-1, n_input)
propeller_vels = [init_f, init_f, init_f, init_f]
delta_forces = [0., 0., 0., 0.]
extended_state=torch.cat((state,torch.from_numpy(np.asarray([propeller_vels], dtype=np.float32))),1)
memory = ReplayMemory(ROLLOUT_LEN)
test_num = 1
timestep = 1
while (vrep.simxGetConnectionId(clientID) != -1):
# Set propeller thrusts
print("Setting propeller thrusts...")
# Only PD control bc can't find api function for getting simulation time
propeller_vels = pid(pos_error,euler_new,euler_error[2],vel_error,angvel_error)
#propeller_vels = apply_forces(propeller_vels, delta_forces) # for dqn
# Send propeller thrusts
print("Sending propeller thrusts...")
[vrep.simxSetFloatSignal(clientID, prop, vels, vrep.simx_opmode_oneshot) for prop, vels in
zip(propellers, propeller_vels)]
# Trigger simulator step
print("Stepping simulator...")
vrep.simxSynchronousTrigger(clientID)
# Get quadrotor initial position and orientation
err, pos_new = vrep.simxGetObjectPosition(clientID, quad_handle, -1, vrep.simx_opmode_buffer)
err, euler_new = vrep.simxGetObjectOrientation(clientID, quad_handle, -1, vrep.simx_opmode_buffer)
err, vel_new, angvel_new = vrep.simxGetObjectVelocity(clientID,
quad_handle, vrep.simx_opmode_buffer)
pos_new = np.asarray(pos_new)
euler_new = np.asarray(euler_new)*10
vel_new = np.asarray(vel_new)
angvel_new = np.asarray(angvel_new)
#euler_new[2]/=100
valid = is_valid_state(pos_start, pos_new, euler_new)
if valid:
next_state = torch.FloatTensor(np.concatenate([euler_new, pos_new - pos_old]))
#next_state = torch.FloatTensor(euler_new )
next_extended_state=torch.FloatTensor([np.concatenate([next_state,np.asarray(propeller_vels)])])
else:
next_state = None
next_extended_state = None
reward=np.float32(0)
memory.push(extended_state, torch.from_numpy(np.asarray([delta_forces],dtype=np.float32)), next_extended_state,
torch.from_numpy(np.asarray([[reward]])))
state = next_state
extended_state=next_extended_state
pos_old = pos_new
euler_old = euler_new
vel_old = vel_new
angvel_old = angvel_new
print("Propeller Velocities:")
print(propeller_vels)
print("\n")
pos_error = (pos_start + setpoint_delta[0:3]) - pos_new
euler_error = (euler_start + setpoint_delta[3:6]) - euler_new
euler_error %= 2*np.pi
for i in range(len(euler_error)):
if euler_error[i] > np.pi:
euler_error[i] -= 2*np.pi
vel_error = (vel_start + setpoint_delta[6:9]) - vel_new
angvel_error = (angvel_start + setpoint_delta[9:12]) - angvel_new
pos_error_all = np.vstack([pos_error_all,pos_error])
euler_error_all = np.vstack([euler_error_all,euler_error])
print("Errors (pos,ang):")
print(pos_error,euler_error)
print("\n")
timestep += 1
if not valid or timestep > ROLLOUT_LEN:
np.savetxt('csv/pos_error{0}.csv'.format(test_num),
pos_error_all,
delimiter=',',
fmt='%8.4f')
np.savetxt('csv/euler_error{0}.csv'.format(test_num),
euler_error_all,
delimiter=',',
fmt='%8.4f')
print('RESET')
# reset
vrep.simxStopSimulation(clientID, vrep.simx_opmode_oneshot_wait)
time.sleep(0.1)
# Setup V-REP simulation
print("Setting simulator to async mode...")
vrep.simxSynchronous(clientID, True)
dt = .001
vrep.simxSetFloatingParameter(clientID,
vrep.sim_floatparam_simulation_time_step,
dt, # specify a simulation time step
vrep.simx_opmode_oneshot)
print("Loading scene...")
vrep.simxLoadScene(clientID, scene_name, 0xFF, vrep.simx_opmode_blocking)
# Get quadrotor handle
err, quad_handle = vrep.simxGetObjectHandle(clientID, quad_name, vrep.simx_opmode_blocking)
# Initialize quadrotor position and orientation
vrep.simxGetObjectPosition(clientID, quad_handle, -1, vrep.simx_opmode_streaming)
vrep.simxGetObjectOrientation(clientID, quad_handle, -1, vrep.simx_opmode_streaming)
vrep.simxGetObjectVelocity(clientID, quad_handle, vrep.simx_opmode_streaming)
# Start simulation
vrep.simxStartSimulation(clientID, vrep.simx_opmode_blocking)
for i in range(len(propellers)):
vrep.simxClearFloatSignal(clientID, propellers[i], vrep.simx_opmode_oneshot)
err, pos_start = vrep.simxGetObjectPosition(clientID, quad_handle, -1, vrep.simx_opmode_buffer)
err, euler_start = vrep.simxGetObjectOrientation(clientID, quad_handle, -1, vrep.simx_opmode_buffer)
err, vel_start, angvel_start = vrep.simxGetObjectVelocity(clientID,
quad_handle, vrep.simx_opmode_buffer)
pos_start = np.asarray(pos_start)
euler_start = np.asarray(euler_start)*10.
vel_start = np.asarray(vel_start)
angvel_start = np.asarray(angvel_start)*10.
pos_old = pos_start
euler_old = euler_start
vel_old = vel_start
angvel_old = angvel_start
pos_new = pos_old
euler_new = euler_old
vel_new = vel_old
angvel_new = angvel_old
pos_error = (pos_start + setpoint_delta[0:3]) - pos_new
euler_error = (euler_start + setpoint_delta[3:6]) - euler_new
vel_error = (vel_start + setpoint_delta[6:9]) - vel_new
angvel_error = (angvel_start + setpoint_delta[9:12]) - angvel_new
pos_error_all = np.vstack([pos_error_all,pos_error])
euler_error_all = np.vstack([euler_error_all,euler_error])
state = [0 for i in range(n_input)]
state = torch.FloatTensor(np.asarray(state)).view(-1, n_input)
propeller_vels = [init_f, init_f, init_f, init_f]
extended_state = torch.cat((state, torch.FloatTensor(np.asarray([propeller_vels]))), 1)
print('duration: ',len(memory))
memory = ReplayMemory(ROLLOUT_LEN)
test_num += 1
timestep = 1
def generate(save_path,
number_episodes,
steps_per_episode,
jointvel_factor=0.5):
# Initialize connection
connection = VrepConnection()
connection.synchronous_mode()
connection.start()
# Use client id from connection
clientID = connection.clientID
# Get joint handles
jhList = [-1, -1, -1, -1, -1, -1]
for i in range(6):
err, jh = vrep.simxGetObjectHandle(clientID, "Mico_joint" + str(i + 1),
vrep.simx_opmode_blocking)
jhList[i] = jh
# Initialize joint position
jointpos = np.zeros(6)
for i in range(6):
err, jp = vrep.simxGetJointPosition(clientID, jhList[i],
vrep.simx_opmode_streaming)
jointpos[i] = jp
# Initialize vision sensor
res, v1 = vrep.simxGetObjectHandle(clientID, "vs1",
vrep.simx_opmode_oneshot_wait)
err, resolution, image = vrep.simxGetVisionSensorImage(
clientID, v1, 0, vrep.simx_opmode_streaming)
vrep.simxGetPingTime(clientID)
err, resolution, image = vrep.simxGetVisionSensorImage(
clientID, v1, 0, vrep.simx_opmode_buffer)
# Initialize distance handle
err, distance_handle = vrep.simxGetDistanceHandle(
clientID, "tipToTarget", vrep.simx_opmode_blocking)
err, distance_to_target = vrep.simxReadDistance(clientID, distance_handle,
vrep.simx_opmode_streaming)
# Initialize data file
f = h5py.File(save_path, "w")
episode_count = 0
total_datapoints = number_episodes * steps_per_episode
size_of_image = resolution[0] * resolution[1] * 3
dset1 = f.create_dataset("images", (total_datapoints, size_of_image),
dtype="uint")
dset2 = f.create_dataset("joint_pos", (total_datapoints, 6), dtype="float")
dset3 = f.create_dataset("joint_vel", (total_datapoints, 6), dtype="float")
dset4 = f.create_dataset("distance", (total_datapoints, 1), dtype="float")
# Step while IK movement has not begun
returnCode, signalValue = vrep.simxGetIntegerSignal(
clientID, "ikstart", vrep.simx_opmode_streaming)
while (signalValue == 0):
vrep.simxSynchronousTrigger(clientID)
vrep.simxGetPingTime(clientID)
returnCode, signalValue = vrep.simxGetIntegerSignal(
clientID, "ikstart", vrep.simx_opmode_streaming)
for i in range(total_datapoints):
# At start of each episode, check if path has been found in vrep
if (i % steps_per_episode) == 0:
returnCode, signalValue = vrep.simxGetIntegerSignal(
clientID, "ikstart", vrep.simx_opmode_streaming)
while (signalValue == 0):
vrep.simxSynchronousTrigger(clientID)
vrep.simxGetPingTime(clientID)
returnCode, signalValue = vrep.simxGetIntegerSignal(
clientID, "ikstart", vrep.simx_opmode_streaming)
episode_count += 1
print "Episode: ", episode_count
# obtain image and place into array
err, resolution, image = vrep.simxGetVisionSensorImage(
clientID, v1, 0, vrep.simx_opmode_buffer)
img = np.array(image, dtype=np.uint8)
dset1[i] = img
# obtain joint pos and place into array
jointpos = np.zeros(6)
for j in range(6):
err, jp = vrep.simxGetJointPosition(clientID, jhList[j],
vrep.simx_opmode_buffer)
jointpos[j] = jp
dset2[i] = jointpos
# obtain distance and place into array
distance = np.zeros(1)
err, distance_to_target = vrep.simxReadDistance(
clientID, distance_handle, vrep.simx_opmode_buffer)
distance[0] = distance_to_target
dset4[i] = distance
# trigger next step and wait for communication time
vrep.simxSynchronousTrigger(clientID)
vrep.simxGetPingTime(clientID)
# stop the simulation:
vrep.simxStopSimulation(clientID, vrep.simx_opmode_blocking)
# Now close the connection to V-REP:
vrep.simxFinish(clientID)
# calculate joint velocities excluding final image
for k in range(number_episodes):
for i in range(steps_per_episode - 1):
#jointvel = dset2[k*steps_per_episode + i+1]-dset2[k*steps_per_episode + i]
jointvel = joint_difference(dset2[k * steps_per_episode + i],
dset2[k * steps_per_episode + i + 1])
abs_sum = np.sum(np.absolute(jointvel))
if abs_sum == 0:
dset3[k * steps_per_episode + i] = np.zeros(6)
else: # abs sum norm
dset3[k * steps_per_episode +
i] = jointvel / abs_sum * jointvel_factor * dset4[
k * steps_per_episode + i]
# close datafile
f.close()
return
clientID = vrep.simxStart(
'127.0.0.1', 19997, True, True, -500000,
5) # Connect to V-REP, set a very large time-out for blocking commands
if clientID != -1:
print('Connected to remote API server')
emptyBuff = bytearray()
# Start the simulation:
vrep.simxStartSimulation(clientID, vrep.simx_opmode_oneshot_wait)
# Load a robot instance: res,retInts,retFloats,retStrings,retBuffer=vrep.simxCallScriptFunction(clientID,'remoteApiCommandServer',vrep.sim_scripttype_childscript,'loadRobot',[],[0,0,0,0],['d:/v_rep/qrelease/release/test.ttm'],emptyBuff,vrep.simx_opmode_oneshot_wait)
# robotHandle=retInts[0]
# Retrieve some handles:
res, robotHandle = vrep.simxGetObjectHandle(clientID, 'UR5#',
vrep.simx_opmode_oneshot_wait)
res, target1 = vrep.simxGetObjectHandle(clientID, 'testPose1#',
vrep.simx_opmode_oneshot_wait)
# Retrieve the poses (i.e. transformation matrices, 12 values, last row is implicit) of some dummies in the scene
res, retInts, target1Pose, retStrings, retBuffer = vrep.simxCallScriptFunction(
clientID, 'remoteApiCommandServer', vrep.sim_scripttype_childscript,
'getObjectPose', [target1], [], [], emptyBuff,
vrep.simx_opmode_oneshot_wait)
res, retInts, robotInitialState, retStrings, retBuffer = vrep.simxCallScriptFunction(
clientID, 'remoteApiCommandServer', vrep.sim_scripttype_childscript,
'getRobotState', [robotHandle], [], [], emptyBuff,
vrep.simx_opmode_oneshot_wait)
# Some parameters:
def get_handle(self, name):
"""
Get a handle of an object identified by [name] in vrep simulation
"""
return vrep.simxGetObjectHandle(self.client_id, name,
vrep.simx_opmode_blocking)[1]
returnProlog = (check_output("swipl -s script_prolog.pl %s %s %s" % (sensors[0], sensors[1], sensors[2]), shell=True).decode('utf-8'))
splitProlog = []
splitProlog = teste = returnProlog.split(',')
vl = splitProlog[0].split('=')[1]
vr = splitProlog[1].split('=')[1]
if(vl == 'Y'):
vl = vr
return (float(vl), float(vr))
clientID = vrep.simxStart(serverIP,serverPort,True,True,2000,5)
if clientID <> -1:
print ('Servidor conectado!')
# inicialização dos motores
erro, leftMotorHandle = vrep.simxGetObjectHandle(clientID,'Pioneer_p3dx_leftMotor',vrep.simx_opmode_oneshot_wait)
if erro <> 0:
print 'Handle do motor esquerdo nao encontrado!'
else:
print 'Conectado ao motor esquerdo!'
erro, rightMotorHandle = vrep.simxGetObjectHandle(clientID,'Pioneer_p3dx_rightMotor',vrep.simx_opmode_oneshot_wait)
if erro <> 0:
print 'Handle do motor direito nao encontrado!'
else:
print 'Conectado ao motor direito!'
#inicialização dos sensores (remoteApi)
for i in range(4):
erro, sensorHandle[i] = vrep.simxGetObjectHandle(clientID,"Pioneer_p3dx_ultrasonicSensor%d" % (i+1),vrep.simx_opmode_oneshot_wait)
if erro <> 0:
def invRightArm(xr, yr, zr):
#start at zero position
r = 0
rpose = rightArmPose(0, r, r, r, r, r, r, r)
l = 0
lpose = leftArmPose(0, l, l, l, l, l, l, l)
moveObj(rpose, clientID, objHandleRightTheoretical)
moveObj(lpose, clientID, objHandleLeftTheoretical)
for i in range(0, 7):
vrep.simxSetJointTargetPosition(clientID, rightArm[i], math.radians(r),
vrep.simx_opmode_oneshot)
vrep.simxSetJointTargetPosition(clientID, leftArm[i], math.radians(l),
vrep.simx_opmode_oneshot)
rightTip = int(
vrep.simxGetObjectHandle(clientID, 'Baxter_rightArm_tip',
vrep.simx_opmode_blocking)[1])
T_2in0 = np.array([[1, 0, 0, xr], [0, 1, 0, yr], [0, 0, 1, zr],
[0.00000000, 0.00000000, 0.00000000, 1.00000000]])
moveObj(T_2in0, clientID, objHandleRightTheoretical)
rightLimits = [(math.radians(-168), math.radians(-168 + 336)),
(math.radians(-96.5), math.radians(-96.5 + 191)),
(math.radians(-121), math.radians(-121 + 179)),
(math.radians(-173), math.radians(-173 + 346)),
(math.radians(0), math.radians(0 + 148)),
(math.radians(-173.3), math.radians(-173.3 + 346.5)),
(math.radians(-88), math.radians(-88 + 206)),
(math.radians(-173.3), math.radians(-173.3 + 346.5))]
SR = rightArmS()
thetaR = np.zeros((SR.shape[1], 1))
thetadot = np.array([100])
TR_1in0 = MR
J = Jmaker(thetaR, SR)
V0 = dvskew(sl.logm(T_2in0.dot(nl.inv(TR_1in0))))
V = nl.inv(admaker(TR_1in0)) @ V0
while (True):
count = 0
failCount = 0
while nl.norm(V) >= .01 and nl.norm(thetadot) >= .0001:
thetadot = td(J, V0, thetaR)
thetaR = thetaR + thetadot * 1
TR_1in0 = TtoM(thetaR, SR, MR)
J = Jmaker(thetaR, SR)
V0 = dvskew(sl.logm(T_2in0.dot(nl.inv(TR_1in0))))
V = nl.inv(admaker(TR_1in0)) @ V0
count = count + 1
if (count == 60):
print(
"Norm of V could not merge - Using new starting position and trying again"
)
print()
thetaR = np.array([[random.uniform(-3.14, 3.14)],
[random.uniform(-3.14, 3.14)],
[random.uniform(-3.14, 3.14)],
[random.uniform(-3.14, 3.14)],
[random.uniform(-3.14, 3.14)],
[random.uniform(-3.14, 3.14)],
[random.uniform(-3.14, 3.14)],
[random.uniform(-3.14, 3.14)]])
thetadot = np.array([100])
TR_1in0 = TtoM(thetaR, SR, MR)
J = Jmaker(thetaR, SR)
V0 = dvskew(sl.logm(T_2in0.dot(nl.inv(TR_1in0))))
V = dvskew(sl.logm(T_2in0.dot(nl.inv(TR_1in0))))
count = 0
failCount = failCount + 1
if (failCount >= 7):
print("Position Unreachable")
print()
return
# print(repr(thetaR))
#checking if the angles are within limit of their respective joints
if (check(thetaR, rightLimits)):
time.sleep(1)
vrep.simxSetJointTargetPosition(clientID, rotJoint, thetaR[0],
vrep.simx_opmode_oneshot)
for i in range(0, 7):
vrep.simxSetJointTargetPosition(clientID, rightArm[i],
thetaR[i + 1],
vrep.simx_opmode_oneshot)
time.sleep(.5)
time.sleep(.5)
leftEndTip = vrep.simxGetObjectPosition(
clientID, rightTip, -1, vrep.simx_opmode_blocking)[1]
if (nl.norm(T_2in0[:3, 3] - np.array(leftEndTip)) > 0.01):
#checking final pose, in case any obsticles got in the way (even though joint angles were okay.)
print(
"Hmmmm, the end of the baxter arm is not where it should be. Collision detected"
)
print("Difference: " +
str(nl.norm(T_2in0[:3, 3] - np.array(leftEndTip))))
print()
thetaR = np.array([[random.uniform(-3.14, 3.14)],
[random.uniform(-3.14, 3.14)],
[random.uniform(-3.14, 3.14)],
[random.uniform(-3.14, 3.14)],
[random.uniform(-3.14, 3.14)],
[random.uniform(-3.14, 3.14)],
[random.uniform(-3.14, 3.14)],
[random.uniform(-3.14, 3.14)]])
thetadot = np.array([100])
TR_1in0 = TtoM(thetaR, SR, MR)
J = Jmaker(thetaR, SR)
V0 = dvskew(sl.logm(T_2in0.dot(nl.inv(TR_1in0))))
V = dvskew(sl.logm(T_2in0.dot(nl.inv(TR_1in0))))
continue
print("BINGO! Position Achieved!")
print()
time.sleep(3)
break
else:
print(
"Came up with set of thetas where some of them where out of range, trying again with different starting position"
)
print()
#try different starting orientation to get different end angles
thetaR = np.array([[random.uniform(-3.14, 3.14)],
[random.uniform(-3.14, 3.14)],
[random.uniform(-3.14, 3.14)],
[random.uniform(-3.14, 3.14)],
[random.uniform(-3.14, 3.14)],
[random.uniform(-3.14, 3.14)],
[random.uniform(-3.14, 3.14)],
[random.uniform(-3.14, 3.14)]])
thetadot = np.array([100])
TR_1in0 = TtoM(thetaR, SR, MR)
J = Jmaker(thetaR, SR)
V0 = dvskew(sl.logm(T_2in0.dot(nl.inv(TR_1in0))))
V = dvskew(sl.logm(T_2in0.dot(nl.inv(TR_1in0))))
DirectionProbability = 50
Turning = False
LeftMotorSignal = 2
RightMotorSignal = 2
Speed = 2
print ('Program started')
vrep.simxFinish(-1) # just in case, close all opened connections
clientID = vrep.simxStart('127.0.0.1', 19999, True, True, 5000, 5) # Connect to V-REP
if clientID != -1:
print ('Connected to remote API server')
errorCode, LeftMotor = vrep.simxGetObjectHandle(clientID, "Pioneer_p3dx_leftMotor", vrep.simx_opmode_oneshot_wait)
print errorCode
print LeftMotor
errorCode, RightMotor = vrep.simxGetObjectHandle(clientID, "Pioneer_p3dx_rightMotor", vrep.simx_opmode_oneshot_wait)
print errorCode
print RightMotor
errorCode, front_left = vrep.simxGetObjectHandle(clientID, "Front_Left",
vrep.simx_opmode_oneshot_wait)
errorCode, front_right = vrep.simxGetObjectHandle(clientID, "Front_Left",
vrep.simx_opmode_oneshot_wait)
errorCode, sLeft = vrep.simxGetObjectHandle(clientID, "Pioneer_p3dx_ultrasonicSensor2",
vrep.simx_opmode_oneshot_wait)
errorCode, sRight = vrep.simxGetObjectHandle(clientID, "Pioneer_p3dx_ultrasonicSensor7",
vrep.simx_opmode_oneshot_wait)
while True:
errorCode, detectionState1, detectedPoint1, detectedObjectHandle, detectedSurfaceNormalVector = vrep.simxReadProximitySensor(
def get_robot_handles (clientID, robo):
if robo == 0:
# Get "handle" to ALL joints of robot
# Get "handle" to the first joint of robot
result, robot0_joint_one_handle = vrep.simxGetObjectHandle(clientID, 'UR3_joint1', vrep.simx_opmode_blocking)
if result != vrep.simx_return_ok:
raise Exception('could not get object handle for first joint')
# Get "handle" to the second joint of robot
result, robot0_joint_two_handle = vrep.simxGetObjectHandle(clientID, 'UR3_joint2', vrep.simx_opmode_blocking)
if result != vrep.simx_return_ok:
raise Exception('could not get object handle for first joint')
# Get "handle" to the third joint of robot
result, robot0_joint_three_handle = vrep.simxGetObjectHandle(clientID, 'UR3_joint3', vrep.simx_opmode_blocking)
if result != vrep.simx_return_ok:
raise Exception('could not get object handle for first joint')
# Get "handle" to the fourth joint of robot
result, robot0_joint_four_handle = vrep.simxGetObjectHandle(clientID, 'UR3_joint4', vrep.simx_opmode_blocking)
if result != vrep.simx_return_ok:
raise Exception('could not get object handle for first joint')
# Get "handle" to the fifth joint of robot
result, robot0_joint_five_handle = vrep.simxGetObjectHandle(clientID, 'UR3_joint5', vrep.simx_opmode_blocking)
if result != vrep.simx_return_ok:
raise Exception('could not get object handle for first joint')
# Get "handle" to the sixth joint of robot
result, robot0_joint_six_handle = vrep.simxGetObjectHandle(clientID, 'UR3_joint6', vrep.simx_opmode_blocking)
if result != vrep.simx_return_ok:
raise Exception('could not get object handle for first joint')
# Wait two seconds
time.sleep(2)
robo_handles = np.array([robot0_joint_one_handle, robot0_joint_two_handle, robot0_joint_three_handle, robot0_joint_four_handle, robot0_joint_five_handle, robot0_joint_six_handle])
return robo_handles
elif robo == 1:
# Get "handle" to ALL joints of robot
# Get "handle" to the first joint of robot
result, robot1_joint_one_handle = vrep.simxGetObjectHandle(clientID, 'UR3_joint1#0', vrep.simx_opmode_blocking)
if result != vrep.simx_return_ok:
raise Exception('could not get object handle for first joint')
# Get "handle" to the second joint of robot
result, robot1_joint_two_handle = vrep.simxGetObjectHandle(clientID, 'UR3_joint2#0', vrep.simx_opmode_blocking)
if result != vrep.simx_return_ok:
raise Exception('could not get object handle for second joint')
# Get "handle" to the third joint of robot
result, robot1_joint_three_handle = vrep.simxGetObjectHandle(clientID, 'UR3_joint3#0', vrep.simx_opmode_blocking)
if result != vrep.simx_return_ok:
raise Exception('could not get object handle for third joint')
# Get "handle" to the fourth joint of robot
result, robot1_joint_four_handle = vrep.simxGetObjectHandle(clientID, 'UR3_joint4#0', vrep.simx_opmode_blocking)
if result != vrep.simx_return_ok:
raise Exception('could not get object handle for fourth joint')
# Get "handle" to the fifth joint of robot
result, robot1_joint_five_handle = vrep.simxGetObjectHandle(clientID, 'UR3_joint5#0', vrep.simx_opmode_blocking)
if result != vrep.simx_return_ok:
raise Exception('could not get object handle for fifth joint')
# Get "handle" to the sixth joint of robot
result, robot1_joint_six_handle = vrep.simxGetObjectHandle(clientID, 'UR3_joint6#0', vrep.simx_opmode_blocking)
if result != vrep.simx_return_ok:
raise Exception('could not get object handle for sixth joint')
# Wait two seconds
time.sleep(2)
robo_handles = np.array([robot1_joint_one_handle, robot1_joint_two_handle, robot1_joint_three_handle, robot1_joint_four_handle,
robot1_joint_five_handle, robot1_joint_six_handle])
return robo_handles
else:
raise Exception('Wrong input, could not get handles')
# lastdist = -1
# bestdist = -1
# dist_ckp = [False]*5
print('Program started')
vrep.simxFinish(-1) # just in case, close all opened connections
clientID = vrep.simxStart(
'127.0.0.1', 19997, True, True, -500000,
5) # Connect to V-REP, set a very large time-out for blocking commands
if clientID != -1:
print('Connected to remote API server')
emptyBuff = bytearray()
# Start the simulation:
vrep.simxStartSimulation(clientID, vrep.simx_opmode_oneshot_wait)
#Retrive the body
res, body = vrep.simxGetObjectHandle(clientID, 'body',
vrep.simx_opmode_blocking)
res, goal = vrep.simxGetObjectHandle(clientID, 'Target',
vrep.simx_opmode_blocking)
# Retrieve the poles
pole = np.zeros(19, dtype='int32')
for i in range(1, 19):
res, pole[i] = vrep.simxGetObjectHandle(clientID, 'P' + str(i),
vrep.simx_opmode_blocking)
# Retrive the U1
U1 = np.zeros(6, dtype='int32')
for i in range(0, 6):
res, U1[i] = vrep.simxGetObjectHandle(clientID, 'Hip' + str(i + 1),
vrep.simx_opmode_blocking)
# Retrive the U2
U2 = np.zeros(6, dtype='int32')
res, U2[0] = vrep.simxGetObjectHandle(clientID, 'shaft2',
def getPosition(clientID):
res, obj = vrep.simxGetObjectHandle(clientID, 'Sphere',
vrep.simx_opmode_blocking)
pose = vrep.simxGetObjectPosition(clientID, obj, -1,
vrep.simx_opmode_blocking)
return pose
try:
import vrep
except:
print ('--------------------------------------------------------------')
print ('"vrep.py" could not be imported. This means very probably that')
print ('either "vrep.py" or the remoteApi library could not be found.')
print ('Make sure both are in the same folder as this file,')
print ('or appropriately adjust the file "vrep.py"')
print ('--------------------------------------------------------------')
print ('')
import time
print('Program Started')
vrep.simxFinish(-1) #close all opened connections
clientID = vrep.simxStart('127.0.0.1', 19997, True, True, 5000, 5)
if clientID != -1:
print('Connected to Remote API Server...')
vrep.simxStartSimulation(clientID, vrep.simx_opmode_oneshot)
res, planeHandle = vrep.simxGetObjectHandle(clientID, 'Quadricopter#', vrep.simx_opmode_blocking)
#res, retInt, retFloat, retString, retBuffer = vrep.simxCallScriptFunction(clientID, 'misson_landing', )
time.sleep(10)
vrep.simxStopSimulation(clientID, vrep.simx_opmode_blocking)
vrep.simxFinish(clientID)
else:
print('Failed connecting to remote API server')
print('Program ended')
clientID = vrep.simxStart('127.0.0.1', 19997, True, True, 5000, 5)
if clientID == -1:
raise Exception('Failed connecting to remote API server')
'''
# Get "handle" to the first joint of robot
result, joint_one_handle = vrep.simxGetObjectHandle(clientID, 'UR3_joint1', vrep.simx_opmode_blocking)
if result != vrep.simx_return_ok:
raise Exception('could not get object handle for first joint')
'''
## define Baxter's joints
# define the body joints
bodyJoints = np.zeros(3)
bodyError = np.zeros(3)
vertError, vertJoint = vrep.simxGetObjectHandle(clientID,
'Baxter_verticalJoint',
vrep.simx_opmode_blocking)
rotError, rotJoint = vrep.simxGetObjectHandle(clientID, 'Baxter_rotationJoint',
vrep.simx_opmode_blocking)
monitorError, monitorJoint = vrep.simxGetObjectHandle(
clientID, 'Baxter_monitorJoint', vrep.simx_opmode_blocking)
bodyJoints[0] = vertJoint
bodyError[0] = vertError
bodyJoints[1] = rotJoint
bodyError[1] = rotError
bodyJoints[2] = monitorJoint
bodyError[2] = monitorError
bodyJoints = bodyJoints.astype(int)
bodyError = bodyError.astype(int)
import matplotlib.pyplot as plt
vrep.simxFinish(-1) #Terminar todas las conexiones
clientID = vrep.simxStart(
'127.0.0.1', 19999, True, True, 5000,
5) #Iniciar una nueva conexion en el puerto 19999 (direccion por defecto)
if clientID != -1:
print('Conexion establecida')
else:
sys.exit("Error: no se puede conectar") #Terminar este script
#Guardar la referencia de los motores
_, left_motor_handle = vrep.simxGetObjectHandle(clientID,
'Pioneer_p3dx_leftMotor',
vrep.simx_opmode_oneshot_wait)
_, right_motor_handle = vrep.simxGetObjectHandle(clientID,
'Pioneer_p3dx_rightMotor',
vrep.simx_opmode_oneshot_wait)
#Guardar la referencia de la camara
_, camhandle = vrep.simxGetObjectHandle(clientID, 'Vision_sensor',
vrep.simx_opmode_oneshot_wait)
#acceder a los datos del laser
_, datosLaserComp = vrep.simxGetStringSignal(clientID, 'LaserData',
vrep.simx_opmode_streaming)
velocidad = 0.35 #Variable para la velocidad de los motores
_, tip_position = vrep.simxGetObjectPosition(arm.clientID, arm.tip, -1, vrep.simx_opmode_blocking)
print("vrep_tip", tip_position)
exit()"""
dist = []
for i in range(1000):
joint_positions = (np.random.rand(5) * 2.0) - 1.0
print(joint_positions)
arm.set_joint_positions(joint_positions)
print(arm.get_joint_positions())
print()
arm.render()
DH_tip = arm.get_tip_position()
_, tip = vrep.simxGetObjectHandle(arm.clientID, 'AL5D_tip', vrep.simx_opmode_blocking)
_, real_tip = vrep.simxGetObjectPosition(arm.clientID, tip, -1, vrep.simx_opmode_blocking)
real_tip = np.array(real_tip)
d = np.sqrt(np.sum(np.power(DH_tip - real_tip, 2)))
dist.append(d)
print(DH_tip)
print(real_tip)
time.sleep(3)
print("mean:", np.mean(dist))
print("min:", np.min(dist))
print("max:", np.max(dist))
print("std:", np.std(dist))
exit()
exit()
sensorHandle = []
dist = []
leftMotorHandle = 0
rightMotorHandle = 0
global v_Left, v_Right, tacoDir, tacoEsq
v_Left = 1
v_Right = 1
if (clientID!=-1):
print ("Servidor Conectado!")
#------------------------------Inicializa Sensores ----------------------------
for i in range(0,8):
nomeSensor.append("sensor" + str(i+1))
res, handle = vrep.simxGetObjectHandle(clientID, nomeSensor[i], vrep.simx_opmode_oneshot_wait)
if(res != vrep.simx_return_ok):
print (nomeSensor[i] + " nao conectado")
else:
print (nomeSensor[i] + " conectado")
sensorHandle.append(handle)
#------------------------------Inicializa Motores ----------------------------
resLeft, leftMotorHandle = vrep.simxGetObjectHandle(clientID, "Pioneer_p3dx_leftMotor", vrep.simx_opmode_oneshot_wait)
if(resLeft != vrep.simx_return_ok):
print("Motor Esquerdo : Handle nao encontrado!")
else:
print("Motor Esquerdo: Conectado")
resRight, rightMotorHandle = vrep.simxGetObjectHandle(clientID, "Pioneer_p3dx_rightMotor", vrep.simx_opmode_oneshot_wait)
import forwardKinematics
import vrep
import time
import numpy as np
# Close all open connections (just in case)
vrep.simxFinish(-1)
# Connect to V-REP (raise exception on failure)
clientID = vrep.simxStart('127.0.0.1', 19997, True, True, 5000, 5)
if clientID == -1:
raise Exception('Failed connecting to remote API server')
# Get "handle" to the first joint of robot
result, joint_one_handle = vrep.simxGetObjectHandle(clientID, 'UR3_joint1',
vrep.simx_opmode_blocking)
if result != vrep.simx_return_ok:
raise Exception('could not get object handle for first joint')
# Get "handle" to the second joint of robot
result, joint_two_handle = vrep.simxGetObjectHandle(clientID, 'UR3_joint2',
vrep.simx_opmode_blocking)
if result != vrep.simx_return_ok:
raise Exception('could not get object handle for second joint')
# Get "handle" to the third joint of robot
result, joint_three_handle = vrep.simxGetObjectHandle(
clientID, 'UR3_joint3', vrep.simx_opmode_blocking)
if result != vrep.simx_return_ok:
raise Exception('could not get object handle for third joint')
initialize()
print(clientID)
generation = 1
genome = initgenome() #initialization
cgenome = convert(genome) #normalization
best_genome = cgenome[:]
for j in range(0, 5000):
print("Generation:")
print(generation)
cur_genome = mutate(best_genome[:]) #Mutation
print("Genome:")
print(cur_genome)
vrep.simxLoadModel(clientID, "C:\quad0_1.ttm", 0,
vrep.simx_opmode_blocking) #simulation loading
time.sleep(1)
errorcode, model_handle = vrep.simxGetObjectHandle(
clientID, "quadruped", vrep.simx_opmode_blocking)
i = 0
while i < 8: #8 joints reduced to 4
name = "quad_joint" + str(i + 1)
jointhandles.append(
vrep.simxGetObjectHandle(clientID, name,
vrep.simx_opmode_blocking)[1])
i = i + 1
errorcode, quad = vrep.simxGetObjectHandle(clientID, 'quad_body',
vrep.simx_opmode_blocking)
k = 0
while k <= 10:
for i in range(0, 4):
vrep.simxSetJointTargetVelocity(clientID, jointhandles[i],
i = 0
ur5ready = 0
while i < TIMEOUT and ur5ready == 0:
i = i + 1
errorCode, ur5ready = vrep.simxGetIntegerSignal(clientID, 'UR5READY',
vrep.simx_opmode_blocking)
time.sleep(step)
if i >= TIMEOUT:
print('An error occurred in your V-REP server')
vrep.simxFinish(clientID)
##### Obtain the handle
jointHandle = np.zeros((jointNum, ), dtype=np.int) # 注意是整型
for i in range(jointNum):
errorCode, returnHandle = vrep.simxGetObjectHandle(
clientID, jointName + str(i + 1), vrep.simx_opmode_blocking)
jointHandle[i] = returnHandle
time.sleep(2)
errorCode, holeHandle = vrep.simxGetObjectHandle(clientID, 'Hole',
vrep.simx_opmode_blocking)
errorCode, ikTipHandle = vrep.simxGetObjectHandle(clientID, 'UR5_ikTip',
vrep.simx_opmode_blocking)
errorCode, connectionHandle = vrep.simxGetObjectHandle(
clientID, 'UR5_connection', vrep.simx_opmode_blocking)
print('Handles available!')
##### Obtain the position of the hole
errorCode, targetPosition = vrep.simxGetObjectPosition(
clientID, holeHandle, -1, vrep.simx_opmode_streaming)
[0, 0, 1, 0.0771], [0, 0, 0, 1]])
pos_wrt_robot = np.linalg.solve(transmation_matrix, np.array(pos_wrtworld))
return pos_wrt_robot
if __name__ == "__main__":
rospy.init_node('points_talker', anonymous=True)
error_collect = []
vrep.simxFinish(-1) # just in case, close all opened connections
clientID = vrep.simxStart('127.0.0.1', 19997, True, True, 5000,
5) # Connect to V-REP
joints = [1] * 3
angles = np.zeros(3)
for i in range(3):
errorCode, joints[i] = vrep.simxGetObjectHandle(
clientID, 'IRB140_joint' + str(i + 1), vrep.simx_opmode_blocking)
angles[i] = (vrep.simxGetJointPosition(clientID, joints[i],
vrep.simx_opmode_streaming)[1])
epsilon = 6e-2
for i in range(3):
vrep.simxSetJointTargetVelocity(clientID, joints[i], 0.,
vrep.simx_opmode_oneshot)
vrep.simxSetJointForce(clientID, joints[i], 9999.,
vrep.simx_opmode_oneshot)
# Kp = np.abs(error/np.sum(np.abs(error)))
#Kp = 2000
#Kv = 100
# kp = [2000, 2000,2000]
# kv = [100,100,100]
res, floor_handle = vrep.simxGetObjectHandle(clientID, 'floor_handle',
vrep.simx_opmode_blocking)
clientID = vrep.simxStart(
'127.0.0.1', 19997, True, True, -500000,
5) # Connect to V-REP, set a very large time-out for blocking commands
if clientID != -1:
print('Connected to remote API server')
emptyBuff = bytearray()
# Start the simulation:
vrep.simxStartSimulation(clientID, vrep.simx_opmode_oneshot_wait)
# Load a robot instance: res,retInts,retFloats,retStrings,retBuffer=vrep.simxCallScriptFunction(clientID,'remoteApiCommandServer',vrep.sim_scripttype_childscript,'loadRobot',[],[0,0,0,0],['d:/v_rep/qrelease/release/test.ttm'],emptyBuff,vrep.simx_opmode_oneshot_wait)
# robotHandle=retInts[0]
# Retrieve some handles:
res, robotHandle = vrep.simxGetObjectHandle(clientID, 'IRB4600#',
vrep.simx_opmode_oneshot_wait)
res, target1 = vrep.simxGetObjectHandle(clientID, 'testPose1#',
vrep.simx_opmode_oneshot_wait)
res, target2 = vrep.simxGetObjectHandle(clientID, 'testPose2#',
vrep.simx_opmode_oneshot_wait)
res, target3 = vrep.simxGetObjectHandle(clientID, 'testPose3#',
vrep.simx_opmode_oneshot_wait)
res, target4 = vrep.simxGetObjectHandle(clientID, 'testPose4#',
vrep.simx_opmode_oneshot_wait)
# Retrieve the poses (i.e. transformation matrices, 12 values, last row is implicit) of some dummies in the scene
res, retInts, target1Pose, retStrings, retBuffer = vrep.simxCallScriptFunction(
clientID, 'remoteApiCommandServer', vrep.sim_scripttype_childscript,
'getObjectPose', [target1], [], [], emptyBuff,
vrep.simx_opmode_oneshot_wait)
res, retInts, target2Pose, retStrings, retBuffer = vrep.simxCallScriptFunction(
# Close all open connections (Clear bad cache)
vrep.simxFinish(-1)
# Connect to V-REP (raise exception on failure)
clientID = vrep.simxStart('127.0.0.1', 19999, True, True, 5000, 5)
if clientID == -1:
raise Exception('Failed connecting to remote API server')
# ======================================== Setup "handle" =========================================== #
# Print object name list
result, joint_name, intData, floatData, stringData = vrep.simxGetObjectGroupData(
clientID, vrep.sim_appobj_object_type, 0, vrep.simx_opmode_blocking)
#print(stringData)
# Get "handle" to the base of robot
result, base_handle = vrep.simxGetObjectHandle(clientID, 'UR5_link1_visible',
vrep.simx_opmode_blocking)
if result != vrep.simx_return_ok:
raise Exception('could not get object handle for base frame')
# Get "handle" to the all joints of robot
result, joint_one_handle = vrep.simxGetObjectHandle(clientID, 'UR5_joint1',
vrep.simx_opmode_blocking)
if result != vrep.simx_return_ok:
raise Exception('could not get object handle for first joint')
result, joint_two_handle = vrep.simxGetObjectHandle(clientID, 'UR5_joint2',
vrep.simx_opmode_blocking)
if result != vrep.simx_return_ok:
raise Exception('could not get object handle for second joint')
result, joint_three_handle = vrep.simxGetObjectHandle(
clientID, 'UR5_joint3', vrep.simx_opmode_blocking)
if result != vrep.simx_return_ok:
import sys
import time
vrep.simxFinish(-1) # just in case, close all opened connections
clientID = vrep.simxStart('127.0.0.1', 19999, True, True, 5000,
5) # Connect to V-REP
if clientID != -1:
print('Connected to remote API server')
else:
print("connection not successful")
sys.exit('Could not connect')
errorCode, left_motor_handle = vrep.simxGetObjectHandle(
clientID, 'left_joint', vrep.simx_opmode_oneshot_wait) #初始化
errorCode, right_motor_handle = vrep.simxGetObjectHandle(
clientID, 'right_joint', vrep.simx_opmode_oneshot_wait)
sensor_h = [] #empty list for handles
#for loop to retrieve sensor arrays and initiate sensors
for x in range(0, 5 + 1):
errorCode, sensor_handle = vrep.simxGetObjectHandle(
clientID, 'line_sensor' + str(x), vrep.simx_opmode_oneshot_wait)
sensor_h.append(sensor_handle) #keep list of handles
errorCode, detectionState, auxPackets = vrep.simxReadVisionSensor(
clientID, sensor_handle, vrep.simx_opmode_streaming)
t = time.time()
def _get_object_handle(self, name):
code, handle = v.simxGetObjectHandle(self._id, name, self._def_op_mode)
if code == v.simx_return_ok:
return handle
else:
return None
def get_object_handle(self, string):
#provides object handle for V-REP object
errorCode, handle = vrep.simxGetObjectHandle(
self.clientID, string, vrep.simx_opmode_oneshot_wait)
assert errorCode == 0, 'Conection to ' + string + 'failed'
return handle
if clientID == -1:
print("ERROR: Cannot establish connection to vrep.")
sys.exit()
#getPath.setID(clientID)
# Set Sampling time
vrep.simxSetFloatingParameter(clientID,
vrep.sim_floatparam_simulation_time_step, dt,
vrep.simx_opmode_oneshot)
vrep.simxSynchronous(clientID, True)
########## Get handles from vrep ###########
_, vehicle_handle = vrep.simxGetObjectHandle(clientID, "dyros_vehicle0",
vrep.simx_opmode_blocking)
_, goal_handle = vrep.simxGetObjectHandle(clientID, "GoalPoint0",
vrep.simx_opmode_blocking)
_, colHandle = vrep.simxGetCollectionHandle(clientID, "Ref",
vrep.simx_opmode_blocking)
_, sensorColHandle = vrep.simxGetCollectionHandle(
clientID, "Sensors", vrep.simx_opmode_blocking)
_, refHandle, intData, doubleData, strData = vrep.simxGetObjectGroupData(
clientID, colHandle, 9, vrep.simx_opmode_blocking)
ref_from_scene_pos = np.reshape(doubleData, (len(refHandle), 6))[:, 0:3]
ref_from_scene_ori = np.reshape(doubleData, (len(refHandle), 6))[:, 3:6]
_, goalPos = vrep.simxGetObjectPosition(clientID, goal_handle, -1,
vrep.simx_opmode_blocking)
# Option2. Move dummy
# res, target_handle = vrep.simxGetObjectHandle(clientID, 'UR5_ikTarget', vrep.simx_opmode_blocking)
# vrep.simxSetObjectPosition(clientID, target_handle, -1, [x, y, z], vrep.simx_opmode_oneshot)
#
# Option3. Move robot with given joint angle NO PATH PLANNING
# jointHandles = [-1, -1, -1, -1, -1, -1]
# targetPos1 = [90 * 3.14 / 180, 90 * 3.14 / 180, -90 * 3.14 / 180, 90 * 3.14 / 180, 90 * 3.14 / 180, 90 * 3.14 / 180]
#
# for j in range(0, len(data), 1):
# for i in range(0, 6, 1):
# res, jointHandles[i] = vrep.simxGetObjectHandle(clientID, 'UR5_joint' + str(i + 1), vrep.simx_opmode_blocking)
# res= vrep.simxSetJointTargetPosition(clientID, jointHandles[i], data[j][i], vrep.simx_opmode_blocking)
emptyBuff = bytearray()
# Step1. Get robot and target handle
res, robotHandle = vrep.simxGetObjectHandle(clientID, 'UR5',
vrep.simx_opmode_oneshot_wait)
# Step2. Retrieve the poses (i.e. transformation matrices, 12 values, last row is implicit) of some dummies in the scene
# Step3. Get the robot initial state:
res, retInts, robotInitialState, retStrings, retBuffer = vrep.simxCallScriptFunction(
clientID, 'remoteApiCommandServer', vrep.sim_scripttype_childscript,
'getRobotState', [robotHandle], [], [], emptyBuff,
vrep.simx_opmode_oneshot_wait)
# Set Some parameters:
approachVector = [
0, 0, 1
] # often a linear approach is required. This should also be part of the calculations when selecting an appropriate state for a given pose
maxConfigsForDesiredPose = 20 # we will try to find 10 different states corresponding to the goal pose and order them according to distance from initial state
maxTrialsForConfigSearch = 300 # a parameter needed for finding appropriate goal states
clientID = vrep.simxStart('127.0.0.1', 19997, True, True, 500, 5)
if clientID != -1: # if we connected successfully
print('Connected to remote API server')
# --------------------- Setup the simulation
vrep.simxSynchronous(clientID, True)
joint_names = ['shoulder', 'elbow']
# joint target velocities discussed below
joint_target_velocities = np.ones(len(joint_names)) * 10000.0
# get the handles for each joint and set up streaming
joint_handles = [
vrep.simxGetObjectHandle(clientID, name,
vrep.simx_opmode_blocking)[1]
for name in joint_names
]
# get handle for target and set up streaming
_, target_handle = vrep.simxGetObjectHandle(clientID, 'target',
vrep.simx_opmode_blocking)
dt = .001
vrep.simxSetFloatingParameter(
clientID,
vrep.sim_floatparam_simulation_time_step,
dt, # specify a simulation time step
vrep.simx_opmode_oneshot)
# --------------------- Start the simulation
def get_new_nao_handles(nbrOfNao, clientID, Body):
for i in range(0, nbrOfNao - 1):
print '-> Head for NAO -> ' + str(i + 2)
Body[0].append(
vrep.simxGetObjectHandle(clientID, 'HeadYaw#' + str(i),
vrep.simx_opmode_oneshot_wait)[1])
Body[1].append(
vrep.simxGetObjectHandle(clientID, 'HeadPitch#' + str(i),
vrep.simx_opmode_oneshot_wait)[1])
#Left Leg
print '-> Left Leg for NAO -> ' + str(i + 2)
Body[2].append(
vrep.simxGetObjectHandle(clientID, 'LHipYawPitch3#' + str(i),
vrep.simx_opmode_oneshot_wait)[1])
Body[3].append(
vrep.simxGetObjectHandle(clientID, 'LHipRoll3#' + str(i),
vrep.simx_opmode_oneshot_wait)[1])
Body[4].append(
vrep.simxGetObjectHandle(clientID, 'LHipPitch3#' + str(i),
vrep.simx_opmode_oneshot_wait)[1])
Body[5].append(
vrep.simxGetObjectHandle(clientID, 'LKneePitch3#' + str(i),
vrep.simx_opmode_oneshot_wait)[1])
Body[6].append(
vrep.simxGetObjectHandle(clientID, 'LAnklePitch3#' + str(i),
vrep.simx_opmode_oneshot_wait)[1])
Body[7].append(
vrep.simxGetObjectHandle(clientID, 'LAnkleRoll3#' + str(i),
vrep.simx_opmode_oneshot_wait)[1])
#Right Leg
print '-> Right Leg for NAO -> ' + str(i + 2)
Body[8].append(
vrep.simxGetObjectHandle(clientID, 'RHipYawPitch3#' + str(i),
vrep.simx_opmode_oneshot_wait)[1])
Body[9].append(
vrep.simxGetObjectHandle(clientID, 'RHipRoll3#' + str(i),
vrep.simx_opmode_oneshot_wait)[1])
Body[10].append(
vrep.simxGetObjectHandle(clientID, 'RHipPitch3#' + str(i),
vrep.simx_opmode_oneshot_wait)[1])
Body[11].append(
vrep.simxGetObjectHandle(clientID, 'RKneePitch3#' + str(i),
vrep.simx_opmode_oneshot_wait)[1])
Body[12].append(
vrep.simxGetObjectHandle(clientID, 'RAnklePitch3#' + str(i),
vrep.simx_opmode_oneshot_wait)[1])
Body[13].append(
vrep.simxGetObjectHandle(clientID, 'RAnkleRoll3#' + str(i),
vrep.simx_opmode_oneshot_wait)[1])
#Left Arm
print '-> Left Arm for NAO -> ' + str(i + 2)
Body[14].append(
vrep.simxGetObjectHandle(clientID, 'LShoulderPitch3#' + str(i),
vrep.simx_opmode_oneshot_wait)[1])
Body[15].append(
vrep.simxGetObjectHandle(clientID, 'LShoulderRoll3#' + str(i),
vrep.simx_opmode_oneshot_wait)[1])
Body[16].append(
vrep.simxGetObjectHandle(clientID, 'LElbowYaw3#' + str(i),
vrep.simx_opmode_oneshot_wait)[1])
Body[17].append(
vrep.simxGetObjectHandle(clientID, 'LElbowRoll3#' + str(i),
vrep.simx_opmode_oneshot_wait)[1])
Body[18].append(
vrep.simxGetObjectHandle(clientID, 'LWristYaw3#' + str(i),
vrep.simx_opmode_oneshot_wait)[1])
#Right Arm
print '-> Right Arm for NAO -> ' + str(i + 2)
Body[19].append(
vrep.simxGetObjectHandle(clientID, 'RShoulderPitch3#' + str(i),
vrep.simx_opmode_oneshot_wait)[1])
Body[20].append(
vrep.simxGetObjectHandle(clientID, 'RShoulderRoll3#' + str(i),
vrep.simx_opmode_oneshot_wait)[1])
Body[21].append(
vrep.simxGetObjectHandle(clientID, 'RElbowYaw3#' + str(i),
vrep.simx_opmode_oneshot_wait)[1])
Body[22].append(
vrep.simxGetObjectHandle(clientID, 'RElbowRoll3#' + str(i),
vrep.simx_opmode_oneshot_wait)[1])
Body[23].append(
vrep.simxGetObjectHandle(clientID, 'RWristYaw3#' + str(i),
vrep.simx_opmode_oneshot_wait)[1])
#Left fingers
print '-> Left Fingers for NAO -> ' + str(i + 2)
Body[24].append(
vrep.simxGetObjectHandle(clientID, 'NAO_LThumbBase#' + str(i),
vrep.simx_opmode_oneshot_wait)[1])
Body[24].append(
vrep.simxGetObjectHandle(clientID, 'Revolute_joint8#' + str(i),
vrep.simx_opmode_oneshot_wait)[1])
Body[24].append(
vrep.simxGetObjectHandle(clientID, 'NAO_LLFingerBase#' + str(i),
vrep.simx_opmode_oneshot_wait)[1])
Body[24].append(
vrep.simxGetObjectHandle(clientID, 'Revolute_joint12#' + str(i),
vrep.simx_opmode_oneshot_wait)[1])
Body[24].append(
vrep.simxGetObjectHandle(clientID, 'Revolute_joint14#' + str(i),
vrep.simx_opmode_oneshot_wait)[1])
Body[24].append(
vrep.simxGetObjectHandle(clientID, 'NAO_LRFinger_Base#' + str(i),
vrep.simx_opmode_oneshot_wait)[1])
Body[24].append(
vrep.simxGetObjectHandle(clientID, 'Revolute_joint11#' + str(i),
vrep.simx_opmode_oneshot_wait)[1])
Body[24].append(
vrep.simxGetObjectHandle(clientID, 'Revolute_joint13#' + str(i),
vrep.simx_opmode_oneshot_wait)[1])
Body[25].append(Body[24][i * 8:(i + 1) * 8])
#Right Fingers
print '-> Right Fingers for NAO -> ' + str(i + 2)
Body[26].append(
vrep.simxGetObjectHandle(clientID, 'NAO_RThumbBase#' + str(i),
vrep.simx_opmode_oneshot_wait)[1])
Body[26].append(
vrep.simxGetObjectHandle(clientID, 'Revolute_joint0#' + str(i),
vrep.simx_opmode_oneshot_wait)[1])
Body[26].append(
vrep.simxGetObjectHandle(clientID, 'NAO_RLFingerBase#' + str(i),
vrep.simx_opmode_oneshot_wait)[1])
Body[26].append(
vrep.simxGetObjectHandle(clientID, 'Revolute_joint5#' + str(i),
vrep.simx_opmode_oneshot_wait)[1])
Body[26].append(
vrep.simxGetObjectHandle(clientID, 'Revolute_joint6#' + str(i),
vrep.simx_opmode_oneshot_wait)[1])
Body[26].append(
vrep.simxGetObjectHandle(clientID, 'NAO_RRFinger_Base#' + str(i),
vrep.simx_opmode_oneshot_wait)[1])
Body[26].append(
vrep.simxGetObjectHandle(clientID, 'Revolute_joint2#' + str(i),
vrep.simx_opmode_oneshot_wait)[1])
Body[26].append(
vrep.simxGetObjectHandle(clientID, 'Revolute_joint3#' + str(i),
vrep.simx_opmode_oneshot_wait)[1])
Body[27].append(Body[26][(i + 1) * 8:(i + 2) * 8])
def get_object_handle(self, name):
handle, = self.RAPI_rc(
vrep.simxGetObjectHandle(self.cID, name,
vrep.simx_opmode_blocking))
return handle
