def __init__(self, client_id, gripper_name):
self.client_id = client_id
self.name = gripper_name
res, self.distance_handle = vrep.simxGetDistanceHandle(
self.client_id, "touches", operationMode=vrep.simx_opmode_blocking)
res, self.distance_handle = vrep.simxGetDistanceHandle(
self.client_id, "touches", operationMode=vrep.simx_opmode_blocking)
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 __init__(self):
# 建立通信
vrep.simxFinish(-1)
# 每隔0.2s检测一次，直到连接上V-rep
while True:
self.clientID = vrep.simxStart('127.0.0.1', 19997, True, True,
5000, 5)
if self.clientID != -1:
break
else:
time.sleep(0.1)
print("Failed connecting to remote API server!")
print("Connection success!")
#设置机械臂仿真步长，为保持API端与VREP端相同步长
vrep.simxSetFloatingParameter(self.clientID,
vrep.sim_floatparam_simulation_time_step,
self.dt, vrep.simx_opmode_oneshot)
# 打开同步模式
vrep.simxSynchronous(self.clientID, True)
vrep.simxStartSimulation(self.clientID, vrep.simx_opmode_oneshot)
# 获取句柄joint
self.robot1_jointHandle = np.zeros((self.jointNum, ),
dtype=np.int) # joint 句柄
for i in range(self.jointNum):
_, returnHandle = vrep.simxGetObjectHandle(
self.clientID, self.jointName + str(i + 1),
vrep.simx_opmode_blocking)
self.robot1_jointHandle[i] = returnHandle
# 获取末端frame
_, self.end_handle = vrep.simxGetObjectHandle(
self.clientID, 'end', vrep.simx_opmode_blocking)
_, self.goal_handle = vrep.simxGetObjectHandle(
self.clientID, 'goal_1', vrep.simx_opmode_blocking)
# 障碍最短距离handle
_, self.dist_handle = vrep.simxGetDistanceHandle(
self.clientID, 'dis_robot1', vrep.simx_opmode_blocking)
_, self.end_dis_handle = vrep.simxGetDistanceHandle(
self.clientID, 'robot1_goal', vrep.simx_opmode_blocking)
# 获取link句柄
self.link_handle = np.zeros((self.jointNum, ), dtype=np.int) #link 句柄
for i in range(self.jointNum):
_, returnHandle = vrep.simxGetObjectHandle(
self.clientID, self.linkName + str(i + 1),
vrep.simx_opmode_blocking)
self.link_handle[i] = returnHandle
print('Handles available!')
def initialize_distance_calculation(self):
rc, dist_handle_ = vrep.simxGetDistanceHandle(clientID, 'EE_to_target',
blocking)
if rc == 0:
# print("Distance calculation started.")
rc, dist_to_EE = vrep.simxReadDistance(clientID, dist_handle_,
streaming)
self.dist_handle = dist_handle_
def readDistance(self, name):
if name not in self.distances:
code, handle = vrep.simxGetDistanceHandle(
self.id, name, vrep.simx_opmode_blocking)
if code != 0:
exit('Distance object "{}" not found'.format(name))
self.distances[name] = handle
vrep.simxReadDistance(self.id, handle, vrep.simx_opmode_streaming)
return None
return vrep.simxReadDistance(self.id, self.distances[name],
vrep.simx_opmode_buffer)[1]
def setup_devices():
""" Assign the devices from the simulator to specific IDs """
global robotID, left_motorID, right_motorID, ultraID, rewardRefID, goalID, wall0_collisionID, wall1_collisionID, wall2_collisionID, wall3_collisionID, target_collisionID
# res: result (1(OK), -1(error), 0(not called))
# robot
res, robotID = vrep.simxGetObjectHandle(clientID, 'robot#', WAIT)
# motors
res, left_motorID = vrep.simxGetObjectHandle(clientID, 'leftMotor#', WAIT)
res, right_motorID = vrep.simxGetObjectHandle(clientID, 'rightMotor#',
WAIT)
# ultrasonic sensors
for idx, item in enumerate(config.ultra_distribution):
res, ultraID[idx] = vrep.simxGetObjectHandle(clientID, item, WAIT)
# reward reference distance object
res, rewardRefID = vrep.simxGetDistanceHandle(clientID, 'Distance#', WAIT)
# if res == vrep.simx_return_ok: # [debug]
# print("vrep.simxGetDistanceHandle executed fine")
# goal reference object
res, goalID = vrep.simxGetObjectHandle(clientID, 'Dummy#', WAIT)
# collision object
res, target_collisionID = vrep.simxGetCollisionHandle(
clientID, "targetCollision#", BLOCKING)
res, wall0_collisionID = vrep.simxGetCollisionHandle(
clientID, "wall0#", BLOCKING)
res, wall1_collisionID = vrep.simxGetCollisionHandle(
clientID, "wall1#", BLOCKING)
res, wall2_collisionID = vrep.simxGetCollisionHandle(
clientID, "wall2#", BLOCKING)
res, wall3_collisionID = vrep.simxGetCollisionHandle(
clientID, "wall3#", BLOCKING)
# start up devices
# wheels
vrep.simxSetJointTargetVelocity(clientID, left_motorID, 0, STREAMING)
vrep.simxSetJointTargetVelocity(clientID, right_motorID, 0, STREAMING)
# pose
vrep.simxGetObjectPosition(clientID, robotID, -1, MODE_INI)
vrep.simxGetObjectOrientation(clientID, robotID, -1, MODE_INI)
# reading-related function initialization according to the recommended operationMode
for i in ultraID:
vrep.simxReadProximitySensor(clientID, i, STREAMING)
vrep.simxReadDistance(clientID, rewardRefID, STREAMING)
vrep.simxReadCollision(clientID, wall0_collisionID, STREAMING)
vrep.simxReadCollision(clientID, wall1_collisionID, STREAMING)
vrep.simxReadCollision(clientID, wall2_collisionID, STREAMING)
vrep.simxReadCollision(clientID, wall3_collisionID, STREAMING)
vrep.simxReadCollision(clientID, target_collisionID, STREAMING)
def __init__(self):
# 建立通信
vrep.simxFinish(-1)
# 每隔0.2s检测一次，直到连接上V-rep
while True:
self.clientID = vrep.simxStart('127.0.0.1', 19999, True, True, 5000, 5)
if self.clientID != -1:
break
else:
time.sleep(0.1)
print("Failed connecting to remote API server!")
print("Connection success!")
_, collisionHandle = vrep.simxGetCollisionHandle(self.clientID, 'Collision', vrep.simx_opmode_blocking)
_, self.end1_Handle = vrep.simxGetObjectHandle(self.clientID, 'end', vrep.simx_opmode_blocking)
_, self.end2_Handle = vrep.simxGetObjectHandle(self.clientID, 'end0', vrep.simx_opmode_blocking)
_, self.goal1_Handle = vrep.simxGetObjectHandle(self.clientID, 'goal_1', vrep.simx_opmode_blocking)
_, self.goal2_Handle = vrep.simxGetObjectHandle(self.clientID, 'goal_2', vrep.simx_opmode_blocking)
_, self.ball1Handle = vrep.simxGetObjectHandle(self.clientID, 'ball1',
vrep.simx_opmode_blocking)
_, self.ball2Handle = vrep.simxGetObjectHandle(self.clientID, 'ball2',
vrep.simx_opmode_blocking)
_,self.base1Handle = vrep.simxGetObjectHandle(self.clientID, 'base', vrep.simx_opmode_blocking)
_,self.base2Handle = vrep.simxGetObjectHandle(self.clientID, 'base0', vrep.simx_opmode_blocking)
_,self.distanceHandle=vrep.simxGetDistanceHandle(self.clientID, 'robots_dist', vrep.simx_opmode_blocking)
_, self.collisionHandle = vrep.simxGetCollisionHandle(self.clientID, 'Collision', vrep.simx_opmode_blocking)
print('Handles available!')
# _, self.pos1 = vrep.simxGetObjectPosition(self.clientID,self.ball1Handle, -1, vrep.simx_opmode_blocking)
# _, self.pos2 = vrep.simxGetObjectPosition(self.clientID, self.ball2Handle, -1, vrep.simx_opmode_blocking)
self.pos1 = [-0.5750,0.2959,0.0750] # robot1末端初始位置
self.pos2 = [0.5750,-0.2959,0.0750] # robot2末端初始位置
_, self.goal_1 = vrep.simxGetObjectPosition(self.clientID, self.goal1_Handle,self.base1Handle, vrep.simx_opmode_blocking)
_, self.goal_2 = vrep.simxGetObjectPosition(self.clientID, self.goal2_Handle,self.base2Handle, vrep.simx_opmode_blocking)
del self.goal_1[2]
del self.goal_2[2]
self.goal_1 = np.array(self.goal_1)
self.goal_2 = np.array(self.goal_2)
# 获取 joint 句柄
self.robot1_jointHandle = np.zeros((self.jointNum,), dtype=np.int) # joint 句柄
self.robot2_jointHandle = np.zeros((self.jointNum,), dtype=np.int) # joint 句柄
for i in range(self.jointNum):
_, returnHandle = vrep.simxGetObjectHandle(self.clientID, self.jointName + str(i + 1),
vrep.simx_opmode_blocking)
self.robot1_jointHandle[i] = returnHandle
for i in range(self.jointNum):
_, returnHandle = vrep.simxGetObjectHandle(self.clientID, self.jointName + str(i + 4),
vrep.simx_opmode_blocking)
self.robot2_jointHandle[i] = returnHandle
A_H = [ampitude_h[0] * math.pi / 180.0, ampitude_h[1] * math.pi / 180.0]
A_V = [ampitude_v[0] * math.pi / 180.0, ampitude_v[1] * math.pi / 180.0]
P_V = [phase_v[0] * math.pi / 180.0, phase_v[1] * math.pi / 180.0]
P_H = [phase_h[0] * math.pi / 180.0, phase_h[1] * math.pi / 180.0]
P_C = [phase_cam[0] * math.pi / 180.0, phase_cam[1] * math.pi / 180.0]
s = 0 # if s=0 we use the first set of parameters, if s=1 we use the second set of parameters
#error_code_cam,joint_cam_handle=vrep.simxGetObjectHandle(clientID,'snake_joint_cam',vrep.simx_opmode_oneshot_wait)
joints_h_handles = np.zeros([num_snakes, num_units, 1])
joints_v_handles = np.zeros([num_snakes, num_units, 1])
distance_measurement_handles = np.zeros([num_snakes, 1])
for snake in range(num_snakes):
error_code_dist, distance_measurement_handles[
snake] = vrep.simxGetDistanceHandle(clientID,
'snake' + str(snake) + '_goal',
vrep.simx_opmode_blocking)
for i in range(1, num_units + 1):
error_code_h, joints_h_handles[snake,
i - 1] = vrep.simxGetObjectHandle(
clientID, 'snake_joint_h' + str(i) +
'#' + str(snake),
vrep.simx_opmode_oneshot_wait)
error_code_v, joints_v_handles[snake,
i - 1] = vrep.simxGetObjectHandle(
clientID, 'snake_joint_v' + str(i) +
'#' + str(snake),
vrep.simx_opmode_oneshot_wait)
if error_code_h != 0 or error_code_v != 0: # or error_code_cam!=0:
print("Problem!")
IPython.embed()
def train(load_model_path, save_model_path, number_training_steps,
data_path, image_shape, ratio=1, batch_size=2,
save_online_batch=False, save_online_batch_path=None):
# Initialize connection
connection = VrepConnection()
connection.synchronous_mode()
connection.start()
# Load data
data_file = h5py.File(data_path,"r")
x = h5py_to_array(data_file['images'][:1000], image_shape)
y = data_file['joint_vel'][:1000]
datapoints = x.shape[0]
# Initialize ratios
online_batchsize = int(np.floor(1.0 * batch_size/(ratio + 1)))
data_images_batchsize = int(batch_size - online_batchsize)
current_online_batch = 0
x_batch = np.empty((batch_size,) + image_shape)
y_batch = np.empty((batch_size, 6))
jointpos_array = np.empty((batch_size, 6))
nextpos_array = np.empty((batch_size, 6))
print "Batch size: ", batch_size
print "Online batch size: ", online_batchsize
print "Dataset batch size: ", data_images_batchsize
# Load keras model
model = load_model(load_model_path)
# 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, distanceHandle = vrep.simxGetDistanceHandle(clientID, "tipToTarget", vrep.simx_opmode_blocking)
err, distance_to_target = vrep.simxReadDistance(clientID, distanceHandle, vrep.simx_opmode_streaming)
# Initialize online batch hdf5 file
if save_online_batch:
f = h5py.File(save_online_batch_path, "w")
dset1 = f.create_dataset("images", (batch_size,) + image_shape, dtype=np.float32)
dset2 = f.create_dataset("joint_pos", (batch_size, 6), dtype=np.float32)
dset3 = f.create_dataset("next_pos", (batch_size, 6), dtype=np.float32)
dset4 = f.create_dataset("distance", (batch_size, 1), dtype=np.float32)
dset5 = f.create_dataset("joint_vel", (batch_size, 6), dtype=np.float32)
# 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)
# Iterate over number of steps to train model online
path_empty_counter = 0
step_counter = 0
while step_counter < number_training_steps:
# 1. Obtain image from vision sensor and next path position from Lua script
err, resolution, image = vrep.simxGetVisionSensorImage(clientID, v1, 0, vrep.simx_opmode_buffer)
img = np.resize(image, (1,) + image_shape) # resize into proper shape for input to neural network
img = img.astype(np.uint8)
img = img.astype(np.float32)
img /= 255
inputInts = []
inputFloats = []
inputStrings = []
inputBuffer = bytearray()
err, _, next_pos, _, _ = vrep.simxCallScriptFunction(clientID, 'Mico',
vrep.sim_scripttype_childscript,
'getNextPathPos', inputInts,
inputFloats, inputStrings,
inputBuffer,
vrep.simx_opmode_blocking)
# 2. Pass into neural network to get joint velocities
jointvel = model.predict(img, batch_size=1)[0] # output is a 2D array of 1X6, access the first variable to get vector
stepsize = 1
jointvel *= stepsize
# 3. Apply joint velocities to arm in V-REP
for j in range(6):
err, jp = vrep.simxGetJointPosition(clientID, jhList[j], vrep.simx_opmode_buffer)
jointpos[j] = jp
err = vrep.simxSetJointPosition(clientID, jhList[j], jointpos[j] + jointvel[j], vrep.simx_opmode_oneshot)
err, distance_to_target = vrep.simxReadDistance(clientID, distanceHandle, vrep.simx_opmode_buffer)
# Check if next pos is valid before fitting
if len(next_pos) != 6:
path_empty_counter += 1
continue
ik_jointvel = joint_difference(jointpos, next_pos)
ik_jointvel = ik_jointvel / np.sum(np.absolute(ik_jointvel)) * 0.5 * distance_to_target
ik_jointvel = np.resize(ik_jointvel, (1, 6))
x_batch[current_online_batch] = img[0]
y_batch[current_online_batch] = ik_jointvel[0]
jointpos_array[current_online_batch] = jointpos
nextpos_array[current_online_batch] = next_pos
dset4[current_online_batch] = distance_to_target
current_online_batch += 1
if current_online_batch == online_batchsize:
# Step counter
print "Training step: ", step_counter
step_counter += 1
# Random sample from dataset
if ratio > 0:
indices = random.sample(range(int(datapoints)), int(data_images_batchsize))
indices = sorted(indices)
x_batch[online_batchsize:] = x[indices]
y_batch[online_batchsize:] = y[indices]
dset4[online_batchsize:] = data_file["distance"][indices]
dset2[online_batchsize:] = data_file["joint_pos"][indices]
# 4. Fit model
model.fit(x_batch, y_batch,
batch_size=batch_size,
epochs=1)
# Reset counter
current_online_batch = 0
# Save to online batch dataset
dset1[:] = x_batch
dset5[:] = y_batch
dset2[:online_batchsize] = jointpos_array[:online_batchsize]
dset3[:] = nextpos_array
# Print statements
# print "Predicted joint velocities: ", jointvel, "Abs sum: ", np.sum(np.absolute(jointvel))
# print "IK Joint velocity: ", ik_jointvel, "Abs sum: ", np.sum(np.absolute(ik_jointvel))
# print "Distance to cube: ", distanceToCube
# 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)
# save updated model and delete model
model.save(save_model_path)
del model
# Close file
if save_online_batch:
f.close()
# Error check
print "No of times path is empty:", path_empty_counter
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
ErrorCode, steeringLeft = vrep.simxGetObjectHandle(
clientID, 'nakedCar_steeringLeft', vrep.simx_opmode_oneshot_wait)
ErrorCode, steeringRight = vrep.simxGetObjectHandle(
clientID, 'nakedCar_steeringRight', vrep.simx_opmode_oneshot_wait)
ErrorCode, motorLeft = vrep.simxGetObjectHandle(clientID, 'nakedCar_motorLeft',
vrep.simx_opmode_oneshot_wait)
ErrorCode, motorRight = vrep.simxGetObjectHandle(clientID,
'nakedCar_motorRight',
vrep.simx_opmode_oneshot_wait)
ErrorCode, vision_sensor = vrep.simxGetObjectHandle(
clientID, 'Vision_sensor', vrep.simx_opmode_oneshot_wait)
ErrorCode, leftdis = vrep.simxGetDistanceHandle(clientID, 'left_dist',
vrep.simx_opmode_oneshot_wait)
ErrorCode, rightdis = vrep.simxGetDistanceHandle(clientID, 'right_dist',
vrep.simx_opmode_oneshot_wait)
ErrorCode, totaldis = vrep.simxGetDistanceHandle(clientID, 'total_dist',
vrep.simx_opmode_oneshot_wait)
ErrorCode, car_size = vrep.simxGetDistanceHandle(clientID, 'car_size',
vrep.simx_opmode_oneshot_wait)
ErrorCode, disL = vrep.simxReadDistance(clientID, leftdis,
vrep.simx_opmode_streaming)
ErrorCode, disR = vrep.simxReadDistance(clientID, rightdis,
vrep.simx_opmode_streaming)
ErrorCode, dt = vrep.simxReadDistance(clientID, totaldis,
vrep.simx_opmode_streaming)
desiredWheelRotSpeed = 10
r = 0.15
err, init_cube_pos = vrep.simxGetObjectPosition(clientID, cube, -1,
vrep.simx_opmode_blocking)
# Get joint handles
joint_handles = [-1, -1, -1]
for i in range(3):
err, jh = vrep.simxGetObjectHandle(clientID, "joint" + str(i + 1),
vrep.simx_opmode_blocking)
joint_handles[i] = jh
# Get vision sensor handle
err, vs_diag = vrep.simxGetObjectHandle(clientID, "vs_diag",
vrep.simx_opmode_oneshot_wait)
# Get distance handle
err, distance_handle = vrep.simxGetDistanceHandle(clientID, "tip_to_target",
vrep.simx_opmode_blocking)
if mode == 0:
# Open textfile
textfile = open(textfile_path, "w")
errorfile = open(error_text, "w")
errorfile.write("cx cy cz j1 j2 j3\n")
err_count = 0
success_count = 0
if pos_type == "random":
while success_count < epochs:
print("Epoch: {}/{}".format(success_count + 1, epochs))
# Reset cube pos
cube_x_pos = np.random.choice([-1.0, 1.0]) * np.random.uniform(
def __init__(self, ip, port, num_robots, max_velocity, num_campos,
tempo_por_partida):
self.clientID = vrep.simxStart(ip, port, True, True, 2000, 5)
if self.clientID == -1:
exit()
self.num_robots = num_robots
self.robotObjectHandle = [0] * self.num_robots * num_campos * 2
self.robotOrientationtHandle = [0] * self.num_robots * num_campos * 2
self.leftMotorHandle = [0] * self.num_robots * num_campos * 2
self.rightMotorHandle = [0] * self.num_robots * num_campos * 2
self.leftRodaHandle = [0] * self.num_robots * num_campos * 2
self.rightRodaHandle = [0] * self.num_robots * num_campos * 2
self.golDistHandle = [0] * num_campos * 2
self.golObjectHandle = [0] * num_campos * 2
for i in range(0, num_campos * 2, 2):
resGolT0, self.golObjectHandle[i] = vrep.simxGetObjectHandle(
self.clientID, "gol" + str(i), vrep.simx_opmode_oneshot_wait)
resGolT1, self.golObjectHandle[i + 1] = vrep.simxGetObjectHandle(
self.clientID, "gol" + str(i + 1),
vrep.simx_opmode_oneshot_wait)
resGolTime0, self.golDistHandle[i] = vrep.simxGetDistanceHandle(
self.clientID, "Dist_bola_gol" + str(i),
vrep.simx_opmode_oneshot_wait)
resGolTime1, self.golDistHandle[i +
1] = vrep.simxGetDistanceHandle(
self.clientID,
"Dist_bola_gol" + str(i + 1),
vrep.simx_opmode_oneshot_wait)
if resGolTime0 != vrep.simx_return_ok or resGolTime1 != vrep.simx_return_ok or resGolT0 != vrep.simx_return_ok or resGolT0 != vrep.simx_return_ok:
exit()
resBall, self.ballObjectHandle = vrep.simxGetObjectHandle(
self.clientID, "bola", vrep.simx_opmode_oneshot_wait)
if resBall:
exit()
for i in range(0, self.num_robots * 2 * num_campos * 2, 2):
resBase, self.robotObjectHandle[int(
i / 2)] = vrep.simxGetObjectHandle(
self.clientID, "robo" + str(int(i / 2)),
vrep.simx_opmode_oneshot_wait)
resOrien, self.robotOrientationtHandle[int(
i / 2)] = vrep.simxGetObjectHandle(
self.clientID, "orientacao" + str(int(i / 2)),
vrep.simx_opmode_oneshot_wait)
resLeft, self.leftMotorHandle[int(
i / 2)] = vrep.simxGetObjectHandle(
self.clientID, "motor" + str(i + 1),
vrep.simx_opmode_oneshot_wait)
resRight, self.rightMotorHandle[int(
i / 2)] = vrep.simxGetObjectHandle(
self.clientID, "motor" + str(i),
vrep.simx_opmode_oneshot_wait)
resLeftR, self.leftRodaHandle[int(
i / 2)] = vrep.simxGetObjectHandle(
self.clientID, "Cylinder" + str(i + 1),
vrep.simx_opmode_oneshot_wait)
resRightR, self.rightRodaHandle[int(
i / 2)] = vrep.simxGetObjectHandle(
self.clientID, "Cylinder" + str(i),
vrep.simx_opmode_oneshot_wait)
if resLeft != vrep.simx_return_ok or resRight != vrep.simx_return_ok or resBase != vrep.simx_return_ok or resLeftR != vrep.simx_return_ok or resRightR != vrep.simx_return_ok:
exit()
self.state_size = self.num_robots * 2 * 2
self.action_size = self.num_robots * 9
self.max_velocity = max_velocity
self.max_width = 0.9
self.max_height = 0.64
self.raio_robo = 0.1562
self.z = 0.0382
self.min_dist_gol = 0.13
self.num_campos = num_campos
self.timers = [0.] * num_campos
self.max_time = tempo_por_partida
self.campo = [False] * self.num_campos
self.semafaro = [False] * self.num_campos
self.team = [0] * (self.num_campos * 2)
self.time_step_action = 0.01
self.weight_rewards = [0.17, 0.17, 0.33, 0.33]
self.actions = [(-1, -1), (-1, 0), (-1, 1), (0, -1), (0, 0), (0, 1),
(1, -1), (1, 0), (1, 1)] * self.num_robots
def __enter__(self):
print('[ROBOTENV] Starting environment...')
sync_mode_str = 'TRUE' if self.sync_mode else 'FALSE'
portNb_str = str(self.portNb)
# launch v-rep
vrep_cmd = [
self.vrepPath, '-gREMOTEAPISERVERSERVICE_' + portNb_str +
'_FALSE_' + sync_mode_str
]
if not self.showGUI:
vrep_cmd.append('-h') # headless mode
vrep_cmd.append(self.scenePath)
# headless mode via ssh
# vrep_cmd = "xvfb-run --auto-servernum --server-num=1 /homes/dam416/V-REP_PRO_EDU_V3_4_0_Linux/vrep.sh -h -s -q MicoRobot.ttt"
# vrep_cmd = ['xvfb-run', '--auto-servernum', '--server-num=1', self.vrepPath, '-h', '-s', '-q', self.scenePath]
# vrep_cmd = ['xvfb-run', '--auto-servernum', '--server-num=1', self.vrepPath, '-h', self.scenePath]
print('[ROBOTENV] Launching V-REP...')
# NOTE: do not use "stdout=subprocess.PIPE" below to buffer logs, causes deadlock at episode 464! (flushing the buffer may work... but buffering is not needed)
self.vrepProcess = subprocess.Popen(vrep_cmd,
shell=False,
preexec_fn=os.setsid)
# connect to V-Rep Remote Api Server
vrep.simxFinish(-1) # close all opened connections
# Connect to V-REP
print('[ROBOTENV] Connecting to V-REP...')
counter = 0
while True:
self.clientID = vrep.simxStart('127.0.0.1', self.portNb, True,
False, 5000, 0)
if self.clientID != -1:
break
else:
print("[ROBOTENV] Connection failed, retrying")
counter += 1
if counter == 10:
raise RuntimeError(
'[ROBOTENV] Connection to V-REP failed.')
if self.clientID == -1:
print('[ROBOTENV] Failed connecting to remote API server')
else:
print('[ROBOTENV] Connected to remote API server')
# close model browser and hierarchy window
vrep.simxSetBooleanParameter(self.clientID,
vrep.sim_boolparam_browser_visible,
False, vrep.simx_opmode_blocking)
vrep.simxSetBooleanParameter(self.clientID,
vrep.sim_boolparam_hierarchy_visible,
False, vrep.simx_opmode_blocking)
vrep.simxSetBooleanParameter(self.clientID,
vrep.sim_boolparam_console_visible,
False, vrep.simx_opmode_blocking)
# load scene
# time.sleep(5) # to avoid errors
# returnCode = vrep.simxLoadScene(self.clientID, self.scenePath, 1, vrep.simx_opmode_oneshot_wait) # vrep.simx_opmode_blocking is recommended
# # Start simulation
# # vrep.simxSetIntegerSignal(self.clientID, 'dummy', 1, vrep.simx_opmode_blocking)
# # time.sleep(5) #to center window for recordings
# if self.initial_joint_positions is not None:
# self.initializeJointPositions(self.initial_joint_positions)
# self.startSimulation()
# # returnCode = vrep.simxStartSimulation(self.clientID, vrep.simx_opmode_blocking)
# # printlog('simxStartSimulation', returnCode)
# get handles and start streaming distance to goal
for i in range(0, self.nSJoints):
returnCode, self.jointHandles[i] = vrep.simxGetObjectHandle(
self.clientID, 'Mico_joint' + str(i + 1),
vrep.simx_opmode_blocking)
printlog('simxGetObjectHandle', returnCode)
returnCode, self.fingersH1 = vrep.simxGetObjectHandle(
self.clientID, 'MicoHand_fingers12_motor1',
vrep.simx_opmode_blocking)
returnCode, self.fingersH2 = vrep.simxGetObjectHandle(
self.clientID, 'MicoHand_fingers12_motor2',
vrep.simx_opmode_blocking)
returnCode, self.goalCubeH = vrep.simxGetObjectHandle(
self.clientID, 'GoalCube', vrep.simx_opmode_blocking)
returnCode, self.targetCubePositionH = vrep.simxGetObjectHandle(
self.clientID, 'GoalPosition', vrep.simx_opmode_blocking)
returnCode, self.robotBaseH = vrep.simxGetObjectHandle(
self.clientID, 'Mico_link1_visible', vrep.simx_opmode_blocking)
returnCode, self.jointsCollectionHandle = vrep.simxGetCollectionHandle(
self.clientID, "sixJoints#", vrep.simx_opmode_blocking)
returnCode, self.distToGoalHandle = vrep.simxGetDistanceHandle(
self.clientID, "distanceToGoal#", vrep.simx_opmode_blocking)
returnCode, self.endEffectorH = vrep.simxGetObjectHandle(
self.clientID, "DummyFinger#", vrep.simx_opmode_blocking)
# returnCode, self.distanceToGoal = vrep.simxReadDistance(self.clientID, self.distToGoalHandle, vrep.simx_opmode_streaming) #start streaming
# returnCode, _, _, floatData, _ = vrep.simxGetObjectGroupData(self.clientID, self.jointsCollectionHandle, 15, vrep.simx_opmode_streaming) #start streaming
if self.targetCubePosition is not None:
# hide goal position plane
visibility_layer_param_ID = 10
visible_layer = 1
returnCode = vrep.simxSetObjectIntParameter(
self.clientID, self.targetCubePositionH,
visibility_layer_param_ID, visible_layer,
vrep.simx_opmode_blocking)
# print('simxSetObjectIntParameter return Code: ', returnCode)
self.initializeGoalPosition()
# Start simulation
if self.initial_joint_positions is not None:
self.initializeJointPositions()
self.reset()
# self.startSimulation()
# # returnCode = vrep.simxStartSimulation(self.clientID, vrep.simx_opmode_blocking)
# # printlog('simxStartSimulation', returnCode)
# self.updateState() # default initial state: 180 degrees (=pi radians) for all angles
# check the state is valid
# while True:
# self.updateState()
# print("\nstate: ", self.state)
# print("\nreward: ", self.reward)
# # wait for a state
# if (self.state.shape == (1,self.observation_space_size) and abs(self.reward) < 1E+5):
# print("sent reward3=", self.reward)
# break
print(
'[ROBOTENV] Environment succesfully initialised, ready for simulations'
)
# while vrep.simxGetConnectionId(self.clientID) != -1:
# ##########################EXECUTE ACTIONS AND UPDATE STATE HERE #################################
# time.sleep(0.1)
# #end of execution loop
return self
# Ultrasonic Sensor
#errorCode, sensor1 = vrep.simxGetObjectHandle(clientID, "Pioneer_p3dx_ultrasonicSensor16", vrep.simx_opmode_blocking)
#returnCode, detectionState, detectedPoint, detectedObjectHandle, detectedSurfaceNormalVector = vrep.simxReadProximitySensor(clientID, sensor1, vrep.simx_opmode_streaming)
# Gyro sensor
#errorCode, GyroSensor = vrep.simxGetObjectHandle(clientID, "GyroSensor_reference", vrep.simx_opmode_blocking)
# Accelerometer
#errorCode, AccelSensor = vrep.simxGetObjectHandle(clientID, "Accelerometer_mass", vrep.simx_opmode_blocking)
# GPS
#errorCode, GPS = vrep.simxGetObjectHandle(clientID, "GPS_reference", vrep.simx_opmode_blocking)
#get Distance Object Handle
errorCode, dist_handle = vrep.simxGetDistanceHandle(clientID, "Distance",
vrep.simx_opmode_blocking)
# Create a workbook and add a worksheet.
workbook = xlsxwriter.Workbook('data.xlsx')
worksheet = workbook.add_worksheet()
# Some data we want to write to the worksheet.
defaults = [
'X-posi', 'Y-posi', 'Z-posi', 'gyroX', 'gyroY', 'gyroZ', 'accelX',
'accelY', 'accelZ', 'alpha', 'beta', 'gamma', 'vx', 'vy', 'vz', 'Time',
'dis'
]
t = 0
# Iterate over the data and write it out row by row.
for i, default in enumerate(defaults):
#Instancia objetos no Python para os handlers
codErro, ref = vrep.simxGetObjectHandle(clientID,
'ref',
operationMode=vrep.simx_opmode_oneshot)
#Destino
codErro, target = vrep.simxGetObjectHandle(
clientID, 'target', operationMode=vrep.simx_opmode_oneshot)
#Medidores de colisão
codErro, pioneerColDetect = vrep.simxGetCollisionHandle(
clientID, 'pioneerColDetect', operationMode=vrep.simx_opmode_blocking)
#Medidores de distância
codErro, roboObstDist = vrep.simxGetDistanceHandle(
clientID, 'roboObstDistDetect', operationMode=vrep.simx_opmode_blocking)
codErro, roboTargetDist = vrep.simxGetDistanceHandle(
clientID, 'roboTargetDistDetect', operationMode=vrep.simx_opmode_blocking)
codErro, obstaculos = vrep.simxGetCollectionHandle(
clientID, 'obstaculos#', operationMode=vrep.simx_opmode_oneshot)
codErro, robo = vrep.simxGetObjectHandle(
clientID, 'Pionee_p3dx', operationMode=vrep.simx_opmode_oneshot)
codErro, motorE = vrep.simxGetObjectHandle(clientID, 'Pioneer_p3dx_leftMotor',
vrep.simx_opmode_blocking)
codErro, motorD = vrep.simxGetObjectHandle(clientID, 'Pioneer_p3dx_rightMotor',
vrep.simx_opmode_blocking)
codErro, colisao = vrep.simxReadCollision(
clientID, pioneerColDetect, operationMode=vrep.simx_opmode_streaming)
def control(load_model_path, total_episodes=10, im_function=False):
# Initialize connection
connection = VrepConnection()
connection.synchronous_mode()
connection.start()
# Load keras model
model = load_model(load_model_path, custom_objects={'empty': empty,
'mmd2_rbf_quad': empty})
# Initialize adam optimizer
adam = keras.optimizers.adam(lr=0.001)
model.compile(loss='mean_squared_error',
optimizer=adam,
metrics=['mae'])
# 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, distanceToCubeHandle = vrep.simxGetDistanceHandle(clientID, "tipToCube", vrep.simx_opmode_blocking)
err, distanceToCube = vrep.simxReadDistance(clientID, distanceToCubeHandle, vrep.simx_opmode_streaming)
err, distanceToTargetHandle = vrep.simxGetDistanceHandle(clientID, "tipToTarget", vrep.simx_opmode_blocking)
err, distanceToTarget = vrep.simxReadDistance(clientID, distanceToTargetHandle, vrep.simx_opmode_streaming)
# numpy print options
np.set_printoptions(precision=5)
# 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)
# Iterate over total steps desired
image_shape = (128, 128, 3)
current_episode = 0
step_counter = 0
while current_episode < total_episodes + 1:
# obtain current episode
inputInts = []
inputFloats = []
inputStrings = []
inputBuffer = bytearray()
episode_table = []
while len(episode_table) == 0:
err, episode_table, _, _, _ = vrep.simxCallScriptFunction(clientID, 'Mico',
vrep.sim_scripttype_childscript,
'episodeCount', inputInts,
inputFloats, inputStrings,
inputBuffer, vrep.simx_opmode_blocking)
if episode_table[0] > current_episode:
step_counter = 0
print "Episode: ", episode_table[0]
current_episode = episode_table[0]
step_counter += 1
# 1. Obtain image from vision sensor
err, resolution, img = vrep.simxGetVisionSensorImage(clientID, v1, 0, vrep.simx_opmode_buffer)
img = np.array(img, dtype=np.uint8)
img = np.resize(img, (1,) + image_shape) # resize into proper shape for input to neural network
img = img.astype('float32')
img = img / 255 # normalize input image
original_img = img
if im_function:
#img = adjust_gamma(img, gamma=2)
img = tint_images(img, [1,1,0])
if (current_episode==1) and (step_counter==1):
print "Displaying before and after image transformation"
display_2images(original_img[0], img[0])
plt.show()
# 2. Pass into neural network to get joint velocities
jointvel = model.predict(img, batch_size=1)[0][0] # output is a 2D array of 1X6, access the first variable to get vector
stepsize = 1
jointvel *= stepsize
# 3. Apply joint velocities to arm in V-REP
for j in range(6):
err, jp = vrep.simxGetJointPosition(clientID, jhList[j], vrep.simx_opmode_buffer)
jointpos[j] = jp
err = vrep.simxSetJointPosition(clientID, jhList[j], jointpos[j] + jointvel[j], vrep.simx_opmode_oneshot)
# Obtain distance
err, distanceToCube = vrep.simxReadDistance(clientID, distanceToCubeHandle, vrep.simx_opmode_buffer)
err, distanceToTarget = vrep.simxReadDistance(clientID, distanceToTargetHandle, vrep.simx_opmode_buffer)
# Print statements
# print "Step: ", step_counter
print "Joint velocities: ", jointvel, " Abs sum: %.3f" % np.sum(np.absolute(jointvel))
# print "Distance to cube: %.3f" % distanceToCube, ", Distance to target: %.3f" % distanceToTarget
# trigger next step and wait for communication time
vrep.simxSynchronousTrigger(clientID)
vrep.simxGetPingTime(clientID)
# obtain performance metrics
inputInts = []
inputFloats = []
inputStrings = []
inputBuffer = bytearray()
err, minDistStep, minDist, _, _ = vrep.simxCallScriptFunction(clientID, 'Mico',
vrep.sim_scripttype_childscript,
'performanceMetrics', inputInts,
inputFloats, inputStrings,
inputBuffer, vrep.simx_opmode_blocking)
if res == vrep.simx_return_ok:
print "Min distance: ", minDist
minThreshold = 0.2
print "Success rate: ", 1.0*np.sum(np.array(minDist) <= minThreshold)/len(minDist)
print "Total episodes: ", len(minDist)
print "Average min distance: %.3f" % np.mean(minDist)
print "Standard deviation: %.3f" % np.std(minDist)
# other performance metrics such as success % can be defined (i.e. % reaching certain min threshold)
# stop the simulation:
vrep.simxStopSimulation(clientID,vrep.simx_opmode_blocking)
# Now close the connection to V-REP:
vrep.simxFinish(clientID)
# delete model
del model
return
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
else:
print("Nao conectado ao VRep!!!")
sys.exit("Xau!!")
#Instancia objetos no Python para os handlers
codErro, ref = vrep.simxGetObjectHandle(clientID, 'ref',
vrep.simx_opmode_oneshot)
codErro, dummy = vrep.simxGetObjectHandle(clientID, 'dummy',
vrep.simx_opmode_oneshot)
codErro, pioneerColDetect = vrep.simxGetCollisionHandle(
clientID, 'pioneerColDetect', vrep.simx_opmode_oneshot)
codErro, pioneerDistDetect = vrep.simxGetDistanceHandle(
clientID, 'pioneerDistDetect', vrep.simx_opmode_oneshot)
# codErro, obstaculos = vrep.simxGetCollectionHandle(clientID, 'obstaculos', vrep.simx_opmode_oneshot)
codErro, robo = vrep.simxGetObjectHandle(clientID, 'Pionee_p3dx',
vrep.simx_opmode_oneshot)
codErro, chao = vrep.simxGetObjectHandle(clientID, 'ResizableFloor_5_25',
vrep.simx_opmode_oneshot)
posicao = vrep.simxGetObjectPosition(clientID, robo, -1,
vrep.simx_opmode_buffer)
angulos = vrep.simxGetObjectOrientation(clientID, robo, -1,
vrep.simx_opmode_buffer)
#distancia = vrep.simxReadDistance(clientID, pioneerDistDetect,
#operationMode=vrep.simx_opmode_blocking)
def __init__(self, client_id, handle=None, sensor_array=None):
Sensor.__init__(self, client_id, handle=handle)
res, self.handle = vrep.simxGetDistanceHandle(
self.client_id, 'beacon', vrep.simx_opmode_blocking)
self.last_read = self.read(vrep.simx_opmode_streaming, self.handle)
# time.sleep(5.0)
# sys.exit(0)
# get Handles
jointHandles = [0] * 6
for i in range(0, 6):
returnCode, jointHandles[i] = vrep.simxGetObjectHandle(
clientID, 'Mico_joint' + str(i + 1), vrep.simx_opmode_blocking)
if i == 0: printlog('simxGetObjectHandle', returnCode)
returnCode, fingersH1 = vrep.simxGetObjectHandle(
clientID, 'MicoHand_fingers12_motor1', vrep.simx_opmode_blocking)
returnCode, fingersH2 = vrep.simxGetObjectHandle(
clientID, 'MicoHand_fingers12_motor2', vrep.simx_opmode_blocking)
returnCode, jointsCollectionHandle = vrep.simxGetCollectionHandle(
clientID, "sixJoints#", vrep.simx_opmode_blocking)
returnCode, distToGoalHandle = vrep.simxGetDistanceHandle(
clientID, "distanceToGoal#", vrep.simx_opmode_blocking)
returnCode, distanceToGoal = vrep.simxReadDistance(
clientID, distToGoalHandle,
vrep.simx_opmode_streaming) #start streaming
print('jointHandles', jointHandles)
#Arm
pi = np.pi
vel = 1 # looks fast in simulation!
#some test target positions for the 6 joints, in radians
targetPos0 = np.zeros(6)
targetPosPi = np.array(
[pi] * 6) #default initial state: pi or 180 degrees for all joints
targetPosPiO2 = np.array([pi / 2] * 6)
targetPos1 = np.array([pi / 2] * 6)
def __init__(self):
#建立通信
vrep.simxFinish(-1)
# 每隔0.2s检测一次，直到连接上V-rep
while True:
self.clientID = vrep.simxStart('127.0.0.1', 19999, True, True,
5000, 5)
if self.clientID != -1:
break
else:
time.sleep(0.1)
print("Failed connecting to remote API server!")
print("Connection success!")
# # 设置机械臂仿真步长，为了保持API端与V-rep端相同步长
vrep.simxSetFloatingParameter(self.clientID, vrep.sim_floatparam_simulation_time_step, self.dt,\
vrep.simx_opmode_oneshot)
# # 然后打开同步模式
vrep.simxSynchronous(self.clientID, False)
vrep.simxStartSimulation(self.clientID, vrep.simx_opmode_oneshot)
# vrep.simxSynchronousTrigger(self.clientID)
#获取 joint 句柄
self.robot1_jointHandle = np.zeros((self.jointNum, ),
dtype=np.int) # joint 句柄
self.robot2_jointHandle = np.zeros((self.jointNum, ),
dtype=np.int) # joint 句柄
for i in range(self.jointNum):
_, returnHandle = vrep.simxGetObjectHandle(
self.clientID, self.jointName + str(i + 1),
vrep.simx_opmode_blocking)
self.robot1_jointHandle[i] = returnHandle
for i in range(self.jointNum):
_, returnHandle = vrep.simxGetObjectHandle(
self.clientID, self.jointName + str(i + 4),
vrep.simx_opmode_blocking)
self.robot2_jointHandle[i] = returnHandle
# 获取 Link 句柄
self.robot1_linkHandle = np.zeros((self.jointNum, ),
dtype=np.int) # link 句柄
self.robot2_linkHandle = np.zeros((self.jointNum, ),
dtype=np.int) # link 句柄
for i in range(self.jointNum):
_, returnHandle = vrep.simxGetObjectHandle(
self.clientID, self.linkName + str(i + 1),
vrep.simx_opmode_blocking)
self.robot1_linkHandle[i] = returnHandle
for i in range(self.jointNum):
_, returnHandle = vrep.simxGetObjectHandle(
self.clientID, self.linkName + str(i + 4),
vrep.simx_opmode_blocking)
self.robot2_linkHandle[i] = returnHandle
# 获取碰撞句柄
_, self.robot1_collisionHandle = vrep.simxGetCollisionHandle(
self.clientID, 'Collision_robot1', vrep.simx_opmode_blocking)
_, self.robot2_collisionHandle = vrep.simxGetCollisionHandle(
self.clientID, 'Collision_robot2', vrep.simx_opmode_blocking)
# 获取距离句柄
# _,self.mindist_robot1_Handle = vrep.simxGetDistanceHandle(self.clientID,'dis_robot1',vrep.simx_opmode_blocking)
# _,self.mindist_robot2_Handle = vrep.simxGetDistanceHandle(self.clientID,'dis_robot2', vrep.simx_opmode_blocking)
# _,self.mindist_robots_Handle = vrep.simxGetDistanceHandle(self.clientID,'robots_dist', vrep.simx_opmode_blocking)
_, self.robot1_goal_Handle = vrep.simxGetDistanceHandle(
self.clientID, 'robot1_goal', vrep.simx_opmode_blocking)
_, self.robot2_goal_Handle = vrep.simxGetDistanceHandle(
self.clientID, 'robot2_goal', vrep.simx_opmode_blocking)
# 获取末端句柄
_, self.end1_Handle = vrep.simxGetObjectHandle(
self.clientID, 'end', vrep.simx_opmode_blocking)
_, self.end2_Handle = vrep.simxGetObjectHandle(
self.clientID, 'end0', vrep.simx_opmode_blocking)
_, self.goal1_Handle = vrep.simxGetObjectHandle(
self.clientID, 'goal_1', vrep.simx_opmode_blocking)
_, self.goal2_Handle = vrep.simxGetObjectHandle(
self.clientID, 'goal_2', vrep.simx_opmode_blocking)
print('Handles available!')
_, self.goal_1 = vrep.simxGetObjectPosition(self.clientID,
self.goal1_Handle, -1,
vrep.simx_opmode_blocking)
_, self.goal_2 = vrep.simxGetObjectPosition(self.clientID,
self.goal2_Handle, -1,
vrep.simx_opmode_blocking)
del self.goal_1[2]
del self.goal_2[2]
self.goal_1 = np.array(self.goal_1)
self.goal_2 = np.array(self.goal_2)
self.jointConfig1 = np.zeros((self.jointNum, ))
self.jointConfig2 = np.zeros((self.jointNum, ))
for i in range(self.jointNum):
_, jpos = vrep.simxGetJointPosition(self.clientID,
self.robot1_jointHandle[i],
vrep.simx_opmode_blocking)
self.jointConfig1[i] = jpos
for i in range(self.jointNum):
_, jpos = vrep.simxGetJointPosition(self.clientID,
self.robot2_jointHandle[i],
vrep.simx_opmode_blocking)
self.jointConfig2[i] = jpos
_, collision1 = vrep.simxReadCollision(self.clientID,
self.robot1_collisionHandle,
vrep.simx_opmode_blocking)
_, collision2 = vrep.simxReadCollision(self.clientID,
self.robot2_collisionHandle,
vrep.simx_opmode_blocking)
_, pos1 = vrep.simxGetObjectPosition(self.clientID, self.end1_Handle,
-1, vrep.simx_opmode_blocking)
_, pos2 = vrep.simxGetObjectPosition(self.clientID, self.end2_Handle,
-1, vrep.simx_opmode_blocking)
_, d1 = vrep.simxReadDistance(self.clientID, self.robot1_goal_Handle,
vrep.simx_opmode_blocking)
_, d2 = vrep.simxReadDistance(self.clientID, self.robot2_goal_Handle,
vrep.simx_opmode_blocking)
for i in range(self.jointNum):
_, returnpos = vrep.simxGetObjectPosition(
self.clientID, self.robot1_linkHandle[i], -1,
vrep.simx_opmode_blocking)
for i in range(self.jointNum):
_, returnpos = vrep.simxGetObjectPosition(
self.clientID, self.robot2_linkHandle[i], -1,
vrep.simx_opmode_blocking)
print('programing start!')
vrep.simxSynchronousTrigger(self.clientID) # 让仿真走一步
