def getState_v1(self): #版本二的状态定义，维度是14
jointConfig1 = np.zeros((self.jointNum, ))
jointConfig2 = np.zeros((self.jointNum, ))
for i in range(self.jointNum):
_, jpos = vrep.simxGetJointPosition(self.clientID,
self.robot1_jointHandle[i],
vrep.simx_opmode_blocking)
jointConfig1[i] = jpos
for i in range(self.jointNum):
_, jpos = vrep.simxGetJointPosition(self.clientID,
self.robot2_jointHandle[i],
vrep.simx_opmode_blocking)
jointConfig2[i] = jpos
s = np.hstack((jointConfig1, jointConfig2))
_, 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)
del pos1[2]
del pos2[2]
pos1 = np.array(pos1)
pos2 = np.array(pos2)
pos = np.hstack((pos1, pos2))
s = np.hstack((s, pos))
_, 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)
s = np.hstack((s, np.array([d1, d2])))
# collision1, collision2 = self.getCollisonStates()
# danger = np.array([1. if collision1 else 0., 1. if collision2 else 0.])
# s = np.hstack((s, danger))
return s
def _r_func(self):
t = 20
collision1, collision2 = self.getCollisonStates()
collision = np.array(
[1. if collision1 else 0., 1. if collision2 else 0.])
_, 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)
d = d1 + d2
r = -d
if d <= 0.1 and not self.get_point:
r += 2.
self.grab_counter += 1
if self.grab_counter > t:
r += 10.
self.get_point = True
elif d > 0.1:
self.grab_counter = 0
self.get_point = False
if (collision[0] == 1.):
r -= 10.
self.obstacles_counter += 1
if self.obstacles_counter >= 3:
self.get_obstacles = True
elif collision[0] == 0.:
self.obstacles_counter = 0
self.get_obstacles = False
return r
def _r_func(self):
t = 20
collision1, collision2 = self.getCollisonStates()
collision = np.array(
[1. if collision1 else 0., 1. if collision2 else 0.])
_, d1 = vrep.simxReadDistance(self.clientID, self.robot1_goal_Handle,
vrep.simx_opmode_buffer)
_, d2 = vrep.simxReadDistance(self.clientID, self.robot2_goal_Handle,
vrep.simx_opmode_buffer)
d = d1 + d2
r = -d
if d <= 0.1 and not self.get_point:
r += 50.
self.grab_counter += 1
if self.grab_counter > t:
r += 100
self.get_point = True
elif d > 0.1:
self.grab_counter = 0
self.get_point = False
if (collision[0] == 1.
or collision[1] == 1.) and (not self.get_obstacles):
r = -300 * collision[0] - 300 * collision[1] + r
self.obstacles_counter += 1
self.get_obstacles = True
return r
def getUpDistance(self):
if self.getUpDistanceFirstTime:
error, distance = vrep.simxReadDistance(self.clientId, self.upDistanceHandle, vrep.simx_opmode_streaming)
self.getUpDistanceFirstTime = False
else:
error, distance = vrep.simxReadDistance(self.clientId, self.upDistanceHandle, vrep.simx_opmode_buffer)
return error, distance
def getDistanceToSceneGoal(self):
if self.getDistanceToGoalFirstTime:
errorL, distanceL = vrep.simxReadDistance(self.clientId, self.distLFootToGoalHandle, vrep.simx_opmode_streaming)
errorR, distanceR = vrep.simxReadDistance(self.clientId, self.distRFootToGoalHandle, vrep.simx_opmode_streaming)
self.getDistanceToGoalFirstTime = False
else:
errorL, distanceL = vrep.simxReadDistance(self.clientId, self.distLFootToGoalHandle, vrep.simx_opmode_buffer)
errorR, distanceR = vrep.simxReadDistance(self.clientId, self.distRFootToGoalHandle, vrep.simx_opmode_buffer)
return errorL or errorR, (distanceL + distanceR)*0.5
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 wait_until_stop(self, threshold=0.001):
while True:
res, distance_1 = vrep.simxReadDistance(
self.client_id,
self.distance_handle,
operationMode=vrep.simx_opmode_blocking)
time.sleep(1)
res, distance_2 = vrep.simxReadDistance(
self.client_id,
self.distance_handle,
operationMode=vrep.simx_opmode_blocking)
diff = abs(distance_2 - distance_1)
if diff < threshold:
return
def read(self, mode, handle=None):
"""
Get absolute orientation of object to scene
"""
res, self.last_read = vrep.simxReadDistance(self.client_id,
self.handle,
vrep.simx_opmode_streaming)
return self.last_read
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 get_reward_distance():
""" return the reference distance for reward """
global reward_ref
res, reward_ref = vrep.simxReadDistance(clientID, rewardRefID, BUFFER)
# print(res) # [debug]
# if res == vrep.simx_return_ok: # [debug]
# print("vrep.simxReadDistance executed fine")
# print("reward distance is ",reward_ref)
return reward_ref
def read_dist_to_target(self):
if self.dist_handle is None:
print("Please Initilise distance calculation first...")
return
rc, dist_to_EE = vrep.simxReadDistance(clientID, self.dist_handle,
buffering)
if rc == 0:
return round(dist_to_EE, 3)
else:
return 0
print("Couldn't calculate Distance")
def follow_path(path,
joint_handles,
vs_handle,
distance_handle,
textfile=None):
num_joints = len(joint_handles)
steps = len(path) / num_joints
global image_counter
for s in range(steps):
text_str = str(image_counter)
for j in range(num_joints):
err = vrep.simxSetJointPosition(clientID, joint_handles[j],
path[s * num_joints + j],
vrep.simx_opmode_oneshot)
err, joint_pos = vrep.simxGetJointPosition(
clientID, joint_handles[j], vrep.simx_opmode_blocking)
joint_pos = np.round(joint_pos, decimals=5)
text_str += " " + str(joint_pos)
err, distance = vrep.simxReadDistance(clientID, distance_handle,
vrep.simx_opmode_blocking)
text_str += " " + str(np.round(distance, decimals=5))
vrep.simxSynchronousTrigger(clientID)
vrep.simxGetPingTime(clientID)
if save_images:
err, resolution, image = vrep.simxGetVisionSensorImage(
clientID, vs_handle, 0, vrep.simx_opmode_blocking)
image = np.flip(np.array(image,
dtype=np.uint8).reshape(image_shape),
axis=0)
image = Image.fromarray(image)
image.save(save_path + "image" + str(image_counter) + ".jpg")
image_counter += 1
textfile.write(text_str + "\n")
return
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
vel = desiredWheelRotSpeed * r * 3.6
vrep.simxSetJointTargetVelocity(clientID, motorLeft, desiredWheelRotSpeed,
vrep.simx_opmode_streaming)
vrep.simxSetJointTargetVelocity(clientID, motorRight, desiredWheelRotSpeed,
vrep.simx_opmode_streaming)
d = 0.53
l = 0.68
min_distance_list = []
final_distance_list = []
for episode in range(num_episodes):
print("Episode {}".format(episode + 1))
config = reset_joint_pos(joint_handles)
# Reset cube pos
cube_x_pos = np.random.choice([-1.0, 1.0]) * np.random.uniform(0., 0.3)
cube_y_pos = np.random.uniform(0.15, 0.45)
cube_z_pos = init_cube_pos[2]
vrep.simxSetObjectPosition(clientID, cube, -1,
[cube_x_pos, cube_y_pos, cube_z_pos],
vrep.simx_opmode_oneshot)
err, distance = vrep.simxReadDistance(clientID, distance_handle,
vrep.simx_opmode_blocking)
min_distance = distance
for step in range(steps_per_episode):
err, resolution, image = vrep.simxGetVisionSensorImage(
clientID, vs_diag, 0, vrep.simx_opmode_blocking)
image = np.flip(np.array(
image, dtype=np.uint8).reshape((1, ) + image_shape),
axis=1)
image = (image / 127.5) - 1
pred = sess.run(pred_op, feed_dict={image_op: image})
pred = pred[0]
display(image[0])
for j in range(num_joints):
err, jp = vrep.simxGetJointPosition(clientID, joint_handles[j],
def get_reward(self, team_id, last_state):
#função bonitinha
'''
Virar para a bola -> cos(atan2(Ybola-Yrobô, Xbola-Xrobô)-GAMMArobô)
Ir até a bola -> 1 - DISTÂNCIA_rb/sqrt((2*width)**2+(2*height)**2)
Mira bola p/ gol -> 1 - abs(atan2(Ybola-Yrobô, Xbola-Xrobô)-atan2(Ygol-Ybola, Xgol-Xbola))/pi
Leva bola p/ gol -> 1 - DISTÂNCIA_bg/sqrt((2*width)**2+(2*height)**2)
'''
x_gol = 0.
y_gol = 0.9
enemy_id = 0
if team_id % 2 != 0:
y_gol = -y_gol
enemy_id = team_id - 1
else:
enemy_id = team_id + 1
result_state, final_state = self.get_state(team_id, last_state)
reward = self.weight_rewards[0] * (math.cos((math.atan2(
(result_state[5 * self.num_robots] - result_state[0]) *
self.max_width,
(result_state[5 * self.num_robots + 1] - result_state[1]) *
self.max_height) - result_state[2 * self.num_robots] * math.pi) *
2) / 2 + 0.5)
reward += self.weight_rewards[1] * (1 - math.sqrt((
(result_state[5 * self.num_robots] - result_state[0]) *
self.max_width)**2 + (
(result_state[5 * self.num_robots + 1] - result_state[1]) *
self.max_height)**2) / math.sqrt((2 * self.max_width)**2 +
(2 * self.max_height)**2))
bola_vector2 = (
(result_state[5 * self.num_robots] - result_state[0]) *
self.max_width,
(result_state[5 * self.num_robots + 1] - result_state[1]) *
self.max_height)
gol_vector2 = (y_gol - result_state[0] * self.max_width,
x_gol - result_state[1] * self.max_height)
produto = bola_vector2[0] * gol_vector2[0] + bola_vector2[
1] * gol_vector2[1]
modulo_bola = math.sqrt(bola_vector2[0]**2 + bola_vector2[1]**2)
modulo_gol = math.sqrt(gol_vector2[0]**2 + gol_vector2[1]**2)
cos_angle = min(1, produto / max(1e-10, modulo_bola * modulo_gol))
reward += self.weight_rewards[2] * math.acos(cos_angle) / math.pi
reward += self.weight_rewards[3] * (1 - math.sqrt(
(result_state[5 * self.num_robots] * self.max_width - y_gol)**2 +
(result_state[5 * self.num_robots + 1] * self.max_height -
x_gol)**2) / math.sqrt((2 * self.max_width)**2 +
(2 * self.max_height)**2))
acabou = False
fiz_gol = False
# verifica se alguem fez gol
ret0, dist_gol_other = vrep.simxReadDistance(
self.clientID, self.golDistHandle[enemy_id],
vrep.simx_opmode_oneshot_wait)
ret1, dist_gol_my = vrep.simxReadDistance(
self.clientID, self.golDistHandle[team_id],
vrep.simx_opmode_oneshot_wait)
if ret0 != vrep.simx_return_ok:
dist_gol_other = self.min_dist_gol
if ret1 != vrep.simx_return_ok:
dist_gol_my = self.min_dist_gol
if dist_gol_other < self.min_dist_gol:
acabou = True
fiz_gol = True
self.timers[int(team_id / 2)] += self.max_time
elif dist_gol_my < self.min_dist_gol:
acabou = True
self.timers[int(team_id / 2)] += self.max_time
elif time.time() > self.timers[int(
team_id / 2)] or self.team[enemy_id] == 0:
acabou = True
return fiz_gol, acabou, reward, final_state
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) # 让仿真走一步
def get_reward_distance():
""" return the reference distance for reward """
global reward_ref, clientID
res, reward_ref = vrep.simxReadDistance(clientID, rewardRefID, BUFFER)
return reward_ref
def step(self, action): # action 是速度：rad/s
currentPosition = np.zeros(6, dtype=float)
for i in range(self.jointNum):
_, currentPosition[i] = vrep.simxGetJointPosition(
self.clientID, self.robot1_jointHandle[i],
vrep.simx_opmode_oneshot)
for i in range(self.jointNum):
_, currentPosition[i + 3] = vrep.simxGetJointPosition(
self.clientID, self.robot2_jointHandle[i],
vrep.simx_opmode_oneshot)
action = np.clip(action, *self.action_bound)
self.JointPosition = currentPosition + action * self.dt
self.action = action
vrep.simxPauseCommunication(self.clientID, True)
for i in range(self.jointNum):
vrep.simxSetJointTargetPosition(self.clientID,
self.robot1_jointHandle[i],
self.JointPosition[i],
vrep.simx_opmode_oneshot)
for i in range(self.jointNum):
vrep.simxSetJointTargetPosition(self.clientID,
self.robot2_jointHandle[i],
self.JointPosition[i + 3],
vrep.simx_opmode_oneshot)
vrep.simxPauseCommunication(self.clientID, False)
vrep.simxSynchronousTrigger(self.clientID)
vrep.simxGetPingTime(self.clientID)
#获取转移状态包括 关节位置，末端,连杆距离目标的差
s = np.zeros(6, dtype=float)
for i in range(self.jointNum):
_, s[i] = vrep.simxGetJointPosition(self.clientID,
self.robot1_jointHandle[i],
vrep.simx_opmode_oneshot)
for i in range(self.jointNum):
_, s[i + 3] = vrep.simxGetJointPosition(self.clientID,
self.robot2_jointHandle[i],
vrep.simx_opmode_oneshot)
_, pos1 = vrep.simxGetObjectPosition(self.clientID, self.end_handle,
self.goal1_handle,
vrep.simx_opmode_oneshot)
_, pos2 = vrep.simxGetObjectPosition(self.clientID, self.end0_handle,
self.goal2_handle,
vrep.simx_opmode_oneshot)
del pos1[1]
del pos2[1]
pos = np.array(pos1 + pos2)
s = np.hstack((s, pos))
linkPos = []
for i in range(self.jointNum):
_, linkpos = vrep.simxGetObjectPosition(
self.clientID, self.link_handle1[i], self.goal1_handle,
vrep.simx_opmode_oneshot) # link1 位置
del linkpos[1]
linkPos += linkpos
for i in range(self.jointNum):
_, linkpos = vrep.simxGetObjectPosition(
self.clientID, self.link_handle2[i], self.goal2_handle,
vrep.simx_opmode_oneshot) # link1 位置
del linkpos[1]
linkPos += linkpos
linkPos = np.array(linkPos)
s = np.hstack((s, linkPos))
_, d1 = vrep.simxReadDistance(self.clientID, self.end_dis_handle,
vrep.simx_opmode_oneshot)
_, d2 = vrep.simxReadDistance(self.clientID, self.end0_dis_handle,
vrep.simx_opmode_oneshot)
# Ra = -np.sqrt(np.sum(action**2))
_, do = vrep.simxReadDistance(self.clientID, self.dist_handle,
vrep.simx_opmode_oneshot)
Ro = -(self.d_ref / (self.d_ref + do))**8
if d1 < self.delta:
Rt1 = -0.5 * d1 * d1
else:
Rt1 = -self.delta * (d1 - 0.5 * self.delta)
if d2 < self.delta:
Rt2 = -0.5 * d2 * d2
else:
Rt2 = -self.delta * (d2 - 0.5 * self.delta)
r = (Rt1 + Rt2) * 1000 + Ro * 1200 #DDPG 取100，Naf取500
danger = False
if do < 0.02:
danger = True
r -= 100
else:
danger = False
if d1 < 0.05 and d2 < 0.05 and self.get_point == False:
self.grab_counter += 1
if self.grab_counter > 80:
r += 100
self.get_point = True
r += 20
else:
self.get_point = False
self.grab_counter = 0
return s, r, danger, self.get_point
posX1 = vrep.simxGetFloatSignal(clientID, 'posX1', vrep.simx_opmode_streaming)
posY1 = vrep.simxGetFloatSignal(clientID, 'posY1', vrep.simx_opmode_streaming)
posX2 = vrep.simxGetFloatSignal(clientID, 'posX2', vrep.simx_opmode_streaming)
posY2 = vrep.simxGetFloatSignal(clientID, 'posY2', vrep.simx_opmode_streaming)
posX3 = vrep.simxGetFloatSignal(clientID, 'posX3', vrep.simx_opmode_streaming)
posY3 = vrep.simxGetFloatSignal(clientID, 'posY3', vrep.simx_opmode_streaming)
posX4 = vrep.simxGetFloatSignal(clientID, 'posX4', vrep.simx_opmode_streaming)
posY4 = vrep.simxGetFloatSignal(clientID, 'posY4', vrep.simx_opmode_streaming)
posX5 = vrep.simxGetFloatSignal(clientID, 'posX5', vrep.simx_opmode_streaming)
posY5 = vrep.simxGetFloatSignal(clientID, 'posY5', vrep.simx_opmode_streaming)
posX6 = vrep.simxGetFloatSignal(clientID, 'posX6', vrep.simx_opmode_streaming)
posY6 = vrep.simxGetFloatSignal(clientID, 'posY6', vrep.simx_opmode_streaming)
#Grava posição dos obstaculos
mapaObstaculos = oVrep.carregaObstaculos(clientID)
posRobo = RF.getRobotPosition(clientID)
distRO = vrep.simxReadDistance(clientID,
roboObstDist,
operationMode=vrep.simx_opmode_blocking)
distRT = vrep.simxReadDistance(clientID,
roboTargetDist,
operationMode=vrep.simx_opmode_blocking)
print mapaObstaculos
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 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
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
def check_in(self, threshold=0.001):
_, distance = vrep.simxReadDistance(
self.client_id,
self.distance_handle,
operationMode=vrep.simx_opmode_blocking)
return distance > threshold
def step(self,action):
action = np.clip(action, *self.action_bound)
_, currentball1_pos = vrep.simxGetObjectPosition(self.clientID, self.ball1Handle,self.base1Handle, vrep.simx_opmode_blocking)
_, currentball2_pos = vrep.simxGetObjectPosition(self.clientID, self.ball2Handle,self.base2Handle, vrep.simx_opmode_blocking)
target1_pos = currentball1_pos
target2_pos = currentball2_pos
target1_pos[0] += self.stepsize*np.cos(action[0])
target1_pos[1] += self.stepsize*np.sin(action[0])
target2_pos[0] += self.stepsize*np.cos(action[1])
target2_pos[1] += self.stepsize*np.sin(action[1])
target1_pos = np.array(target1_pos)
target2_pos = np.array(target2_pos)
r1 = np.sqrt(np.sum(target1_pos[0:2]**2))
if target1_pos[0]>=0:
th1 = math.atan(target1_pos[1]/(target1_pos[0]+1e-9))
else:
th1 = np.pi - math.atan(target1_pos[1]/(-target1_pos[0]))
if r1 < 0.4:
r1 = 0.4
if r1 > 2.30/2.0:
r1 = 2.30/2.0
if th1<30.*self.ToRad:
th1 = 30.*self.ToRad
if th1>150.*self.ToRad:
th1 =150. * self.ToRad
target1_pos[0] = r1 * math.cos(th1)
target1_pos[1] = r1 * math.sin(th1)
r2 = np.sqrt(np.sum(target2_pos[0:2] ** 2))
if target2_pos[0] >= 0:
th2 = math.atan(target2_pos[1] / (target2_pos[0] + 1e-9))
else:
th2 = np.pi - math.atan(target2_pos[1] / (-target2_pos[0]))
if r2 < 0.4:
r2 = 0.4
if r2 > 2.30/2:
r2 = 2.30/2
if th2<30.*self.ToRad:
th2 = 30.*self.ToRad
if th2>150.*self.ToRad:
th2 =150. * self.ToRad
target2_pos[0] = r2 * math.cos(th2)
target2_pos[1] = r2 * math.sin(th2)
# time.sleep(0.1)
_ = vrep.simxSetObjectPosition(self.clientID, self.ball1Handle, self.base1Handle, target1_pos, vrep.simx_opmode_oneshot)
_ = vrep.simxSetObjectPosition(self.clientID, self.ball2Handle, self.base2Handle, target2_pos,vrep.simx_opmode_oneshot)
jointConfig1 = np.zeros((self.jointNum,))
jointConfig2 = np.zeros((self.jointNum,))
for i in range(self.jointNum):
_, jpos = vrep.simxGetJointPosition(self.clientID, self.robot1_jointHandle[i], vrep.simx_opmode_blocking)
jointConfig1[i] = jpos
for i in range(self.jointNum):
_, jpos = vrep.simxGetJointPosition(self.clientID, self.robot2_jointHandle[i], vrep.simx_opmode_blocking)
jointConfig2[i] = jpos
s = np.hstack((jointConfig1, jointConfig2))
_, end1 = vrep.simxGetObjectPosition(self.clientID, self.end1_Handle,self.base1Handle, vrep.simx_opmode_blocking)
_, end2 = vrep.simxGetObjectPosition(self.clientID, self.end2_Handle,self.base2Handle, vrep.simx_opmode_blocking)
del end1[2]
del end2[2]
end1 = np.array(end1)
end2 = np.array(end2)
d1 = self.goal_1 - end1
d2 = self.goal_2 - end2
s = np.hstack((s, np.hstack((d1, d2))))
r=0.
dis = np.sqrt(np.sum(d1**2))+np.sqrt(np.sum(d2**2))
r -= dis/2.3
if dis<=0.05:
self.grab_counter +=1
r+=10.
if self.grab_counter>10:
r+=50.
self.get_point = True
else:
self.grab_counter = 0
self.get_point = False
_, d = vrep.simxReadDistance(self.clientID, self.distanceHandle, vrep.simx_opmode_blocking)
if d<0.15 and d>1e-3:
r-=(0.2/(0.2+d))**8
if d<=1e-3:
r-=10.
return s,r,self.get_point,self.get_obstacles
# Absolute Position Get
errorCode, pos = vrep.simxGetObjectPosition(clientID, car_handle, -1,
vrep.simx_opmode_streaming)
#print("Position : {:.5f} {:.5f} {:.5f}\n".format(pos[0], pos[1], pos[2]))
# Orientation Get
errorCode, ori = vrep.simxGetObjectOrientation(
clientID, car_handle, -1, vrep.simx_opmode_streaming)
# Velocity
errorCode, LinearV, AngularV = vrep.simxGetObjectVelocity(
clientID, car_handle,
vrep.simx_opmode_streaming if i is 1 else vrep.simx_opmode_buffer)
# Distance
errorCode, d = vrep.simxReadDistance(clientID, dist_handle,
vrep.simx_opmode_streaming)
# Simulation Time
if t != vrep.simxGetLastCmdTime(clientID):
t = vrep.simxGetLastCmdTime(clientID)
if not i % 100:
print(i)
for col, data in enumerate([
pos[0], pos[1], pos[2], gyroX, gyroY, gyroZ, accelX,
accelY, accelZ, ori[0], ori[1], ori[2], LinearV[0],
LinearV[1], LinearV[2], t, d
]):
worksheet.write(i, col, data)
else:
def step(self, action): # action 是速度：rad/s
currentPosition = np.zeros(3, dtype=float)
for i in range(self.jointNum):
_, currentPosition[i] = vrep.simxGetJointPosition(
self.clientID, self.robot1_jointHandle[i],
vrep.simx_opmode_oneshot)
action = np.clip(action, *self.action_bound)
self.JointPosition = currentPosition + (action * 1 +
0 * self.action) * self.dt
self.action = action
vrep.simxPauseCommunication(self.clientID, True)
for i in range(self.jointNum):
vrep.simxSetJointTargetPosition(self.clientID,
self.robot1_jointHandle[i],
self.JointPosition[i],
vrep.simx_opmode_oneshot)
vrep.simxPauseCommunication(self.clientID, False)
vrep.simxSynchronousTrigger(self.clientID)
vrep.simxGetPingTime(self.clientID)
#获取转移状态包括 关节位置，速度，末端距离目标的差
s = np.zeros(3, dtype=float)
for i in range(self.jointNum):
_, s[i] = vrep.simxGetJointPosition(self.clientID,
self.robot1_jointHandle[i],
vrep.simx_opmode_oneshot)
_, pos = vrep.simxGetObjectPosition(self.clientID, self.end_handle,
self.goal_handle,
vrep.simx_opmode_oneshot)
del pos[1]
pos = np.array(pos)
s = np.hstack(
(np.exp(s) / sum(np.exp(s)), np.exp(pos) / sum(np.exp(pos))))
linkPos = []
for i in range(self.jointNum):
_, linkpos = vrep.simxGetObjectPosition(
self.clientID, self.link_handle[i], self.goal_handle,
vrep.simx_opmode_oneshot) # link1 位置
del linkpos[1]
linkPos += linkpos
linkPos = np.array(linkPos)
s = np.hstack((s, np.exp(linkPos) / sum(np.exp(linkPos))))
_, d = vrep.simxReadDistance(self.clientID, self.end_dis_handle,
vrep.simx_opmode_oneshot)
# Ra = -np.sqrt(np.sum(action**2))
_, do = vrep.simxReadDistance(self.clientID, self.dist_handle,
vrep.simx_opmode_oneshot)
Ro = -(self.d_ref / (self.d_ref + do))**8
if d < self.delta:
Rt = -0.5 * d * d
else:
Rt = -self.delta * (d - 0.5 * self.delta)
r = Rt * 1000 + Ro * 250 #DDPG 取100，Naf取500
danger = False
if do < 0.02:
danger = True
r -= 100
else:
danger = False
if d < 0.05 and self.get_point == False:
self.grab_counter += 1
if self.grab_counter > 80:
r += 100
self.get_point = True
r += 20
else:
self.get_point = False
self.grab_counter = 0
return s, r, danger, self.get_point
def updateState(self, centerInZeroRad=False):
if vrep.simxGetConnectionId(self.clientID) != -1:
jointPositions = self.getJointRawAngles() # np.array
jointPositions = jointPositions % (2 * np.pi
) # convert values to [0, 2*pi[
if centerInZeroRad:
newJPositions = [
angle if angle <= np.pi else angle - 2 * np.pi
for angle in jointPositions
] # convert values to ]-pi, +pi]
newJPositions = np.array(
newJPositions) + np.pi # convert values to ]0, +2*pi]
else:
# center in pi
newJPositions = np.array(jointPositions)
# newJPositions2 = newJPositions / np.pi # previous version (mistake), convert to ]0, 2]
newJPositions2 = newJPositions / (2 * np.pi) # convert to ]0, 1]
newJPositions3 = newJPositions2.reshape(6)
# select the first relevant joints
self.state = newJPositions3[0:self.nSJoints]
returnCode, self.goalCubeRelPos = vrep.simxGetObjectPosition(
self.clientID, self.goalCubeH, self.robotBaseH,
vrep.simx_opmode_blocking)
returnCode, self.endEffectorRelPos = vrep.simxGetObjectPosition(
self.clientID, self.endEffectorH, self.robotBaseH,
vrep.simx_opmode_blocking)
if self.ignore_cube_z:
self.goalCubeRelPos = (self.goalCubeRelPos[0],
self.goalCubeRelPos[1])
self.state = np.concatenate((self.state, self.endEffectorRelPos))
self.state = np.concatenate(
(self.state,
self.goalCubeRelPos)) # (x,y,z), z doesnt change in the plane
returnCode, self.distanceToCube = vrep.simxReadDistance(
self.clientID, self.distToGoalHandle,
vrep.simx_opmode_blocking) # dist in metres
if self.task == self.TASK_REACH_CUBE:
self.reward = self.distance2reward(self.distanceToCube,
self.rewards_decay_rate)
if self.distanceToCube < self.minDistance:
self.goalReached = True
print('[ROBOTENV] #### GOAL STATE ####')
elif self.task == self.TASK_PUSH_CUBE_TO_TARGET_POSITION:
self.distanceCubeTargetPos = np.sqrt(
(self.targetCubePosition[0] - self.goalCubeRelPos[0])**2 +
(self.targetCubePosition[1] - self.goalCubeRelPos[1])**2)
self.reward = self.distance2reward(
self.distanceToCube,
self.rewards_decay_rate) + self.distance2reward(
self.distanceCubeTargetPos,
3 * self.rewards_decay_rate)
if self.distanceCubeTargetPos < self.minDistance:
self.goalReached = True
print('[ROBOTENV] #### SUCCESS ####')
else:
raise RuntimeError('[ROBOTENV] Invalid Task')
# 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)
targetPos2 = np.array([90, 135, 225, 180, 180, 350])
targetPos2 = degrees2Radians(targetPos2)
def reset(self):
# self.lastCmdTime = vrep.simxGetLastCmdTime(self.clientID) # 记录当前时间 单位：秒
vrep.simxSynchronousTrigger(self.clientID) # 让仿真走一步
vrep.simxGetPingTime(self.clientID)
# self.currCmdTime = vrep.simxGetLastCmdTime(self.clientID)
vrep.simxPauseCommunication(self.clientID, True)
vrep.simxSetJointTargetPosition(self.clientID,
self.robot1_jointHandle[0], 0,
vrep.simx_opmode_oneshot)
vrep.simxSetJointTargetPosition(self.clientID,
self.robot1_jointHandle[1], 0,
vrep.simx_opmode_oneshot)
vrep.simxSetJointTargetPosition(self.clientID,
self.robot1_jointHandle[2], 0,
vrep.simx_opmode_oneshot)
vrep.simxPauseCommunication(self.clientID, False)
# self.lastCmdTime = self.currCmdTime
vrep.simxSynchronousTrigger(self.clientID)
vrep.simxGetPingTime(self.clientID)
self.JointPosition = np.zeros(3, dtype=float)
for i in range(self.jointNum):
_, self.JointPosition[i] = vrep.simxGetJointPosition(
self.clientID, self.robot1_jointHandle[i],
vrep.simx_opmode_oneshot)
while abs(self.JointPosition[0]) > 1e-4 or abs(
self.JointPosition[1]) > 1e-4 or abs(
self.JointPosition[2]) > 1e-4:
vrep.simxSynchronousTrigger(self.clientID)
vrep.simxGetPingTime(self.clientID)
for i in range(self.jointNum):
_, self.JointPosition[i] = vrep.simxGetJointPosition(
self.clientID, self.robot1_jointHandle[i],
vrep.simx_opmode_oneshot)
# print("resetting...")
# print("ready...")
# 获取转移状态
s = np.zeros(3, dtype=float)
for i in range(self.jointNum):
_, s[i] = vrep.simxGetJointPosition(self.clientID,
self.robot1_jointHandle[i],
vrep.simx_opmode_oneshot)
_, pos = vrep.simxGetObjectPosition(self.clientID, self.end_handle,
self.goal_handle,
vrep.simx_opmode_oneshot)
del pos[1]
pos = np.array(pos)
s = np.hstack(
(np.exp(s) / sum(np.exp(s)), np.exp(pos) / sum(np.exp(pos))))
linkPos = []
for i in range(self.jointNum):
_, linkpos = vrep.simxGetObjectPosition(
self.clientID, self.link_handle[i], self.goal_handle,
vrep.simx_opmode_oneshot) # link1 位置
del linkpos[1]
linkPos += linkpos
linkPos = np.array(linkPos)
s = np.hstack((s, np.exp(linkPos) / sum(np.exp(linkPos))))
self.get_point = False
self.grab_counter = 0
_, self.dis = vrep.simxReadDistance(self.clientID, self.end_dis_handle,
vrep.simx_opmode_oneshot)
self.action = np.zeros(3, dtype=float)
return s
