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 copyObject(self,name,position,relativeName=None):
"""
Copy and paste an object in a specific position
"""
if name in self.handles.keys():
pass
else:
self.initHandle(name)
objectHandle=self.handles[name]
if relativeName:
if relativeName in self.handles.keys():
pass
else:
self.initHandle(relativeName)
relativeHandle=self.handles[relativeName]
else:
relativeHandle = -1
returnCode, newObjectHandles=vrep.simxCopyPasteObjects(self.clientID, [objectHandle], vrep.simx_opmode_oneshot_wait)
print("newOH",newObjectHandles)
newObjectHandle=newObjectHandles[0]
vrep.simxSetObjectPosition(self.clientID,newObjectHandle,relativeHandle,position,vrep.simx_opmode_oneshot)
return newObjectHandle
def copyRobot(id_robot, num):
ret, item = vrep.simxCopyPasteObjects(0, [id_robot], vrep.simx_opmode_blocking)
print(num)
wrist = getHandle("WristMotor#" + str(num))
elbow = getHandle("ElbowMotor#" + str(num))
shoulder = getHandle("ShoulderMotor#" + str(num))
return item[0], wrist, elbow, shoulder
def duplicate(self):
ret, handles = vrep.simxCopyPasteObjects(self.client_id, [self.handle],
self.BLOCK)
if ret == 0:
# TODO return proper objects, we are not savages.
return handles
else:
raise ConnectionError(VRepError.create(ret))
def __init__(self,clientID):
motors =['motor1','motor2','motor3','motor4','motor5','motor6','motor7']
self.motor_handles = [vrep.simxGetObjectHandle(clientID,name, vrep.simx_opmode_blocking)[1] for name in motors]
sensors = ['head_sensor','sensor1r','sensor1l','sensor2r','sensor2l','sensor3r','sensor3l','sensor4r','sensor4l', 'sensor5r','sensor5l','sensor6r','sensor6l','sensor7r','sensor7l','sensor8r','sensor8l']
self.sensor_handles = [vrep.simxGetObjectHandle(clientID, sensor, vrep.simx_opmode_blocking)[1] for sensor in sensors]
obstacles = ['Obj0','Obj1','Obj2','Obj3','Obj4','Obj5','Obj6','Obj7','Obj8','Obj9','Obj10']
self.obst_handles = [vrep.simxGetObjectHandle(clientID, obj, vrep.simx_opmode_blocking)[1] for obj in obstacles]
body_modules = ['snake_v2', 'module2', 'module3', 'module4', 'module5', 'module6', 'module7', 'module8']
self.body_module_handles = [vrep.simxGetObjectHandle(clientID, obj, vrep.simx_opmode_blocking)[1] for obj in body_modules]
self.snake_body = vrep.simxGetObjectHandle(clientID, 'snake_v2', vrep.simx_opmode_blocking)[1]
print ("\nSet motor torques...\n")
for motor_handle in self.motor_handles:
_=vrep.simxSetJointForce(clientID,motor_handle,5,vrep.simx_opmode_blocking)
if _ !=0: raise Exception()
self.setMotor(clientID, 0, 1)
print ("\nAssign sensor values\n")
for sensor_handle in self.sensor_handles:
_,_,_,_,_ = vrep.simxReadProximitySensor(clientID, sensor_handle, vrep.simx_opmode_streaming)
print ("\nSetting Object postions...\n")
self.obst_count = r.randint(5,8)
for i in range (self.obst_count):
obst = r.choice(self.obst_handles)
_, [obst_copy] = vrep.simxCopyPasteObjects(clientID, [obst], vrep.simx_opmode_blocking)
x = round(r.uniform(-5,8),3)
y = round(r.uniform(-7.5,8),3)
_ = vrep.simxSetObjectPosition(clientID, obst_copy, -1, (x, y, 0.25),vrep.simx_opmode_oneshot)
_ = vrep.simxSetObjectOrientation(clientID, obst_copy, obst_copy, (r.randint(0,180),0,0), vrep.simx_opmode_oneshot)
#_ = vrep.simxSetObjectPosition(clientID, self.snake_body, -1, (-9.5, 10, 0.085), vrep.simx_opmode_oneshot)
#---------------------------------------------Initialising the motor position buffer
self.modulepos = list()
for motor_handle in self.motor_handles:
_,moduleCord = vrep.simxGetObjectPosition(clientID,motor_handle,-1,vrep.simx_opmode_streaming)
self.modulepos.append(moduleCord)
if _ !=1 :
print (_)
raise Exception()
#---------------------------------------------Initialising body_modules collision buffer
for module in self.body_module_handles:
_,collision_state = vrep.simxReadCollision(clientID, module, vrep.simx_opmode_streaming)
print _
def _add_to_env(self, cell):
# cell[0]: type of cell. 1: red. 2: white.
if (cell[0] == 1):
cell_type = 'red'
elif (cell[0] == 2):
cell_type = 'white'
# get object handle to copy
res, obj_h = vrep.simxGetObjectHandle(self.clientID, cell_type,
vrep.simx_opmode_blocking)
if res == vrep.simx_return_ok:
print(
"Get Red cell handle"
) # display the reply from V-REP (in this case, the handle of the created dummy)
return True
else:
print('Remote function call failed: Get red cell handle')
return False
res = vrep.simxCopyPasteObjects(self.clientID, obj_h,
vrep.simx_opmode_blocking)
def copy_paste(self):
"""Copy and paste object."""
if self._handle < 0:
if self._handle == MISSING_HANDLE:
raise RuntimeError("Could not copy and paste {}: missing name "
"or handle.".format(self._name))
if self._handle == REMOVED_OBJ_HANDLE:
raise RuntimeError("Could not copy and paste {}: object "
"removed.".format(self._name))
client_id = self.client_id
if client_id is None:
raise ConnectionError(
"Could not copy and paste {}: not connected to V-REP remote "
"API server.".format(self._name))
res, handles = vrep.simxCopyPasteObjects(client_id, [self._handle],
vrep.simx_opmode_blocking)
if res != vrep.simx_return_ok:
raise ServerError("Could not copy and paste {}.".format(
self._name))
return handles[0]
if nbrOfNao == 1:
motionProxy.append(ALProxy('ALMotion', naoIP[0], naoPort[0]))
postureProxy.append(ALProxy('ALRobotPosture', naoIP[0], naoPort[0]))
else:
motionProxy.append(ALProxy('ALMotion', naoIP[0], naoPort[0]))
postureProxy.append(ALProxy('ALRobotPosture', naoIP[0], naoPort[0]))
y = 0.8
# Pause the simulation to avoid collision problems
vrep.simxPauseSimulation(clientID, vrep.simx_opmode_oneshot)
time.sleep(2)
for i in range(0, nbrOfNao - 1):
motionProxy.append(ALProxy('ALMotion', naoIP[i + 1], naoPort[i + 1]))
postureProxy.append(
ALProxy('ALRobotPosture', naoIP[i + 1], naoPort[i + 1]))
# Create new NAo in VRep
vrep.simxCopyPasteObjects(clientID, NAO_Handle,
vrep.simx_opmode_oneshot_wait)
# Get the handle of the new NAO
copyNAO = vrep.simxGetObjectHandle(clientID, 'NAO#' + str(i),
vrep.simx_opmode_oneshot_wait)
# Change the position of the new NAO so it won't collide with others
vrep.simxSetObjectPosition(clientID, copyNAO[1], -1, [0, y, 0.3518],
vrep.simx_opmode_oneshot)
y += 0.8
# Restart the simulation
vrep.simxStartSimulation(clientID, vrep.simx_opmode_oneshot)
print '================ Posture Initialization ================'
# Go to the posture StandZero
print 'Posture Initialization : StandZero'
for i in range(0, nbrOfNao):
motionProxy[i].stiffnessInterpolation('Body', 1.0, 1.0)
cv2.imshow("img",img)
cv2.waitKey()
cv2.destroyAllWindows()
paths = rPoly(img)
vrep_paths = []
for path in paths:
vrep_path = []
for point in path:
vrep_point = (point[0] * 25. / 512. - 12.5, (511 - point[1]) * 25. / 512. - 12.5)
vrep_path.append(vrep_point)
vrep_paths.append(vrep_path)
path = vrep_paths[0]
_, [disc_copy] = vrep.simxCopyPasteObjects(clientID, [disc], vrep.simx_opmode_blocking)
_ = vrep.simxSetObjectPosition(clientID, disc_copy, -1,(path[0][0], path[0][1], 1), vrep.simx_opmode_oneshot)
#---------------------------------------------------------testing remove plane to see the snake
_ = vrep.simxSetObjectPosition(clientID, black_plane, -1, (0,0,-2), vrep.simx_opmode_oneshot)
n=0 #------------------------------------------index of next point on path
ret =1000#--------------------------------------return value of dist2disc
count = 0.0 # --------------------------------Simulation time counter
while count<5000 and n< (len(path)-1):
#----------------------------------------------Reading collision status
# collision = False
y_modulus = np.sqrt(b[0]**2+b[1]**2+b[2]**2)
cos_angle = dot / x_modulus / y_modulus
angle = np.arccos(cos_angle) # angle in radiant
if a[2]>0:
return angle
else:
return -angle
errorCode,Ball=vrep.simxGetObjectHandle(clientID,'A_star_points',vrep.simx_opmode_oneshot_wait)
objectHandles=np.array([Ball])
for next in path2:
(a,b,c)=next
x=0.4+0.4*a
y=0.2+0.4*b
z=0.3+0.4*c
returnCode,newObjectHandles=vrep.simxCopyPasteObjects(clientID,objectHandles,vrep.simx_opmode_oneshot_wait)
Ball_new=newObjectHandles[0]
vrep.simxSetObjectPosition (clientID,Ball_new,-1,(x,y,z),vrep.simx_opmode_oneshot)
#interpolation
#1. step elimination of unnecessary nodes in the path, makes the path shorter, because of more direct movements
in_progress=1
while in_progress>0:
in_progress=0
i=0
while i <(len(path2)-2):
if collision(path2[i],path2[i+2]):
path2.pop(i+1)
in_progress=1
i=i+1
#Proxy initialization
if nbrOfNao == 1:
motionProxy.append(ALProxy('ALMotion',naoIP[0],naoPort[0]))
postureProxy.append(ALProxy('ALRobotPosture', naoIP[0], naoPort[0]))
else:
motionProxy.append(ALProxy('ALMotion',naoIP[0],naoPort[0]))
postureProxy.append(ALProxy('ALRobotPosture', naoIP[0], naoPort[0]))
y=0.8
#Pause the simulation to avoid collision problems
vrep.simxPauseSimulation(clientID,vrep.simx_opmode_oneshot)
time.sleep(2)
for i in range(0,nbrOfNao-1):
motionProxy.append(ALProxy('ALMotion',naoIP[i+1],naoPort[i+1]))
postureProxy.append(ALProxy('ALRobotPosture', naoIP[i+1], naoPort[i+1]))
#Create new NAo in VRep
vrep.simxCopyPasteObjects(clientID,NAO_Handle,vrep.simx_opmode_oneshot_wait)
#Get the handle of the new NAO
copyNAO = vrep.simxGetObjectHandle(clientID,'NAO#'+str(i),vrep.simx_opmode_oneshot_wait)
#Change the position of the new NAO so it won't collide with others
vrep.simxSetObjectPosition(clientID,copyNAO[1],-1,[0,y,0.3518],vrep.simx_opmode_oneshot )
y+=0.8
#Restart the simulation
vrep.simxStartSimulation(clientID,vrep.simx_opmode_oneshot)
print '================ Posture Initialization ================'
#Go to the posture StandZero
print 'Posture Initialization : StandZero'
for i in range(0,nbrOfNao):
motionProxy[i].stiffnessInterpolation('Body', 1.0, 1.0)
postureProxy[i].goToPosture('StandZero',1.0)
def mapgen_fast(scene_name,x,y,z,clientID):
#load mapdata if file exits
if os.path.isfile('./Mapdata/'+scene_name+'.txt'):
fo = open('./Mapdata/'+scene_name+'.txt', "r")
data_string = fo.read();
print "Read String is : ", str
# Close opend file
fo.close()
arr = np.loads(data_string)
return arr
#otherwise generate mapdata
else:
x0=x
y0=y
#generate the 100 sensors and put them to theirstarting position
errorCode,sensor1=vrep.simxGetObjectHandle(clientID,'Sensor_1',vrep.simx_opmode_oneshot_wait)
objectHandles=np.array([sensor1])
sensor_data=np.zeros(shape=(10,10,1),dtype=int)
for x in range(10):
for y in range(10):
returnCode,new_sensor_handle=vrep.simxCopyPasteObjects(clientID,objectHandles,vrep.simx_opmode_oneshot_wait)
vrep.simxReadProximitySensor(clientID,new_sensor_handle[0],vrep.simx_opmode_streaming)
sensor_data[x,y]=new_sensor_handle[0]
x1=0.4+x*0.4
y1=0.2+y*0.4
z1=0.21
vrep.simxSetObjectPosition (clientID,sensor_data[x,y],-1,(x1,y1,z1),vrep.simx_opmode_oneshot)
#init some values
xmax=x0/4
ymax=y0/4
zmax=z/0.4
xmax=int(math.ceil(xmax))
ymax=int(math.ceil(ymax))
zmax=int(math.ceil(zmax))
arr=np.zeros(shape=(xmax*10,ymax*10,zmax),dtype=int)
#loop to detect the whole area
for index in range(xmax):
for index2 in range(ymax):
for index3 in range(zmax):
for x in range(10):
for y in range(10):
errorCode,detectionState1,detectedPoint,detectedObjectHandle,detectedSurfaceNormalVector=vrep.simxReadProximitySensor (clientID,sensor_data[x,y],vrep.simx_opmode_buffer)
#save result
if (detectionState1):
arr[index*10+x,index2*10+y,index3]=1
else:
arr[index*10+x,index2*10+y,index3]=0
for x in range(10):
for y in range(10):
x1=0.4+4*(index)+x*0.4
y1=0.2+4*(index2)+y*0.4
z1=0.4*index3+0.21
vrep.simxSetObjectPosition (clientID,sensor_data[x,y],-1,(x1,y1,z1),vrep.simx_opmode_oneshot)
time.sleep(1)
#save mapdata to file
arr=save(arr)
data_string=np.ndarray.dumps(arr)
print data_string
fo = open('./Mapdata/'+scene_name+'.txt', "w")
fo.write(data_string);
fo.close()
return arr
def mapgen_fast(scene_name,x,y,z,clientID):
if os.path.isfile('./Mapdata/'+scene_name+'.txt'):
fo = open('./Mapdata/'+scene_name+'.txt', "rb")
arr = np.load(fo)['arr_0']
#data_string = fo.read();
#print ("Read String is : ", str)
# Close opend file
fo.close()
#arr = np.loads(data_string)
return arr
else:
x0=x
y0=y
errorCode,sensor1=vrep.simxGetObjectHandle(clientID,'Sensor_1',vrep.simx_opmode_oneshot_wait)
objectHandles=np.array([sensor1])
#create hundred sensors
sensor_data=np.zeros(shape=(10,10,1),dtype=int)
for x in range(10):
for y in range(10):
returnCode,new_sensor_handle=vrep.simxCopyPasteObjects(clientID,objectHandles,vrep.simx_opmode_oneshot_wait)
vrep.simxReadProximitySensor(clientID,new_sensor_handle[0],vrep.simx_opmode_streaming)
sensor_data[x,y]=new_sensor_handle[0]
x1=0.0+x*0.2
y1=0.0+y*0.2
z1=0.0
vrep.simxSetObjectPosition (clientID,sensor_data[x,y],-1,(x1,y1,z1),vrep.simx_opmode_oneshot)
#move sensors to every point of map
xmax=x0/2
ymax=y0/2
zmax=z/0.2
xmax=int(math.ceil(xmax))
ymax=int(math.ceil(ymax))
zmax=int(math.ceil(zmax))
arr=np.zeros(shape=(xmax*10,ymax*10,zmax),dtype=int)
for index in range(xmax):
for index2 in range(ymax):
for index3 in range(zmax):
for x in range(10):
for y in range(10):
#judge barrier
errorCode,detectionState1,detectedPoint,detectedObjectHandle,detectedSurfaceNormalVector=vrep.simxReadProximitySensor (clientID,sensor_data[x,y],vrep.simx_opmode_buffer)
if (detectionState1):
arr[index*10+x,index2*10+y,index3]=1 #save result
else:
arr[index*10+x,index2*10+y,index3]=0
for x in range(10):
for y in range(10):
x1=0.0 + 2*index + x*0.2
y1=0.0 + 2*index2 + y*0.2
z1=0.0 + 0.2*index3
vrep.simxSetObjectPosition (clientID,sensor_data[x,y],-1,(x1,y1,z1),vrep.simx_opmode_oneshot)
time.sleep(0.2)
#arr=save(arr)
data_string=np.ndarray.dumps(arr)
#print ("Data string is:", data_string)
fo = open('./Mapdata/'+scene_name+'.txt', "wb")
np.savez(fo,arr)
#fo.write(data_string);
fo.close()
return arr
def mapgen_fast(scene_name, x, y, z, clientID):
#load mapdata if file exits
if os.path.isfile('./Mapdata/' + scene_name + '.txt'):
fo = open('./Mapdata/' + scene_name + '.txt', "r")
data_string = fo.read()
print "Read String is : ", str
# Close opend file
fo.close()
arr = np.loads(data_string)
return arr
#otherwise generate mapdata
else:
x0 = x
y0 = y
#generate the 100 sensors and put them to theirstarting position
errorCode, sensor1 = vrep.simxGetObjectHandle(
clientID, 'Sensor_1', vrep.simx_opmode_oneshot_wait)
objectHandles = np.array([sensor1])
sensor_data = np.zeros(shape=(10, 10, 1), dtype=int)
for x in range(10):
for y in range(10):
returnCode, new_sensor_handle = vrep.simxCopyPasteObjects(
clientID, objectHandles, vrep.simx_opmode_oneshot_wait)
vrep.simxReadProximitySensor(clientID, new_sensor_handle[0],
vrep.simx_opmode_streaming)
sensor_data[x, y] = new_sensor_handle[0]
x1 = 0.4 + x * 0.4
y1 = 0.2 + y * 0.4
z1 = 0.21
vrep.simxSetObjectPosition(clientID, sensor_data[x, y], -1,
(x1, y1, z1),
vrep.simx_opmode_oneshot)
#init some values
xmax = x0 / 4
ymax = y0 / 4
zmax = z / 0.4
xmax = int(math.ceil(xmax))
ymax = int(math.ceil(ymax))
zmax = int(math.ceil(zmax))
arr = np.zeros(shape=(xmax * 10, ymax * 10, zmax), dtype=int)
#loop to detect the whole area
for index in range(xmax):
for index2 in range(ymax):
for index3 in range(zmax):
for x in range(10):
for y in range(10):
errorCode, detectionState1, detectedPoint, detectedObjectHandle, detectedSurfaceNormalVector = vrep.simxReadProximitySensor(
clientID, sensor_data[x, y],
vrep.simx_opmode_buffer)
#save result
if (detectionState1):
arr[index * 10 + x, index2 * 10 + y,
index3] = 1
else:
arr[index * 10 + x, index2 * 10 + y,
index3] = 0
for x in range(10):
for y in range(10):
x1 = 0.4 + 4 * (index) + x * 0.4
y1 = 0.2 + 4 * (index2) + y * 0.4
z1 = 0.4 * index3 + 0.21
vrep.simxSetObjectPosition(
clientID, sensor_data[x, y], -1, (x1, y1, z1),
vrep.simx_opmode_oneshot)
time.sleep(1)
#save mapdata to file
arr = save(arr)
data_string = np.ndarray.dumps(arr)
print data_string
fo = open('./Mapdata/' + scene_name + '.txt', "w")
fo.write(data_string)
fo.close()
return arr
