def __init__(self, name):
self.__name = self.NAME[name%2]
self.__obname = self.NAME[name%2 + 1]
self.viewer = None
high = np.array([1.,1.,1.,1.,1.,1.,1.,1., #8
1.,1.,1.,1.,1.,1.,1., #7
0.,0.,0.,0.,0.,0., #6
0.,0.,0.,0.,0.,0.,0.,0.,0.,#9
0.,0.,0., #2
0.,0.,0.]) #1
#24
low = -1*high
# ox,oy,oz,oa,ob,oc,od,of,
# vx,vy,vz,va,vb,vc,vd,vf
# dis of Link(15)
#
# joint_angle(7),
# limit(1), rate(3)
self.observation_space = spaces.Box(low=low, high=high, dtype=np.float32)
self.action_space = spaces.Discrete(3)
self.act_dim=7
self.obs_dim=39
self.state = []
self.action = []
self.cmd = []
self.point = []
self.goal = []
self.goal_pos = []
self.goal_quat = []
self.goal_phi = 0
self.goal_rpy = []
self.old = []
self.old_pos = []
self.old_quat = []
self.old_phi = 0
self.joint_pos = []
self.joint_angle = []
self.limit = []
self.s_jointpos = []
# self.dis_pos
self.cc = CheckCollision()
self.collision = False
self.range_cnt = 0.8
self.rpy_range = 0.1
self.s_cnt = 0
self.done = True
self.goal_err = 0.08
self.ori_err = 0.24
self.quat_inv = False
self.set_mode_pub = rospy.Publisher(
'/gazebo/set_model_state',
ModelState,
queue_size=1,
latch=True
)
self.seed()
self.reset()
def __init__(self, name):
self.__name = self.NAME[name%2]
self.__obname = self.NAME[name%2 + 1]
self.viewer = None
high = np.array([1.,1.,1.,1.,1.,1.,1.,1., #8
1.,1.,1.,1.,1.,1.,1.,1., #8
0.,0.,0.,0.,0.,0., #6
0.,0.,0.,0.,0.,0.,0.,0.,0.,#9
0.,0.,0.,0.,0.,0.,0., #7
0.]) #1
#42
low = -high
# ox,oy,oz,oa,ob,oc,od,of,
# vx,vy,vz,va,vb,vc,vd,vf
# dis of Link(15)
#
# joint_angle(7),
# limit(1), rate(3)
self.observation_space = spaces.Box(low=low, high=high, dtype=np.float32)
self.action_space = spaces.Discrete(8)
self.state = []
self.action = []
self.cmd = []
self.point = []
self.goal = []
self.goal_pos = []
self.goal_quat = []
self.goal_phi = 0
self.old = []
self.old_pos = []
self.old_quat = []
self.old_phi = 0
self.joint_pos = []
self.joint_angle = []
self.limit = []
self.s_jointpos = []
# self.dis_pos
self.cc = CheckCollision()
self.collision = False
self.range_cnt = 0.1
self.s_cnt = 0
self.seed()
self.reset()
self.done = False
class Test(core.Env):
ACTION_VEC_TRANS = 1 / 180
ACTION_ORI_TRANS = 1 / 60
ACTION_PHI_TRANS = 1 / 60
NAME = ['/right_', '/left_', '/right_']
def __init__(self, name, workers):
self.__name = self.NAME[name % 2]
self.__obname = self.NAME[name % 2 + 1]
if workers == 0:
self.workers = 'arm'
else:
self.workers = str(workers)
high = np.array([
1.,
1.,
1.,
1.,
1.,
1.,
1.,
1., #8
1.,
1.,
1.,
1.,
1.,
1.,
1.,
1.,
1.,
1.,
1., #7
0.,
0.,
0.,
0.,
0.,
0., #6
0.,
0.,
0.,
0.,
0.,
0.,
0.,
0.,
0., #9
0.,
0.,
0.,
0.,
0.,
0.,
0.,
0.,
0., #2
0.,
0.,
0.,
0.,
0.,
0.
]) #1
#24
low = -1 * high
# ox,oy,oz,oa,ob,oc,od,of,
# vx,vy,vz,va,vb,vc,vd,vf
# dis of Link(15)
#
# joint_angle(7),
# limit(1), rate(3)
self.observation_space = spaces.Box(low=low,
high=high,
dtype=np.float32)
self.action_space = spaces.Discrete(3)
self.act_dim = 8
self.obs_dim = 61
self.state = []
self.action = []
self.cmd = []
self.point = []
self.goal = []
self.goal_pos = []
self.goal_quat = []
self.goal_phi = 0
self.goal_rpy = []
self.old = []
self.old_pos = []
self.old_quat = []
self.old_phi = 0
self.joint_pos = []
self.joint_angle = []
self.limit = []
self.s_jointpos = []
# self.dis_pos
self.cc = CheckCollision()
self.collision = False
self.range_cnt = 0.6
self.rpy_range = 0.2 * (workers + 1)
self.done = True
self.s_cnt = 0
self.goal_err = 0.08
self.ori_err = 0.4
self.quat_inv = False
self.goal_angle = []
self.set_mode_pub = rospy.Publisher('/gazebo/set_model_state',
ModelState,
queue_size=1,
latch=True)
self.seed()
self.reset()
@property
def is_success(self):
return self.done
@property
def success_cnt(self):
return self.s_cnt
def get_state_client(self, name):
service = name + self.workers + '/get_state'
try:
rospy.wait_for_service(service, timeout=1.)
except rospy.ROSException as e:
rospy.logwarn('wait_for_service timeout')
self.get_state_client(name)
client = rospy.ServiceProxy(service, get_state)
# res = client(cmd)
res = client.call()
return res
def move_cmd_client(self, cmd, name):
service = name + self.workers + '/move_cmd'
try:
rospy.wait_for_service(service, timeout=1.)
except rospy.ROSException as e:
rospy.logwarn('wait_for_service timeout')
self.move_cmd_client(cmd, name)
client = rospy.ServiceProxy(service, move_cmd)
# res = client(cmd)
res = client.call(cmd)
return res
def set_start_client(self, cmd, rpy, name):
service = name + self.workers + '/set_start'
try:
rospy.wait_for_service(service, timeout=1.)
except rospy.ROSException as e:
rospy.logwarn('wait_for_service timeout')
self.set_start_client(cmd, rpy, name)
client = rospy.ServiceProxy(service, set_start)
# res = client(cmd)
res = client(action=cmd, rpy=rpy)
return res
def set_goal_client(self, cmd, rpy, name):
service = name + self.workers + '/set_goal'
try:
rospy.wait_for_service(service, timeout=1.)
except rospy.ROSException as e:
rospy.logwarn('wait_for_service timeout')
self.set_goal_client(cmd, rpy, name)
client = rospy.ServiceProxy(service, set_goal)
# res = client(cmd)
res = client(action=cmd, rpy=rpy)
return res
def set_object(self, name, pos, ori):
msg = ModelState()
msg.model_name = name + self.workers
msg.pose.position.x = pos[0]
msg.pose.position.y = pos[1]
msg.pose.position.z = pos[2]
msg.pose.orientation.w = ori[0]
msg.pose.orientation.x = ori[1]
msg.pose.orientation.y = ori[2]
msg.pose.orientation.z = ori[3]
msg.reference_frame = 'world'
self.set_mode_pub.publish(msg)
def seed(self, seed=None):
self.np_random, seed = seeding.np_random(seed)
return [seed]
def quat_angle(self):
cos_q = np.dot(self.goal[3:7], self.old[3:7])
if (self.quat_inv and cos_q > 0) or ((not self.quat_inv)
and cos_q <= 0):
cos_q = np.dot(-1 * self.goal[3:7], self.old[3:7])
return math.acos(cos_q) / pi
def move(self, goal):
self.goal = goal
res = self.get_state_client(self.__name)
res_ = self.get_state_client(self.__obname)
self.old, self.joint_pos[:15], self.joint_angle = np.array(
res.state), res.joint_pos, res.joint_angle
self.limit, self.goal_quat, self.quat_inv, self.joint_pos[
15:30] = res.limit, res.quaterniond, res.quat_inv, res_.joint_pos
linkPosM, linkPosS = self.collision_init()
_, Link_dis = self.cc.checkCollision(linkPosM, linkPosS)
s = np.append(self.old[:3], np.subtract(self.goal[:3], self.old[:3]))
s = np.append(s, Link_dis)
s = np.append(s, self.joint_angle)
s = np.append(s, self.limit[0])
self.dis_pos = np.linalg.norm(self.goal[:3] - s[:3])
s = np.append(s, self.dis_pos)
self.state = s
return s
def reset(self):
self.goal, self.goal_angle = self.set_goal()
self.old, self.joint_pos[:15], self.joint_pos[
15:
30], self.joint_angle, self.limit, self.goal_quat, self.quat_inv = self.set_old(
)
linkPosM, linkPosS = self.collision_init()
alarm, Link_dis = self.cc.checkCollision(linkPosM, linkPosS)
alarm_cnt = 0
for i in alarm:
alarm_cnt += i
if alarm_cnt > 0:
return self.reset()
self.state = np.append(self.old,
np.subtract(self.goal[:3], self.old[:3]))
# self.state = np.append(self.state, self.goal_quat)
self.state = np.append(self.state,
np.subtract(self.goal[3:7], self.old[3:7]))
self.state = np.append(self.state,
np.subtract(-1 * self.goal[3:7], self.old[3:7]))
self.state = np.append(self.state, Link_dis)
self.state = np.append(self.state, self.joint_angle)
self.state = np.append(self.state, self.limit)
self.dis_pos = np.linalg.norm(self.goal[:3] - self.old[:3])
self.dis_ori = math.sqrt(
np.linalg.norm(self.goal[3:7] - self.old[3:7]) +
np.linalg.norm(-1 * self.goal[3:7] - self.old[3:7]) - 2)
# self.angle_ori = self.quat_angle()
self.state = np.append(self.state, self.dis_pos)
self.state = np.append(self.state, self.dis_ori)
self.state = np.append(self.state, self.joint_pos[6:12])
self.state = np.append(self.state, self.goal_angle)
self.collision = False
self.done = False
return self.state
def set_goal(self):
self.goal = self.np_random.uniform(low=0.,
high=self.range_cnt,
size=(8, ))
rpy = self.np_random.uniform(low=-1 * self.rpy_range,
high=self.rpy_range,
size=(3, ))
# print('self.goal = ', self.goal)
self.goal[0] = 0
# self.goal[3] = self.range_cnt/2
self.goal = np.append(self.goal, self.range_cnt)
res = self.set_goal_client(self.goal, rpy, self.__name)
goal_pos = np.array(res.state)
if not res.success:
return self.set_goal()
if np.linalg.norm(goal_pos[:2]) > 0.2:
return goal_pos[:7], res.joint_angle
else:
return self.set_goal()
def set_old(self):
self.start = self.np_random.uniform(low=0.,
high=self.range_cnt,
size=(8, ))
rpy = self.np_random.uniform(low=-1 * self.rpy_range,
high=self.rpy_range,
size=(3, ))
self.start[0] = 0
# self.start[3] = self.range_cnt/2
self.start = np.append(self.start, self.range_cnt)
res = self.set_start_client(self.start, rpy, self.__name)
res_ = self.get_state_client(self.__obname)
old_pos = np.array(res.state)
if not res.success:
return self.set_old()
if np.linalg.norm(np.subtract(old_pos[:3], self.goal[:3])) > 0.1:
return old_pos, res.joint_pos, res_.joint_pos, res.joint_angle, res.limit, res.quaterniond, res.quat_inv
else:
return self.set_old()
def collision_init(self):
linkPosM = np.array(self.joint_pos[:15])
linkPosS = np.array(self.joint_pos[15:])
linkPosM = np.append([0., 0., -0.8], linkPosM)
linkPosS = np.append([0., 0., -0.8], linkPosS)
linkPosM = linkPosM.reshape(6, 3)
linkPosS = linkPosS.reshape(6, 3)
return linkPosM, linkPosS
def step(self, a):
alarm = []
Link_dis = []
s = self.state
self.collision = False
action_vec = a[:3] * self.ACTION_VEC_TRANS
action_ori = a[3:7] * self.ACTION_ORI_TRANS
action_phi = a[7] * self.ACTION_PHI_TRANS
self.action = np.append(action_vec, action_ori)
self.action = np.append(self.action, action_phi)
self.cmd = np.add(s[:8], self.action)
self.cmd[3:7] /= np.linalg.norm(self.cmd[3:7])
res = self.move_cmd_client(self.cmd, self.__name)
res_ = self.get_state_client(self.__obname)
if res.success:
self.old, self.joint_pos[:15], self.joint_angle = np.array(
res.state), res.joint_pos, res.joint_angle
self.limit, self.goal_quat, self.quat_inv, self.joint_pos[
15:
30] = res.limit, res.quaterniond, res.quat_inv, res_.joint_pos
linkPosM, linkPosS = self.collision_init()
alarm, Link_dis = self.cc.checkCollision(linkPosM, linkPosS)
s = np.append(self.old, np.subtract(self.goal[:3], self.old[:3]))
# s = np.append(s, self.goal_quat)\
s = np.append(s, np.subtract(self.goal[3:7], self.old[3:7]))
s = np.append(s, np.subtract(-1 * self.goal[3:7], self.old[3:7]))
s = np.append(s, Link_dis)
s = np.append(s, self.joint_angle)
s = np.append(s, self.limit)
self.dis_pos = np.linalg.norm(self.goal[:3] - s[:3])
self.dis_ori = math.sqrt(
np.linalg.norm(self.goal[3:7] - s[3:7]) +
np.linalg.norm(-1 * self.goal[3:7] - s[3:7]) - 2)
# self.angle_ori = self.quat_angle()
s = np.append(s, self.dis_pos)
s = np.append(s, self.dis_ori)
s = np.append(s, self.joint_pos[6:12])
s = np.append(s, self.goal_angle)
terminal = self._terminal(s, res.success, alarm)
reward = self.get_reward(s, res.success, terminal)
if (not self.collision) and math.fabs(s[7]) < 0.9:
self.state = s
if self.workers == 'arm':
self.set_object(
self.__name,
(self.goal[0] - 0.08, self.goal[1], self.goal[2] + 1.45086),
(0, 0, 0, 0))
return self.state, reward, terminal, self.collision
def _terminal(self, s, ik_success, alarm):
alarm_cnt = 0
for i in alarm:
alarm_cnt += i
if alarm_cnt > 0.4:
self.collision = True
if ik_success and not self.collision:
if self.dis_pos < self.goal_err and self.dis_ori < self.ori_err:
if not self.done:
self.done = True
self.s_cnt += 1
self.range_cnt = self.range_cnt + 0.01 if self.range_cnt < 0.95 else 0.95
self.rpy_range = self.rpy_range + 0.004 if self.rpy_range < 0.8 else 0.8
self.goal_err = self.goal_err * 0.993 if self.goal_err > 0.015 else 0.015
self.ori_err = self.ori_err * 0.993 if self.ori_err > 0.2 else 0.2
return True
else:
return False
else:
return False
def get_reward(self, s, ik_success, terminal):
reward = 0.
if not ik_success:
return -4
if self.collision:
return -5
if math.fabs(s[7]) > 0.9:
return -3
reward -= self.dis_pos
reward -= self.dis_ori
reward += 0.4
if reward > 0:
reward *= 2
cos_vec = np.dot(self.action[:3], self.state[8:11]) / (
np.linalg.norm(self.action[:3]) * np.linalg.norm(self.state[8:11]))
reward += (cos_vec * self.dis_pos - self.dis_pos)
reward -= 1.8
return reward
def __init__(self, name, workers):
self.__name = self.NAME[name % 2]
self.__obname = self.NAME[name % 2 + 1]
if workers == 0:
self.workers = 'arm'
else:
self.workers = str(workers)
high = np.array([
1.,
1.,
1.,
1.,
1.,
1.,
1.,
1., #8
1.,
1.,
1.,
1.,
1.,
1.,
1.,
1.,
1.,
1.,
1., #7
0.,
0.,
0.,
0.,
0.,
0., #6
0.,
0.,
0.,
0.,
0.,
0.,
0.,
0.,
0., #9
0.,
0.,
0.,
0.,
0.,
0.,
0.,
0.,
0., #2
0.,
0.,
0.,
0.,
0.,
0.
]) #1
#24
low = -1 * high
# ox,oy,oz,oa,ob,oc,od,of,
# vx,vy,vz,va,vb,vc,vd,vf
# dis of Link(15)
#
# joint_angle(7),
# limit(1), rate(3)
gym.logger.set_level(40)
self.observation_space = spaces.Box(low=low,
high=high,
dtype=np.float32)
self.action_space = spaces.Discrete(3)
self.act_dim = 8
self.obs_dim = 57 + 24 + 2
self.state = []
self.action = []
self.cmd = []
self.point = []
self.goal = []
self.goal_pos = []
self.goal_quat = []
self.goal_phi = 0
self.goal_rpy = []
self.old = []
self.old_pos = []
self.old_quat = []
self.old_phi = 0
self.joint_pos = []
self.joint_angle = []
self.limit = []
self.s_jointpos = []
# self.dis_pos
self.cc = CheckCollision()
self.collision = False
self.range_cnt = 0.6 #+(workers*0.1)
self.rpy_range = 0.2 * (workers + 1)
self.done = True
self.s_cnt = 0
self.goal_err = 0.06
self.ori_err = 0.3
# self.ori_err = 0.15
self.quat_inv = False
self.goal_angle = []
self.object_pub = 0
self.set_model_pub = rospy.Publisher('/gazebo/set_model_state',
ModelState,
queue_size=1,
latch=True)
self.set_mode_pub = rospy.Publisher(self.__name + self.workers +
'/set_mode_msg',
String,
queue_size=1,
latch=True)
self.aa_box_possition = []
self.bridge = CvBridge()
self.depth_dim = 768
self.cv_depth = None
self.depth_image = None
self.image_input = None
self.__bumper = None
self.__robot = "_arm"
self.images_ = []
self.aa_box_x = 0.58
self.aa_box_y = 0.
self.dis_obstacle_1 = []
self.dis_obstacle_2 = []
self.aa_box_pos_x = 0.
self.aa_box_pos_y = 0.
self.aa_box_pos_z = 0.
self.set_aabox_pub = rospy.Publisher('aa_box_pos',
aa_box_pos,
queue_size=1,
latch=True)
self.set_aabox_vel = rospy.Publisher('cmd_vel',
Accel,
queue_size=1,
latch=True)
# self.set_mode_pub.publish('set_mode')
self.seed(345 * (workers + 1) + 467 * (name + 1))
self.reset()
## image (gazebo)
rospy.Subscriber('/gazebo/model_states', ModelStates, self.callback)
class Test(core.Env):
ACTION_VEC_TRANS = 1 / 180
ACTION_ORI_TRANS = 1 / 60
ACTION_PHI_TRANS = 1 / 60
NAME = ['/right_', '/left_', '/right_']
def __init__(self, name, workers):
self.__name = self.NAME[name % 2]
self.__obname = self.NAME[name % 2 + 1]
if workers == 0:
self.workers = 'arm'
else:
self.workers = str(workers)
high = np.array([
1.,
1.,
1.,
1.,
1.,
1.,
1.,
1., #8
1.,
1.,
1.,
1.,
1.,
1.,
1.,
1.,
1.,
1.,
1., #7
0.,
0.,
0.,
0.,
0.,
0., #6
0.,
0.,
0.,
0.,
0.,
0.,
0.,
0.,
0., #9
0.,
0.,
0.,
0.,
0.,
0.,
0.,
0.,
0., #2
0.,
0.,
0.,
0.,
0.,
0.
]) #1
#24
low = -1 * high
# ox,oy,oz,oa,ob,oc,od,of,
# vx,vy,vz,va,vb,vc,vd,vf
# dis of Link(15)
#
# joint_angle(7),
# limit(1), rate(3)
gym.logger.set_level(40)
self.observation_space = spaces.Box(low=low,
high=high,
dtype=np.float32)
self.action_space = spaces.Discrete(3)
self.act_dim = 8
self.obs_dim = 57 + 24 + 2
self.state = []
self.action = []
self.cmd = []
self.point = []
self.goal = []
self.goal_pos = []
self.goal_quat = []
self.goal_phi = 0
self.goal_rpy = []
self.old = []
self.old_pos = []
self.old_quat = []
self.old_phi = 0
self.joint_pos = []
self.joint_angle = []
self.limit = []
self.s_jointpos = []
# self.dis_pos
self.cc = CheckCollision()
self.collision = False
self.range_cnt = 0.6 #+(workers*0.1)
self.rpy_range = 0.2 * (workers + 1)
self.done = True
self.s_cnt = 0
self.goal_err = 0.06
self.ori_err = 0.3
# self.ori_err = 0.15
self.quat_inv = False
self.goal_angle = []
self.object_pub = 0
self.set_model_pub = rospy.Publisher('/gazebo/set_model_state',
ModelState,
queue_size=1,
latch=True)
self.set_mode_pub = rospy.Publisher(self.__name + self.workers +
'/set_mode_msg',
String,
queue_size=1,
latch=True)
self.aa_box_possition = []
self.bridge = CvBridge()
self.depth_dim = 768
self.cv_depth = None
self.depth_image = None
self.image_input = None
self.__bumper = None
self.__robot = "_arm"
self.images_ = []
self.aa_box_x = 0.58
self.aa_box_y = 0.
self.dis_obstacle_1 = []
self.dis_obstacle_2 = []
self.aa_box_pos_x = 0.
self.aa_box_pos_y = 0.
self.aa_box_pos_z = 0.
self.set_aabox_pub = rospy.Publisher('aa_box_pos',
aa_box_pos,
queue_size=1,
latch=True)
self.set_aabox_vel = rospy.Publisher('cmd_vel',
Accel,
queue_size=1,
latch=True)
# self.set_mode_pub.publish('set_mode')
self.seed(345 * (workers + 1) + 467 * (name + 1))
self.reset()
## image (gazebo)
rospy.Subscriber('/gazebo/model_states', ModelStates, self.callback)
# rospy.Subscriber("odom", Odometry,self.get_aa_box_position)
# rospy.Subscriber("/bumper",ContactsState,self.Sub_Bumper)
@property
def is_success(self):
return self.done
@property
def success_cnt(self):
return self.s_cnt
# def set_object(self, name, pos, ori):
# msg = ModelState()
# msg.model_name = name
# msg.pose.position.x = pos[0]
# msg.pose.position.y = pos[1]
# msg.pose.position.z = pos[2]
# msg.pose.orientation.w = ori[0]
# msg.pose.orientation.x = ori[1]
# msg.pose.orientation.y = ori[2]
# msg.pose.orientation.z = ori[3]
# msg.reference_frame = 'world'
# self.set_model_pub.publish(msg)
def aa_suck(self, data):
self.aa_box_x = data.x
self.aa_box_y = data.y
## image (gazebo)
def callback(self, data):
try:
self.aa_box_pos_x = data.pose[6].position.x
self.aa_box_pos_y = data.pose[6].position.y
self.aa_box_pos_z = data.pose[6].position.z
# depth_array = self.bridge.imgmsg_to_cv2(data,"32FC1")
# # depth_array = np.array(tmp, dtype=np.float32)
# depth_array = cv2.resize(depth_array , (160,120))
# print('f**k',depth_array[0][0])
# cv2.normalize(depth_array, depth_array, 0, 1, cv2.NORM_MINMAX)
# print('f**k', tmp[100])
# cv_image_array = np.array(tmp, dtype = np.dtype('f8'))
# cv_image_norm = cv2.normalize(cv_image_array, cv_image_array, 0, 1, cv2.NORM_MINMAX)
# cv_image_resized = cv2.resize(cv_image_norm, self.desired_shape, interpolation = cv2.INTER_CUBIC)
# # self.images_ =
# where_are_nan = np.isnan(depth_array)
# where_are_inf = np.isinf(depth_array)
# depth_array[where_are_nan] = 10
# depth_array[where_are_inf] = 10
# # self.images_ = cv2.resize(tmp , (640,480))
# # print(self.images_)
# self.images_ = depth_array / 10
# print(self.images_[0][0])
# cv2.imshow('My Image', self.images_)
# cv2.waitKey(1)
# cv2.imshow("Image from my node", self.depthimg)
# cv2.waitKey(1)
# tmp = cv2.applyColorMap(cv2.convertScaleAbs(tmp, alpha=0.03), cv2.COLORMAP_JET)
# print(self.depth_image[240,240])
# print("WTFFFFFFFFFFFFFFFFFFFff")
# self.image_input = tmp
#
# self.__image= int.from_bytes(data.data, byteorder='big', signed=False)
# print(tmp)
# print(data)
# img = cv2.imread()
# cv2.imshow('My Image', self.images_)
# cv2.imwrite('output.jpg', self.images_)
# cv2.waitKey(0)
except CvBridgeError as e:
print(e)
# # cv2.destroyAllWindows()
# def image_client(self):
# # rospy.init_node('depth_image',anonymous=True)
# print("aaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaa")
# rospy.Subscriber('ir_depth/depth/image_raw',Image,self.callback )
# rospy.Subscriber('/camera/rgb/image_raw',Image,self.callback )
# rospy.spin()
##bumper gazebo
### model state
def Gazebo_aa_box_state(self):
rospy.Subscriber("/gazebo/model_states", ModelState,
self.gazebo_state_information)
print(self.gazebo_state_information.name)
# CNN
def build_cnnlayer(self, conv_in, net_name):
# build conv layer
conv_out = None
for key in sorted(cfg[net_name]):
com = cfg[net_name][key] #component
if com['type'] in 'conv':
stride = com['stride']
conv_out = Conv2D(conv_in,
com['kernel_size'],
com['out_channel'],
name_prefix=net_name + '_' + key,
strides=[1, stride, stride, 1])
conv_in = conv_out
if 'spatial_softmax' in com:
self.logger.debug('Last_conv.shape = {}'.format(
conv_in.shape))
conv_out = tf.contrib.layers.spatial_softmax(
conv_in, name='spatial_softmax')
# print(conv_out.shape)
# if don't have spatial softmax need to flatten
if len(conv_out.shape) > 2:
conv_out = Flaten(conv_out)
return conv_out
# def _save_img(self, img_buffer, img_):
# def get_aa_box_position(pose)
# print(pose)
def get_state_client(self, name):
service = name + self.workers + '/get_state'
try:
rospy.wait_for_service(service, timeout=1.)
except rospy.ROSException as e:
rospy.logwarn('wait_for_service timeout')
self.get_state_client(name)
client = rospy.ServiceProxy(service, get_state)
# res = client(cmd)
res = client.call()
return res
def move_cmd_client(self, cmd, name):
service = name + self.workers + '/move_cmd'
try:
rospy.wait_for_service(service, timeout=1.)
except rospy.ROSException as e:
rospy.logwarn('wait_for_service timeout')
self.move_cmd_client(cmd, name)
client = rospy.ServiceProxy(service, move_cmd)
# res = client(cmd)
res = client.call(cmd)
return res
def set_start_client(self, cmd, rpy, name):
service = name + self.workers + '/set_start'
try:
rospy.wait_for_service(service, timeout=1.)
except rospy.ROSException as e:
rospy.logwarn('wait_for_service timeout')
self.set_start_client(cmd, rpy, name)
client = rospy.ServiceProxy(service, set_start)
# res = client(cmd)
res = client(action=cmd, rpy=rpy)
return res
def set_goal_client(self, cmd, rpy, name):
service = name + self.workers + '/set_goal'
try:
rospy.wait_for_service(service, timeout=1.)
except rospy.ROSException as e:
rospy.logwarn('wait_for_service timeout')
self.set_goal_client(cmd, rpy, name)
client = rospy.ServiceProxy(service, set_goal)
# res = client(cmd)
res = client(action=cmd, rpy=rpy)
return res
def set_object(self, name, pos, ori):
msg = ModelState()
msg.model_name = name
msg.pose.position.x = pos[0]
msg.pose.position.y = pos[1]
msg.pose.position.z = pos[2]
msg.pose.orientation.w = ori[0]
msg.pose.orientation.x = ori[1]
msg.pose.orientation.y = ori[2]
msg.pose.orientation.z = ori[3]
msg.reference_frame = 'world'
self.set_model_pub.publish(msg)
def set_object2(self, name, pos, ori):
msg = ModelState()
msg.model_name = name + self.workers
msg.pose.position.x = pos[0]
msg.pose.position.y = pos[1]
msg.pose.position.z = pos[2]
msg.pose.orientation.w = ori[0]
msg.pose.orientation.x = ori[1]
msg.pose.orientation.y = ori[2]
msg.pose.orientation.z = ori[3]
msg.reference_frame = 'world'
self.set_model_pub.publish(msg)
def set_aa_vel(self, vel):
msg = Accel()
msg.linear.x = vel[0]
msg.linear.y = vel[1]
self.set_aabox_vel.publish(msg)
def seed(self, seed=None):
self.np_random, seed = seeding.np_random(seed)
return [seed]
def quat_angle(self):
cos_q = np.dot(self.goal[3:7], self.old[3:7])
if (self.quat_inv and cos_q > 0) or ((not self.quat_inv)
and cos_q <= 0):
cos_q = np.dot(-1 * self.goal[3:7], self.old[3:7])
return math.acos(cos_q) / pi
def move(self, goal):
self.goal = goal
res = self.get_state_client(self.__name)
res_ = self.get_state_client(self.__obname)
self.old, self.joint_pos[:15], self.joint_angle = np.array(
res.state), res.joint_pos, res.joint_angle
self.limit, self.goal_quat, self.quat_inv, self.joint_pos[
15:30] = res.limit, res.quaterniond, res.quat_inv, res_.joint_pos
linkPosM, linkPosS = self.collision_init()
_, Link_dis = self.cc.checkCollision(linkPosM, linkPosS)
s = np.append(self.old[:3], np.subtract(self.goal[:3], self.old[:3]))
s = np.append(s, Link_dis)
s = np.append(s, self.joint_angle)
s = np.append(s, self.limit[0])
self.dis_pos = np.linalg.norm(self.goal[:3] - s[:3])
s = np.append(s, self.dis_pos)
self.state = s
return s
def reset(self):
self.old, self.joint_pos[:15], self.joint_pos[
15:
30], self.joint_angle, self.limit, self.goal_quat, self.quat_inv = self.set_old(
)
linkPosM, linkPosS = self.collision_init()
alarm, Link_dis = self.cc.checkCollision(linkPosM, linkPosS)
alarm_cnt = 0
for i in alarm:
alarm_cnt += i
if alarm_cnt > 0:
return self.reset()
joint_pos_tmp = self.joint_pos
self.goal, self.goal_angle, self.joint_pos[:15], self.joint_pos[
15:30] = self.set_goal()
linkPosM, linkPosS = self.collision_init()
alarm, Link_dis = self.cc.checkCollision(linkPosM, linkPosS)
alarm_cnt = 0
for i in alarm:
alarm_cnt += i
if alarm_cnt > 0:
return self.reset()
self.joint_pos = joint_pos_tmp
self.state = np.append(self.old,
np.subtract(self.goal[:3], self.old[:3]))
self.state = np.append(self.state, self.goal_quat[:4])
# self.state = np.append(self.state, np.subtract(self.goal[3:7], self.old[3:7]))
# self.state = np.append(self.state, np.subtract(-1*self.goal[3:7], self.old[3:7]))
self.state = np.append(self.state, Link_dis)
self.state = np.append(self.state, self.joint_angle)
self.state = np.append(self.state, self.limit)
self.dis_pos = np.linalg.norm(self.goal[:3] - self.old[:3])
self.dis_ori = math.sqrt(
np.linalg.norm(self.goal[3:7] - self.old[3:7]) +
np.linalg.norm(-1 * self.goal[3:7] - self.old[3:7]) - 2)
# self.angle_ori = self.quat_angle()
self.state = np.append(self.state, self.dis_pos)
self.state = np.append(self.state, self.dis_ori)
self.state = np.append(self.state, self.joint_pos[6:12])
self.state = np.append(self.state, self.goal_angle)
# print("aaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaa")
# print(self.images_)
# self.img_suckkkkkkkkkkkkk = np.reshape(self.images_,-1)
# print("aaa")
# print(self.img_suckkkkkkkkkkkkk.shape)
# print("bbb")
# self.state = np.append(self.state, self.img_suckkkkkkkkkkkkk)
if self.__name == '/right_':
self.aa_box_x = random.uniform(0.1, 0.3)
self.aa_box_y = random.uniform(-0.5, 0.5)
self.set_object('table_box', (0.55, 0, 0.345), (0, 0, 0, 0))
self.set_object('aa_box', (self.aa_box_x, self.aa_box_y, 0.974718),
(0, 0, 0, 0))
aa_box = aa_box_pos()
aa_box.x = self.aa_box_x
# aa_box.x = 0.3
aa_box.y = self.aa_box_y
self.set_aabox_pub.publish(aa_box)
# rospy.Publisher('aa_box_pos',aa_box_pos)
# print(self.aa_box_x,"III",self.aa_box_x,self.aa_box_y)
arm_x = 0.08
arm_r_y = 1
arm_l_y = -1
arm_z = -1.4125
# self.aa_box_z = self.aa_box_z + 0.125
rospy.Subscriber('aa_box_pos', aa_box_pos, self.aa_suck)
# if self.__name == '/right_':
# self.aa_box_possition = [ self.aa_box_x+0.05+arm_x, (self.aa_box_y+0.05)*arm_r_y, 0.975+arm_z + 0.125,
# self.aa_box_x+0.05+arm_x, (self.aa_box_y-0.05)*arm_r_y, 0.975+arm_z + 0.125,
# self.aa_box_x-0.05+arm_x, (self.aa_box_y+0.05)*arm_r_y, 0.975+arm_z + 0.125,
# self.aa_box_x-0.05+arm_x, (self.aa_box_y-0.05)*arm_r_y, 0.975+arm_z + 0.125,
# self.aa_box_x+0.05+arm_x, (self.aa_box_y+0.05)*arm_r_y, 0.625+arm_z,
# self.aa_box_x+0.05+arm_x, (self.aa_box_y-0.05)*arm_r_y, 0.625+arm_z,
# self.aa_box_x-0.05+arm_x, (self.aa_box_y+0.05)*arm_r_y, 0.625+arm_z,
# self.aa_box_x-0.05+arm_x, (self.aa_box_y-0.05)*arm_r_y, 0.625+arm_z]
# if self.__name == '/left_':
# self.aa_box_possition = [ self.aa_box_x+0.05+arm_x, (self.aa_box_y+0.05)*arm_l_y, 0.975+arm_z + 0.125,
# self.aa_box_x+0.05+arm_x, (self.aa_box_y-0.05)*arm_l_y, 0.975+arm_z + 0.125,
# self.aa_box_x-0.05+arm_x, (self.aa_box_y+0.05)*arm_l_y, 0.975+arm_z + 0.125,
# self.aa_box_x-0.05+arm_x, (self.aa_box_y-0.05)*arm_l_y, 0.975+arm_z + 0.125,
# self.aa_box_x+0.05+arm_x, (self.aa_box_y+0.05)*arm_l_y, 0.625+arm_z,
# self.aa_box_x+0.05+arm_x, (self.aa_box_y-0.05)*arm_l_y, 0.625+arm_z,
# self.aa_box_x-0.05+arm_x, (self.aa_box_y+0.05)*arm_l_y, 0.625+arm_z,
# self.aa_box_x-0.05+arm_x, (self.aa_box_y-0.05)*arm_l_y, 0.625+arm_z]
if self.__name == '/right_':
self.aa_box_possition = [
self.aa_box_pos_x + 0.05, (self.aa_box_pos_y + 0.05),
self.aa_box_pos_z + 0.3, self.aa_box_pos_x + 0.05,
(self.aa_box_pos_y - 0.05), self.aa_box_pos_z + 0.3,
self.aa_box_pos_x - 0.05, (self.aa_box_pos_y + 0.05),
self.aa_box_pos_z + 0.3, self.aa_box_pos_x - 0.05,
(self.aa_box_pos_y - 0.05), self.aa_box_pos_z + 0.3,
self.aa_box_pos_x + 0.05, (self.aa_box_pos_y + 0.05),
self.aa_box_pos_z - 0.3, self.aa_box_pos_x + 0.05,
(self.aa_box_pos_y - 0.05), self.aa_box_pos_z - 0.3,
self.aa_box_pos_x - 0.05, (self.aa_box_pos_y + 0.05),
self.aa_box_pos_z - 0.3, self.aa_box_pos_x - 0.05,
(self.aa_box_pos_y - 0.05), self.aa_box_pos_z - 0.3
]
if self.__name == '/left_':
self.aa_box_possition = [
self.aa_box_pos_x + 0.05, (self.aa_box_pos_y + 0.05),
self.aa_box_pos_z + 0.3, self.aa_box_pos_x + 0.05,
(self.aa_box_pos_y - 0.05), self.aa_box_pos_z + 0.3,
self.aa_box_pos_x - 0.05, (self.aa_box_pos_y + 0.05),
self.aa_box_pos_z + 0.3, self.aa_box_pos_x - 0.05,
(self.aa_box_pos_y - 0.05), self.aa_box_pos_z + 0.3,
self.aa_box_pos_x + 0.05, (self.aa_box_pos_y + 0.05),
self.aa_box_pos_z - 0.3, self.aa_box_pos_x + 0.05,
(self.aa_box_pos_y - 0.05), self.aa_box_pos_z - 0.3,
self.aa_box_pos_x - 0.05, (self.aa_box_pos_y + 0.05),
self.aa_box_pos_z - 0.3, self.aa_box_pos_x - 0.05,
(self.aa_box_pos_y - 0.05), self.aa_box_pos_z - 0.3
]
self.state = np.append(self.state, self.aa_box_possition)
######### VEL
vel = self.np_random.uniform(low=-0.15, high=0.15, size=(2, ))
vel[0] = 0
self.set_aa_vel(vel)
###########
aa_box_possssss = np.array([self.aa_box_x, self.aa_box_y, 0.8])
self.dis_obstacle_1 = np.linalg.norm(aa_box_possssss[:3] -
self.joint_pos[6:9])
self.dis_obstacle_2 = np.linalg.norm(aa_box_possssss[:3] -
self.joint_pos[9:12])
self.state = np.append(self.state, self.dis_obstacle_1)
self.state = np.append(self.state, self.dis_obstacle_2)
self.collision = False
self.done = False
self.success = False
return self.state
def set_goal(self):
self.goal = self.np_random.uniform(low=-0.5, high=0.5, size=(8, ))
self.goal[1] = self.np_random.uniform(low=0.1, high=0.5)
# self.goal[1] = 0.3
if self.__name == '/left_':
self.goal[1] = self.np_random.uniform(low=-0.5, high=-0.1)
self.start[2] = self.np_random.uniform(low=-0.3, high=0.5)
rpy = self.np_random.uniform(low=-1 * self.rpy_range,
high=self.rpy_range,
size=(4, ))
# print('self.goal = ', self.goal)
# if self.goal[0]>0.5:
# if self.goal[0]>0.75:
# self.goal[2] /= -3
# else:
# self.goal[2] /= -2
# self.goal[2]+=1
self.goal[0] = 0
# self.goal[3] = self.range_cnt/2
self.goal = np.append(self.goal, self.range_cnt)
res = self.set_goal_client(self.goal, rpy, self.__name)
res_ = self.get_state_client(self.__obname)
goal_pos = np.array(res.state)
if not res.success:
return self.set_goal()
else:
return goal_pos[:7], res.joint_angle, res.joint_pos, res_.joint_pos
def set_old(self):
self.start = self.np_random.uniform(low=-0.5, high=0.5, size=(8, ))
# self.start[1] = self.np_random.uniform(low=0.1, high=0.5)
# if self.__name == '/left_':
# self.start[1] = self.np_random.uniform(low=-0.5, high=-0.1)
# self.start[2] = self.np_random.uniform(low=-0.3, high=0.5)
rpy = self.np_random.uniform(low=-1 * self.rpy_range,
high=self.rpy_range,
size=(4, ))
# if self.start[0]>0.5:
# if self.start[0]>0.75:
# self.start[2] /= -3
# else:
# self.start[2] /= -2
# self.start[2] +=1
self.start[0] = 0
# if self.__name == '/left_':
# self.start = [0,-46.55,-26.77,0,-55.59,-65.46,-52.94,52.85]
# self.start = np.pi*(self.start)/180
# else:
# self.start = [0,46.55,-26.77,0,55.59,65.46,-52.94,-52.85]
# self.start = np.pi*(self.start)/180
# self.start[3] = self.range_cnt/2
self.start = np.append(self.start, self.range_cnt)
res = self.set_start_client(self.start, rpy, self.__name)
res_ = self.get_state_client(self.__obname)
old_pos = np.array(res.state)
if not res.success:
return self.set_old()
else:
return old_pos, res.joint_pos, res_.joint_pos, res.joint_angle, res.limit, res.quaterniond, res.quat_inv
def collision_init(self):
linkPosM = np.array(self.joint_pos[:15])
linkPosS = np.array(self.joint_pos[15:])
linkPosM = np.append([0., 0., -0.8], linkPosM)
linkPosS = np.append([0., 0., -0.8], linkPosS)
linkPosM = linkPosM.reshape(6, 3)
linkPosS = linkPosS.reshape(6, 3)
return linkPosM, linkPosS
def step(self, a):
alarm = []
Link_dis = []
bumpalarm = []
s = self.state
suck = self.image_input
self.collision = False
action_vec = a[:3] * self.ACTION_VEC_TRANS
action_ori = a[3:7] * self.ACTION_ORI_TRANS
action_phi = a[7] * self.ACTION_PHI_TRANS
self.action = np.append(action_vec, action_ori)
self.action = np.append(self.action, action_phi)
self.cmd = np.add(s[:8], self.action)
self.cmd[3:7] /= np.linalg.norm(self.cmd[3:7])
res = self.move_cmd_client(self.cmd, self.__name)
res_ = self.get_state_client(self.__obname)
if res.success:
self.old, self.joint_pos[:15], self.joint_angle = np.array(
res.state), res.joint_pos, res.joint_angle
self.limit, self.goal_quat, self.quat_inv, self.joint_pos[
15:
30] = res.limit, res.quaterniond, res.quat_inv, res_.joint_pos
linkPosM, linkPosS = self.collision_init()
alarm, Link_dis = self.cc.checkCollision(linkPosM, linkPosS)
s = np.append(self.old, np.subtract(self.goal[:3], self.old[:3]))
s = np.append(s, self.goal_quat[:4])
# s = np.append(s, np.subtract(self.goal[3:7], self.old[3:7]))
# s = np.append(s, np.subtract(-1*self.goal[3:7], self.old[3:7]))
s = np.append(s, Link_dis)
s = np.append(s, self.joint_angle)
s = np.append(s, self.limit)
self.dis_pos = np.linalg.norm(self.goal[:3] - s[:3])
self.dis_ori = math.sqrt(
np.linalg.norm(self.goal[3:7] - s[3:7]) +
np.linalg.norm(-1 * self.goal[3:7] - s[3:7]) - 2)
# self.angle_ori = self.quat_angle()
s = np.append(s, self.dis_pos)
s = np.append(s, self.dis_ori)
s = np.append(s, self.joint_pos[6:12])
s = np.append(s, self.goal_angle)
rospy.Subscriber('aa_box_pos', aa_box_pos, self.aa_suck)
# print(self.aa_box_x,"III",self.aa_box_y)
# if self.__name == '/right_':
# self.set_object('table_box', (0.55,0,0.345), (0, 0, 0, 0))
# self.set_object('aa_box', (self.aa_box_pos_x,self.aa_box_pos_y,0.82691), (0, 0, 0, 0))
# self.image_input = np.reshape(self.images_,-1)
# print(self.image_cnn[10])
self.aa_box_x = random.uniform(0.1, 0.3)
self.aa_box_y = random.uniform(-0.5, 0.5)
if self.aa_box_pos_y < -0.5:
vel = self.np_random.uniform(low=0, high=0.15, size=(2, ))
vel[0] = 0
self.set_aa_vel(vel)
if self.aa_box_pos_y > 0.5:
vel = self.np_random.uniform(low=-0.15, high=0, size=(2, ))
vel[0] = 0
self.set_aa_vel(vel)
# if self.aa_box_pos_x < 0.1 or self.aa_box_pos_x > 0.3 or self.aa_box_pos_y < -0.5 or self.aa_box_pos_y > 0.5:
# vel=self.np_random.uniform(low=-0.3, high=0.3, size=(2,))
# self.set_aa_vel(vel)
arm_x = 0.08
arm_r_y = 1
arm_l_y = -1
arm_z = -1.4125
# if self.__name == '/right_':
# self.aa_box_possition = [ self.aa_box_pos_x+0.05+arm_x, (self.aa_box_pos_y+0.05)*arm_r_y, 0.975+arm_z + 0.125 ,
# self.aa_box_pos_x+0.05+arm_x, (self.aa_box_pos_y-0.05)*arm_r_y, 0.975+arm_z + 0.125 ,
# self.aa_box_pos_x-0.05+arm_x, (self.aa_box_pos_y+0.05)*arm_r_y, 0.975+arm_z + 0.125 ,
# self.aa_box_pos_x-0.05+arm_x, (self.aa_box_pos_y-0.05)*arm_r_y, 0.975+arm_z + 0.125 ,
# self.aa_box_pos_x+0.05+arm_x, (self.aa_box_pos_y+0.05)*arm_r_y, 0.625+arm_z,
# self.aa_box_pos_x+0.05+arm_x, (self.aa_box_pos_y-0.05)*arm_r_y, 0.625+arm_z,
# self.aa_box_pos_x-0.05+arm_x, (self.aa_box_pos_y+0.05)*arm_r_y, 0.625+arm_z,
# self.aa_box_pos_x-0.05+arm_x, (self.aa_box_pos_y-0.05)*arm_r_y, 0.625+arm_z]
# if self.__name == '/left_':
# self.aa_box_possition = [ self.aa_box_pos_x+0.05+arm_x, (self.aa_box_pos_y+0.05)*arm_l_y, 0.975+arm_z + 0.125 ,
# self.aa_box_pos_x+0.05+arm_x, (self.aa_box_pos_y-0.05)*arm_l_y, 0.975+arm_z + 0.125 ,
# self.aa_box_pos_x-0.05+arm_x, (self.aa_box_pos_y+0.05)*arm_l_y, 0.975+arm_z + 0.125 ,
# self.aa_box_pos_x-0.05+arm_x, (self.aa_box_pos_y-0.05)*arm_l_y, 0.975+arm_z + 0.125 ,
# self.aa_box_pos_x+0.05+arm_x, (self.aa_box_pos_y+0.05)*arm_l_y, 0.625+arm_z,
# self.aa_box_pos_x+0.05+arm_x, (self.aa_box_pos_y-0.05)*arm_l_y, 0.625+arm_z,
# self.aa_box_pos_x-0.05+arm_x, (self.aa_box_pos_y+0.05)*arm_l_y, 0.625+arm_z,
# self.aa_box_pos_x-0.05+arm_x, (self.aa_box_pos_y-0.05)*arm_l_y, 0.625+arm_z]
if self.__name == '/right_':
self.aa_box_possition = [
self.aa_box_pos_x + 0.05, (self.aa_box_pos_y + 0.05),
self.aa_box_pos_z + 0.3, self.aa_box_pos_x + 0.05,
(self.aa_box_pos_y - 0.05), self.aa_box_pos_z + 0.3,
self.aa_box_pos_x - 0.05, (self.aa_box_pos_y + 0.05),
self.aa_box_pos_z + 0.3, self.aa_box_pos_x - 0.05,
(self.aa_box_pos_y - 0.05), self.aa_box_pos_z + 0.3,
self.aa_box_pos_x + 0.05, (self.aa_box_pos_y + 0.05),
self.aa_box_pos_z - 0.3, self.aa_box_pos_x + 0.05,
(self.aa_box_pos_y - 0.05), self.aa_box_pos_z - 0.3,
self.aa_box_pos_x - 0.05, (self.aa_box_pos_y + 0.05),
self.aa_box_pos_z - 0.3, self.aa_box_pos_x - 0.05,
(self.aa_box_pos_y - 0.05), self.aa_box_pos_z - 0.3
]
if self.__name == '/left_':
self.aa_box_possition = [
self.aa_box_pos_x + 0.05, (self.aa_box_pos_y + 0.05),
self.aa_box_pos_z + 0.3, self.aa_box_pos_x + 0.05,
(self.aa_box_pos_y - 0.05), self.aa_box_pos_z + 0.3,
self.aa_box_pos_x - 0.05, (self.aa_box_pos_y + 0.05),
self.aa_box_pos_z + 0.3, self.aa_box_pos_x - 0.05,
(self.aa_box_pos_y - 0.05), self.aa_box_pos_z + 0.3,
self.aa_box_pos_x + 0.05, (self.aa_box_pos_y + 0.05),
self.aa_box_pos_z - 0.3, self.aa_box_pos_x + 0.05,
(self.aa_box_pos_y - 0.05), self.aa_box_pos_z - 0.3,
self.aa_box_pos_x - 0.05, (self.aa_box_pos_y + 0.05),
self.aa_box_pos_z - 0.3, self.aa_box_pos_x - 0.05,
(self.aa_box_pos_y - 0.05), self.aa_box_pos_z - 0.3
]
s = np.append(s, self.aa_box_possition)
aa_box_possssss = np.array([self.aa_box_x, self.aa_box_y, 0.8])
self.dis_obstacle_1 = np.linalg.norm(aa_box_possssss[:3] -
self.joint_pos[6:9])
self.dis_obstacle_2 = np.linalg.norm(aa_box_possssss[:3] -
self.joint_pos[9:12])
# suck = np.append(suck, self.image_input)
s = np.append(s, self.dis_obstacle_1)
s = np.append(s, self.dis_obstacle_2)
# s = np.append(s, self.image_input)
# print(suck)
# print(s)
terminal = self._terminal(s, res.success, alarm)
alarm_cnt = 0
# for i in bumpalarm:
# alarm_cnt += i
# if alarm_cnt>0.4:
# terminal = True
reward = self.get_reward(s, res.success, terminal, res.singularity)
# self.images_ = suck
if (not self.collision) and math.fabs(s[7]) < 0.9:
self.state = s
## see
if self.workers == 'arm':
if self.object_pub == 0:
self.set_object2(self.__name,
(self.goal[0] - 0.08, self.goal[1],
self.goal[2] + 1.45086), (0, 0, 0, 0))
self.object_pub = 0
# else:
# self.set_object2(self.__name+'q', (self.goal[0]-0.08, self.goal[1], self.goal[2]+1.45086), self.goal[3:7])
# self.object_pub = 0
fail = False
# if not res.success or self.collision or res.singularity:
# fail = True
alarm_cnt = 0
for i in alarm:
alarm_cnt += i
if alarm_cnt > 0.4:
fail = True
return self.state, reward, terminal, self.success, fail
# , self.images_
def _terminal(self, s, ik_success, alarm):
alarm_cnt = 0
for i in alarm:
alarm_cnt += i
if alarm_cnt > 0.4:
self.collision = True
if ik_success and not self.collision:
# if self.dis_pos < self.goal_err and self.dis_ori < self.ori_err:
if self.dis_pos < self.goal_err:
self.success = True
if not self.done:
self.done = True
self.s_cnt += 1
self.range_cnt = self.range_cnt + 0.001 if self.range_cnt < 0.85 else 0.85 #0.004
self.rpy_range = self.rpy_range + 0.001 if self.rpy_range < 0.8 else 0.8 #0.002
self.goal_err = self.goal_err * 0.993 if self.goal_err > 0.03 else 0.03
# self.goal_err = self.goal_err*0.993 if self.goal_err > 0.015 else 0.015
# self.ori_err = self.ori_err*0.993 if self.ori_err > 0.2 else 0.2
return True
else:
self.success = False
return False
else:
self.success = False
return False
def get_reward(self, s, ik_success, terminal, singularity):
reward = 0.
# if not ik_success:
# return -20
# reward -= 20
reward -= self.dis_pos * 10
# reward -= self.dis_ori*2
if self.collision:
reward -= 100
# reward -= 100
# return -100
# reward -= 20
# if math.fabs(s[7])>0.9:
# return -20
# reward -= 10
if self.dis_pos < 0.03:
reward += 10
# reward += 5
# return 10
# if self.dis_ori < 0.3:
# reward += 10
# return 10
# reward += 0.4
# reward += self.dis_obstacle_1
# reward += self.dis_obstacle_2
# if reward > 0:
# reward *= 2
# cos_vec = np.dot(self.action[:3], self.state[8:11])/(np.linalg.norm(self.action[:3]) *np.linalg.norm(self.state[8:11]))
# reward += (cos_vec*self.dis_pos - self.dis_pos)/8
# reward -= 3
# if singularity:
# reward -= 5
return reward
class Test(core.Env):
# dt = .2
LINK_LENGTH_1 = 1. # [m]
LINK_LENGTH_2 = 1. # [m]
LINK_MASS_1 = 1. #: [kg] mass of link 1
LINK_MASS_2 = 1. #: [kg] mass of link 2
LINK_COM_POS_1 = 0.5 #: [m] position of the center of mass of link 1
LINK_COM_POS_2 = 0.5 #: [m] position of the center of mass of link 2
LINK_MOI = 1. #: moments of inertia for both links
# MAX_VEL_1 = 2.
# MAX_VEL_2 = 2.
# MAX_VEL_3 = 2.
ACTION_VEC_TRANS = 1/60
ACTION_ORI_TRANS = 1/10
ACTION_PHI_TRANS = 1/10
NAME = ['/right_arm', '/left_arm', '/right_arm']
# torque_noise_max = 0.
#: use dynamics equations from the nips paper or the book
# book_or_nips = "book"
# action_arrow = None
# domain_fig = None
# actions_num = 8
def __init__(self, name):
self.__name = self.NAME[name%2]
self.__obname = self.NAME[name%2 + 1]
self.viewer = None
high = np.array([1.,1.,1.,1.,1.,1.,1.,1., #8
1.,1.,1.,1.,1.,1.,1.,1., #8
0.,0.,0.,0.,0.,0., #6
0.,0.,0.,0.,0.,0.,0.,0.,0.,#9
0.,0.,0.,0.,0.,0.,0., #7
0.]) #1
#42
low = -high
# ox,oy,oz,oa,ob,oc,od,of,
# vx,vy,vz,va,vb,vc,vd,vf
# dis of Link(15)
#
# joint_angle(7),
# limit(1), rate(3)
self.observation_space = spaces.Box(low=low, high=high, dtype=np.float32)
self.action_space = spaces.Discrete(8)
self.state = []
self.action = []
self.cmd = []
self.point = []
self.goal = []
self.goal_pos = []
self.goal_quat = []
self.goal_phi = 0
self.old = []
self.old_pos = []
self.old_quat = []
self.old_phi = 0
self.joint_pos = []
self.joint_angle = []
self.limit = []
self.s_jointpos = []
# self.dis_pos
self.cc = CheckCollision()
self.collision = False
self.range_cnt = 0.1
self.s_cnt = 0
self.seed()
self.reset()
self.done = False
def get_state_client(self, cmd, name):
ik_service = name+'/train_env'
try:
rospy.wait_for_service(ik_service, timeout=1.)
except rospy.ROSException as e:
rospy.logwarn('wait_for_service timeout')
self.get_state_client(cmd, name)
client = rospy.ServiceProxy(
ik_service,
environment
)
# res = client(cmd)
res = client.call(cmd)
return res
def env_reset_client(self, cmd, name):
reset_service = name+'/env_reset'
try:
rospy.wait_for_service(reset_service, timeout=1.)
except rospy.ROSException as e:
rospy.logwarn('wait_for_service timeout')
self.env_reset_client(cmd, name)
client = rospy.ServiceProxy(
reset_service,
environment
)
# res = client(cmd)
res = client.call(cmd)
return res
def seed(self, seed=None):
self.np_random, seed = seeding.np_random(seed)
return [seed]
def get_state_jointpos(self):
self.s_jointpos = []
self.s_jointpos = np.append(self.joint_pos[6:12], self.joint_pos[18:27])
def reset(self):
self.goal = self.set_goal()
self.old, self.joint_pos[:12], self.joint_pos[12:24], self.joint_pos[24:27],self.joint_angle, self.limit = self.set_old()
linkPosM, linkPosS = self.collision_init(self.old[:3])
_, Link_dis = self.cc.checkCollision(linkPosM, linkPosS)
self.state = np.append(self.old, np.subtract(self.goal, self.old))
self.state = np.append(self.state, Link_dis)
self.state = np.append(self.state, self.joint_angle)
self.state = np.append(self.state, self.limit[0])
self.dis_pos = np.linalg.norm(np.subtract(self.goal[:3], self.old[:3]))
self.dis_ori = np.linalg.norm(np.subtract(self.goal[3:7], self.old[3:7]))
self.dis_phi = math.fabs(self.goal[7] - self.old[7])
# r_ori = (self.dis_ori/self.dis_pos)/6
# r_phi = (self.dis_phi/self.dis_pos)/6
# r_pos = 1 if self.dis_pos > 0.04 else self.dis_pos*20+0.2
# move_rate = [r_pos, r_ori, r_phi]
# self.state = np.append(self.state, move_rate)
self.collision = False
self.done = False
return self.state
def set_goal(self):
self.goal = self.np_random.uniform(low=0., high=self.range_cnt, size=(8,))
# print('self.goal = ', self.goal)
self.goal[0] = 0
self.goal = np.append(self.goal, self.range_cnt)
res = self.env_reset_client(self.goal, self.__name)
if res.success:
return res.state
else:
return self.set_goal()
def set_old(self):
self.old = self.np_random.uniform(low=0., high=self.range_cnt, size=(8,))
# print('self.old = ', self.old)
self.old[0] = 0
self.old = np.append(self.old, self.range_cnt)
res = self.env_reset_client(self.old, self.__name)
res_ = self.env_reset_client([0], self.__obname)
old_pos = []
old_pos = np.append(old_pos, res.state)
if np.linalg.norm(old_pos[:3] - self.goal[:3]) > 0.1:
return res.state, res.joint_pos, res_.joint_pos,[res_.state[0], res_.state[1], res_.state[2]], res.joint_angle, res.limit
else:
return self.set_old()
def collision_init(self, endpos):
linkPosM = np.array(self.joint_pos[0:12])
linkPosS = np.array(self.joint_pos[12:27])
linkPosM = np.append(linkPosM, endpos)
linkPosM = np.append([0.,0.,-0.8], linkPosM)
linkPosS = np.append([0.,0.,-0.8], linkPosS)
linkPosM = linkPosM.reshape(6,3)
linkPosS = linkPosS.reshape(6,3)
return linkPosM, linkPosS
def step(self, a):
alarm = []
Link_dis = []
s = self.state
action_vec = a[:3]*self.ACTION_VEC_TRANS
action_ori = a[3:7]*self.ACTION_ORI_TRANS
action_phi = a[7]*self.ACTION_PHI_TRANS
self.action = np.append(action_vec, action_ori)
self.action = np.append(self.action, action_phi)
self.cmd = np.add(s[:8], self.action)
self.cmd[3:7] /= np.linalg.norm(self.cmd[3:7])
if self.cmd[7]>1: self.cmd[7] = 1
elif self.cmd[7]<-1: self.cmd[7] = -1
res = self.get_state_client(self.cmd, self.__name)
res_ = self.get_state_client([0], self.__obname)
if res.success:
self.old, self.joint_pos[:12], self.joint_angle self.limit= res.state, res.joint_pos, res.joint_angle, res.limit
self.joint_pos[12:24] = res_.joint_pos
self.joint_pos[24:27] = [res_.state[0], res_.state[1], res_.state[2]]
linkPosM, linkPosS = self.collision_init(self.old[:3])
alarm, Link_dis = self.cc.checkCollision(linkPosM, linkPosS)
s = np.append(self.old, np.subtract(self.goal, self.old))
s = np.append(s, Link_dis)
s = np.append(s, self.joint_angle)
s = np.append(s, self.limit[0])
self.dis_pos = np.linalg.norm(self.goal[:3] - s[:3])
self.dis_ori = np.linalg.norm(self.goal[3:7] - s[3:7])
self.dis_phi = math.fabs(self.goal[7] - s[7])
self.dis_state = np.linalg.norm(self.goal[:7] - s[:7])
# r_ori = (self.dis_ori/self.dis_pos)/6
# r_phi = (self.dis_phi/self.dis_pos)/6
# r_pos = 1 if self.dis_pos > 0.04 else self.dis_pos*20+0.2
# move_rate = [r_pos, r_ori, r_phi]
# s = np.append(s, move_rate)
terminal = self._terminal(s, res.success, alarm)
reward = self.get_reward(s, res.success, terminal)
if not self.collision:
self.state = s
return self.state, reward, terminal, 1
def _terminal(self, s, ik_success, alarm):
if ik_success:
alarm_cnt = 0
for i in alarm:
alarm_cnt += i
if alarm_cnt>0:
self.collision = True
if (self.dis_pos < 0.01 and self.dis_ori < 0.2):
if not self.done:
self.done = True
self.s_cnt += 1
self.range_cnt = self.range_cnt + 0.003 if self.range_cnt < 0.95 else 0.95
print('ssssssuuuuuuccccccccceeeeeeeesssssssssss' , self.s_cnt)
return True
else:
return False
else:
return False
def get_reward(self, s, ik_success, terminal):
# goal_vec = self.goal[:3] - self.state[:3]
# goal_ori = self.goal[3:7]- self.state[3:7]
# goal_phi = self.goal[7] - self.state[7]
# old_dis = np.linalg.norm(self.goal - self.state[:8])
# cos_vec = np.dot(self.action[:3], goal_vec)/(np.linalg.norm(self.action[:3]) *np.linalg.norm(goal_vec))
# cos_ori = np.dot(self.action[3:7], goal_ori)/(np.linalg.norm(self.action[3:7])*np.linalg.norm(goal_ori))
# goal_dis = np.linalg.norm(self.dis_pos)
# a_leng = np.linalg.norm(self.action[:3]/self.ACTION_VEC_TRANS)
reward = 0
# if terminal:
# if not self.collision:
# reward += 100
# else:
# reward += -100
# return reward
# if not ik_success:
# return -100
# if a_leng<0.2 or a_leng>2:
# reward += -2
# if cos_vec > np.math.cos(30*np.pi/180):
# r = (cos_vec*cos_vec*cos_vec)/(goal_dis**0.5)
# reward += 10 if r > 10 else r
# elif self.dis_pos > 0.05:
# r = -goal_dis/(cos_vec+1)
# reward += -2 if r<-2 else r
# if cos_ori > np.math.cos(30*np.pi/180):
# reward += 3
# elif self.dis_ori > 0.15:
# reward += -2
# # if cos_ori < np.math.cos(60*np.pi/180):
# # reward += -1
# if goal_phi*self.action[7] > 0:
# reward += 2
# elif self.dis_phi > 0.15:
# reward += -1
# if self.dis_pos < 0.05 or self.dis_ori < 0.15 or self.dis_phi < 0.15:
# reward += 5
# return reward/5
#===============================================================================
if terminal:
return 3
if not ik_success:
reward -= 3
if self.collision:
reward -=3
# if a_leng<0.2 or a_leng>2:
# reward += -1
# if cos_vec > np.math.cos(10*np.pi/180):
# r = (cos_vec*cos_vec*cos_vec)
# reward += 2*r
# elif self.dis_state < old_dis:
# reward += 1
reward -= (self.dis_pos + self.dis_ori/2)
if self.dis_pos < 0.04:
reward += 1
if self.dis_ori < 0.3:
reward += 0.5
reward /= 2
# if self.dis_pos < 0.05 or self.dis_ori < 0.15 or self.dis_phi < 0.15:
# reward += 2
# reward /=
return reward
class Test(core.Env):
dt = .2
LINK_LENGTH_1 = 1. # [m]
LINK_LENGTH_2 = 1. # [m]
LINK_MASS_1 = 1. #: [kg] mass of link 1
LINK_MASS_2 = 1. #: [kg] mass of link 2
LINK_COM_POS_1 = 0.5 #: [m] position of the center of mass of link 1
LINK_COM_POS_2 = 0.5 #: [m] position of the center of mass of link 2
LINK_MOI = 1. #: moments of inertia for both links
MAX_VEL_1 = 2.
MAX_VEL_2 = 2.
MAX_VEL_3 = 2.
ACTION_VEC_TRANS = 1 / 240
ACTION_ORI_TRANS = 1 / 40
ACTION_PHI_TRANS = 1 / 40
NAME = ['/right_arm', '/left_arm', '/right_arm']
torque_noise_max = 0.
#: use dynamics equations from the nips paper or the book
book_or_nips = "book"
action_arrow = None
domain_fig = None
actions_num = 8
def __init__(self, name):
self.__name = self.NAME[name % 2]
self.__obname = self.NAME[name % 2 + 1]
self.viewer = None
high = np.array([
1.,
1.,
1., #1.,1.,1.,1.,1., #8
1.,
1.,
1., #1.,1.,1.,1.,1., #8
0.,
0.,
0.,
0.,
0.,
0., #6
0.,
0.,
0.,
0.,
0.,
0.,
0.,
0.,
0., #9
0.,
0.,
0., #2
0.,
0
]) #1
#24
low = -high
# ox,oy,oz,oa,ob,oc,od,of,
# vx,vy,vz,va,vb,vc,vd,vf
# dis of Link(15)
#
# joint_angle(7),
# limit(1), rate(3)
self.observation_space = spaces.Box(low=low,
high=high,
dtype=np.float32)
self.action_space = spaces.Discrete(3)
self.act_dim = 3
self.obs_dim = 26
self.state = []
self.action = []
self.cmd = []
self.point = []
self.goal = []
self.goal_pos = []
self.goal_quat = []
self.goal_phi = 0
self.old = []
self.old_pos = []
self.old_quat = []
self.old_phi = 0
self.joint_pos = []
self.joint_angle = []
self.limit = []
self.s_jointpos = []
# self.dis_pos
self.cc = CheckCollision()
self.collision = False
self.range_cnt = 0.8
self.s_cnt = 0
self.done = True
self.goal_err = 0.08
self.set_mode_pub = rospy.Publisher('/gazebo/set_model_state',
ModelState,
queue_size=1,
latch=True)
self.seed()
self.reset()
def get_state_client(self, cmd, name):
ik_service = name + '/train_env'
try:
rospy.wait_for_service(ik_service, timeout=1.)
except rospy.ROSException as e:
rospy.logwarn('wait_for_service timeout')
self.get_state_client(cmd, name)
client = rospy.ServiceProxy(ik_service, environment)
# res = client(cmd)
res = client.call(cmd)
return res
def env_reset_client(self, cmd, name):
reset_service = name + '/env_reset'
try:
rospy.wait_for_service(reset_service, timeout=1.)
except rospy.ROSException as e:
rospy.logwarn('wait_for_service timeout')
self.env_reset_client(cmd, name)
client = rospy.ServiceProxy(reset_service, environment)
# res = client(cmd)
res = client.call(cmd)
return res
def set_object(self, name, pos, ori):
msg = ModelState()
msg.model_name = name
msg.pose.position.x = pos[0]
msg.pose.position.y = pos[1]
msg.pose.position.z = pos[2]
msg.pose.orientation.w = ori[0]
msg.pose.orientation.x = ori[1]
msg.pose.orientation.y = ori[2]
msg.pose.orientation.z = ori[3]
msg.reference_frame = 'world'
self.set_mode_pub.publish(msg)
def seed(self, seed=None):
self.np_random, seed = seeding.np_random(seed)
return [seed]
def get_state_jointpos(self):
self.s_jointpos = []
self.s_jointpos = np.append(self.joint_pos[6:12],
self.joint_pos[18:27])
def move(self, goal):
self.goal = goal
res = self.get_state_client([0], self.__name)
res_ = self.get_state_client([0], self.__obname)
self.old, self.joint_pos[:12], self.joint_angle, self.limit = np.array(
res.state), res.joint_pos, res.joint_angle, res.limit
self.joint_pos[12:24] = res_.joint_pos
self.joint_pos[24:27] = [res_.state[0], res_.state[1], res_.state[2]]
linkPosM, linkPosS = self.collision_init(self.old[:3])
_, Link_dis = self.cc.checkCollision(linkPosM, linkPosS)
s = np.append(self.old[:3], np.subtract(self.goal[:3], self.old[:3]))
s = np.append(s, Link_dis)
s = np.append(s, self.joint_angle)
s = np.append(s, self.limit[0])
self.dis_pos = np.linalg.norm(self.goal[:3] - s[:3])
s = np.append(s, self.dis_pos)
self.state = s
return s
def reset(self):
# if self.done:
self.goal = self.set_goal()
self.old, self.joint_pos[:12], self.joint_pos[12:24], self.joint_pos[
24:27], self.joint_angle, self.limit = self.set_old()
linkPosM, linkPosS = self.collision_init(self.old[:3])
alarm, Link_dis = self.cc.checkCollision(linkPosM, linkPosS)
alarm_cnt = 0
for i in alarm:
alarm_cnt += i
if alarm_cnt > 0:
print('fuckfuck')
return self.reset()
self.state = np.append(self.old[:3],
np.subtract(self.goal, self.old[:3]))
self.state = np.append(self.state, Link_dis)
self.state = np.append(self.state, self.joint_angle)
self.state = np.append(self.state, self.limit[0])
self.dis_pos = np.linalg.norm(self.goal[:3] - self.old[:3])
self.state = np.append(self.state, self.dis_pos)
self.collision = False
self.done = False
return self.state
def set_goal(self):
self.goal = self.np_random.uniform(low=0.,
high=self.range_cnt,
size=(8, ))
# print('self.goal = ', self.goal)
self.goal[0] = 0
self.goal[3] = self.range_cnt / 2
self.goal = np.append(self.goal, self.range_cnt)
res = self.env_reset_client(self.goal, self.__name)
goal_pos = np.array(res.state)
if not res.success:
return self.set_goal()
if np.linalg.norm(goal_pos[:2]) > 0.2:
return goal_pos[:3]
else:
return self.set_goal()
def set_old(self):
# if self.done:
self.start = self.np_random.uniform(low=0.,
high=self.range_cnt,
size=(8, ))
# print('self.old = ', self.old)
self.start[0] = 0
self.start[3] = self.range_cnt / 2
self.start = np.append(self.start, self.range_cnt)
res = self.env_reset_client(self.start, self.__name)
res_ = self.env_reset_client([0], self.__obname)
old_pos = np.array(res.state)
if not res.success:
return self.set_old()
if np.linalg.norm(np.subtract(old_pos[:3], self.goal[:3])) > 0.1:
return np.array(res.state), res.joint_pos, res_.joint_pos, [
res_.state[0], res_.state[1], res_.state[2]
], res.joint_angle, res.limit
else:
return self.set_old()
def collision_init(self, endpos):
linkPosM = np.array(self.joint_pos[0:12])
linkPosS = np.array(self.joint_pos[12:27])
linkPosM = np.append(linkPosM, endpos)
linkPosM = np.append([0., 0., -0.8], linkPosM)
linkPosS = np.append([0., 0., -0.8], linkPosS)
linkPosM = linkPosM.reshape(6, 3)
linkPosS = linkPosS.reshape(6, 3)
return linkPosM, linkPosS
def step(self, a):
t1 = time.time()
alarm = []
Link_dis = []
s = self.state
self.collision = False
action_vec = a[:3] * self.ACTION_VEC_TRANS
# action_ori = a[3:7]*self.ACTION_ORI_TRANS
# action_phi = a[7]*self.ACTION_PHI_TRANS
self.action = action_vec
self.cmd = np.add(s[:3], self.action)
self.cmd = np.append(self.cmd, self.old[3:])
# if self.__name == '/right_arm':
# print(self.cmd)
t2 = time.time()
res = self.get_state_client(self.cmd, self.__name)
t3 = time.time()
res_ = self.get_state_client([0], self.__obname)
t4 = time.time()
if res.success:
self.old, self.joint_pos[:12], self.joint_angle, self.limit = np.array(
res.state), res.joint_pos, res.joint_angle, res.limit
self.joint_pos[12:24] = res_.joint_pos
self.joint_pos[24:27] = [
res_.state[0], res_.state[1], res_.state[2]
]
linkPosM, linkPosS = self.collision_init(self.old[:3])
alarm, Link_dis = self.cc.checkCollision(linkPosM, linkPosS)
s = np.append(self.old[:3], np.subtract(self.goal[:3],
self.old[:3]))
s = np.append(s, Link_dis)
s = np.append(s, self.joint_angle)
s = np.append(s, self.limit[0])
self.dis_pos = np.linalg.norm(self.goal[:3] - s[:3])
s = np.append(s, self.dis_pos)
# print(Link_dis)
# else:
# print(res.joint_angle, res.limit)
terminal = self._terminal(s, res.success, alarm)
reward = self.get_reward(s, res.success, terminal)
# print(self.collision)
# if self.__name == '/right_arm':
# print('cmdcmdcmdcmdcmd', self.cmd)
# print('oldoldoldoldold', self.old)
# print(reward)
if not self.collision:
self.state = s
# else:
# print(Link_dis)
self.set_object(
self.__name,
(self.goal[0] - 0.08, self.goal[1], self.goal[2] + 1.45086),
(0, 0, 0, 0))
return self.state, reward, terminal, self.collision
def _terminal(self, s, ik_success, alarm):
alarm_cnt = 0
for i in alarm:
alarm_cnt += i
if alarm_cnt > 0.5:
self.collision = True
if ik_success and not self.collision:
if self.dis_pos < self.goal_err:
if not self.done:
self.done = True
self.s_cnt += 1
self.range_cnt = self.range_cnt + 0.003 if self.range_cnt < 0.95 else 0.95
self.goal_err = self.goal_err * 0.995 if self.goal_err > 0.01 else 0.01
print('ssssssuuuuuuccccccccceeeeeeeesssssssssss',
self.s_cnt, self.goal_err)
return True
else:
return False
else:
return False
def get_reward(self, s, ik_success, terminal):
reward = 0.
if not ik_success:
return -2
if self.collision:
return -3
reward -= self.dis_pos
reward += 0.2
if reward > 0:
reward *= 3
cos_vec = np.dot(self.action[:3], self.state[3:6]) / (
np.linalg.norm(self.action[:3]) * np.linalg.norm(self.state[3:6]))
reward += (cos_vec * self.dis_pos - self.dis_pos)
reward -= 1.4
# if terminal and ik_success and not self.collision:
# reward += 1
# if self.dis_pos < 0.04:
# reward += 1
# if self.dis_pos < 0.1:
# reward += 1
return reward
class Test(core.Env):
ACTION_VEC_TRANS = 1/180
ACTION_ORI_TRANS = 1/60
ACTION_PHI_TRANS = 1/60
NAME = ['/right_', '/left_', '/right_']
def __init__(self, name, workers):
self.__name = self.NAME[name%2]
self.__obname = self.NAME[name%2 + 1]
# if workers == 0:
self.workers = 'arm'
# else:
# self.workers = str(workers)
high = np.array([1.,1.,1.,1.,1.,1.,1.,1., #8
1.,1.,1.,1.,1.,1.,1.,1.,1.,1.,1., #7
0.,0.,0.,0.,0.,0., #6
0.,0.,0.,0.,0.,0.,0.,0.,0.,#9
0.,0.,0.,0.,0.,0.,0.,0.,0., #2
0.,0.,0.,0.,0.,0.]) #1
#24
low = -1*high
# ox,oy,oz,oa,ob,oc,od,of,
# vx,vy,vz,va,vb,vc,vd,vf
# dis of Link(15)
#
# joint_angle(7),
# limit(1), rate(3)
self.observation_space = spaces.Box(low=low, high=high, dtype=np.float32)
self.action_space = spaces.Discrete(3)
self.act_dim=8
self.obs_dim=57
self.state = []
self.action = []
self.cmd = []
self.point = []
self.goal = []
self.goal_pos = []
self.goal_quat = []
self.goal_phi = 0
self.goal_rpy = []
self.old = []
self.old_pos = []
self.old_quat = []
self.old_phi = 0
self.joint_pos = []
self.joint_angle = []
self.limit = []
self.s_jointpos = []
# self.dis_pos
self.cc = CheckCollision()
self.collision = False
self.range_cnt = 0.6#+(workers*0.1)
self.rpy_range = 0.2*(workers+1)
self.done = True
self.s_cnt = 0
self.goal_err = 0.08
self.ori_err = 0.4
self.quat_inv = False
self.goal_angle = []
self.object_pub = 0
self.set_model_pub = rospy.Publisher(
'/gazebo/set_model_state',
ModelState,
queue_size=1,
latch=True
)
self.set_mode_pub = rospy.Publisher(
self.__name+self.workers+'/set_mode_msg',
String,
queue_size=1,
latch=True
)
# self.set_aabox_pub = rospy.Publisher(
# 'aa_box_pos_msg',
# aa_box_pos_msg,
# queue_size=1,
# latch=True
# )
self.bridge = CvBridge()
self.depth_dim = 76800
self.depth_image = None
self.image_input = None
self.__bumper = None
self.__robot = "_arm"
self.images_ = []
self.aa_box_x = 0
self.aa_box_y = 0
# self.set_mode_pub.publish('set_mode')
rospy.Subscriber('/ir_depth/depth/image_raw',Image,self.callback )
self.seed(345*(workers+1) + 467*(name+1))
self.reset()
## image (gazebo)
# rospy.Subscriber("odom", Odometry,self.get_aa_box_position)
# rospy.Subscriber("/bumper",ContactsState,self.Sub_Bumper)
@property
def is_success(self):
return self.done
@property
def success_cnt(self):
return self.s_cnt
## image (gazebo)
# def get_aa_box_position(self,msg = None):
# print(msg.pose.pose.position.x)
# print(msg.pose.pose.position.y)
def aa_suck(self,data):
try:
self.aa_box_x = data.x
self.aa_box_y = data.y
except CvBridgeError as e:
print(e)
# def set_aa_box_vel(self, goal_x, goal_y):
def callback(self,data):
try:
# if len(data.data) == 0:
# print("data.data == 0")
tmp = self.bridge.imgmsg_to_cv2(data,"32FC1")
tmp = cv2.resize(tmp , (320,240))
where_are_nan = np.isnan(tmp)
where_are_inf = np.isinf(tmp)
tmp[where_are_nan] = 10
tmp[where_are_inf] = 10
# print(tmp)
self.images_ = tmp / 10
# print("aaas")
# if self.images_.shape == (0,):
# rospy.logwarn("self.images_ = 0")
# print("aaaaaaaaaaa")
# print(self.images_)
# cv2.imshow('My Image', self.images_)
# cv2.waitKey(1)
except CvBridgeError as e:
print(e)
# # cv2.destroyAllWindows()
# def image_client(self):
# # rospy.init_node('depth_image',anonymous=True)
# print("aaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaa")
# rospy.Subscriber('ir_depth/depth/image_raw',Image,self.callback )
# rospy.Subscriber('/camera/rgb/image_raw',Image,self.callback )
# rospy.spin()
##bumper gazebo
# def Sub_Bumper(self,msg = None):
# while msg is None and not rospy.is_shutdown():
# try:
# # msg = rospy.wait_for_message("/bumper", ContactsState)
# msg = rospy.wait_for_message("/bumper", ContactsState, timeout=1.)
# except:
# print("don't listen bumper")
# if(len(msg.states)):
# for state in msg.states:
# if(self.__robot in state.info):
# self.__bumper = True
# print("fuckkkk")
# # else:
# # print("aaa")
# def Check_Connection(self):
# init = None
# # while init is None and not rospy.is_shutdown():
# # try:
# # init = rospy.wait_for_message("/scan", LaserScan, timeout=1.0)
# # except:
# # print('scan not init')
# # init = None
# while init is None and not rospy.is_shutdown():
# try:
# init = rospy.wait_for_message("/bumper", ContactsState, timeout=1.0)
# except:
# print('bumper not init')
# # print('init success nh')
# return True
# CNN
# def build_cnnlayer(self, conv_in, net_name):
# # build conv layer
# conv_out = None
# for key in sorted(cfg[net_name]):
# com = cfg[net_name][key] #component
# if com['type'] in 'conv':
# stride = com['stride']
# conv_out = Conv2D(conv_in, com['kernel_size'], com['out_channel'], name_prefix=net_name+'_'+key, strides=[1, stride, stride, 1])
# conv_in = conv_out
# if 'spatial_softmax' in com:
# self.logger.debug('Last_conv.shape = {}'.format(conv_in.shape))
# conv_out = tf.contrib.layers.spatial_softmax(conv_in, name='spatial_softmax')
# # print(conv_out.shape)
# # if don't have spatial softmax need to flatten
# if len(conv_out.shape) > 2:
# conv_out = Flaten(conv_out)
# return conv_out
# def _save_img(self, img_buffer, img_):
def get_state_client(self, name):
service = name+self.workers+'/get_state'
try:
rospy.wait_for_service(service, timeout=1.)
except rospy.ROSException as e:
rospy.logwarn('wait_for_service timeout')
self.get_state_client(name)
client = rospy.ServiceProxy(
service,
get_state
)
# res = client(cmd)
res = client.call()
return res
def move_cmd_client(self, cmd, name):
service = name+self.workers+'/move_cmd'
try:
rospy.wait_for_service(service, timeout=1.)
except rospy.ROSException as e:
rospy.logwarn('wait_for_service timeout')
self.move_cmd_client(cmd, name)
client = rospy.ServiceProxy(
service,
move_cmd
)
# res = client(cmd)
res = client.call(cmd)
return res
def set_start_client(self, cmd, rpy, name):
service = name+self.workers+'/set_start'
try:
rospy.wait_for_service(service, timeout=1.)
except rospy.ROSException as e:
rospy.logwarn('wait_for_service timeout')
self.set_start_client(cmd, rpy, name)
client = rospy.ServiceProxy(
service,
set_start
)
# res = client(cmd)
res = client(action=cmd, rpy=rpy)
return res
def set_goal_client(self, cmd, rpy, name):
service = name+self.workers+'/set_goal'
try:
rospy.wait_for_service(service, timeout=1.)
except rospy.ROSException as e:
rospy.logwarn('wait_for_service timeout')
self.set_goal_client(cmd, rpy, name)
client = rospy.ServiceProxy(
service,
set_goal
)
# res = client(cmd)
res = client(action=cmd, rpy=rpy)
return res
def set_object(self, name, pos, ori):
msg = ModelState()
msg.model_name = name
msg.pose.position.x = pos[0]
msg.pose.position.y = pos[1]
msg.pose.position.z = pos[2]
msg.pose.orientation.w = ori[0]
msg.pose.orientation.x = ori[1]
msg.pose.orientation.y = ori[2]
msg.pose.orientation.z = ori[3]
msg.reference_frame = 'world'
self.set_model_pub.publish(msg)
def set_object2(self, name, pos, ori):
msg = ModelState()
msg.model_name = name+self.workers
msg.pose.position.x = pos[0]
msg.pose.position.y = pos[1]
msg.pose.position.z = pos[2]
msg.pose.orientation.w = ori[0]
msg.pose.orientation.x = ori[1]
msg.pose.orientation.y = ori[2]
msg.pose.orientation.z = ori[3]
msg.reference_frame = 'world'
self.set_model_pub.publish(msg)
def seed(self, seed=None):
self.np_random, seed = seeding.np_random(seed)
return [seed]
def quat_angle(self):
cos_q = np.dot(self.goal[3:7], self.old[3:7])
if (self.quat_inv and cos_q>0) or ((not self.quat_inv) and cos_q<=0):
cos_q = np.dot(-1*self.goal[3:7], self.old[3:7])
return math.acos(cos_q)/pi
def move(self, goal):
self.goal = goal
res = self.get_state_client(self.__name)
res_ = self.get_state_client(self.__obname)
self.old, self.joint_pos[:15], self.joint_angle = np.array(res.state), res.joint_pos, res.joint_angle
self.limit, self.goal_quat, self.quat_inv, self.joint_pos[15:30] = res.limit, res.quaterniond, res.quat_inv, res_.joint_pos
linkPosM, linkPosS = self.collision_init()
alarm, Link_dis = self.cc.checkCollision(linkPosM, linkPosS)
s = np.append(self.old[:3], np.subtract(self.goal[:3], self.old[:3]))
s = np.append(s, Link_dis)
s = np.append(s, self.joint_angle)
s = np.append(s, self.limit[0])
self.dis_pos = np.linalg.norm(self.goal[:3] - s[:3])
s = np.append(s, self.dis_pos)
self.state = s
return s
def reset(self):
self.old, self.joint_pos[:15], self.joint_pos[15:30], self.joint_angle, self.limit, self.goal_quat, self.quat_inv = self.set_old()
linkPosM, linkPosS = self.collision_init()
alarm, Link_dis = self.cc.checkCollision(linkPosM, linkPosS)
alarm_cnt = 0
for i in alarm:
alarm_cnt += i
if alarm_cnt>0:
return self.reset()
joint_pos_tmp = self.joint_pos
self.goal, self.goal_angle, self.joint_pos[:15], self.joint_pos[15:30]= self.set_goal()
linkPosM, linkPosS = self.collision_init()
alarm, Link_dis = self.cc.checkCollision(linkPosM, linkPosS)
alarm_cnt = 0
for i in alarm:
alarm_cnt += i
if alarm_cnt>0:
return self.reset()
self.joint_pos = joint_pos_tmp
self.state = np.append(self.old, np.subtract(self.goal[:3], self.old[:3]))
self.state = np.append(self.state, self.goal_quat[:4])
# self.state = np.append(self.state, np.subtract(self.goal[3:7], self.old[3:7]))
# self.state = np.append(self.state, np.subtract(-1*self.goal[3:7], self.old[3:7]))
self.state = np.append(self.state, Link_dis)
self.state = np.append(self.state, self.joint_angle)
self.state = np.append(self.state, self.limit)
self.dis_pos = np.linalg.norm(self.goal[:3] - self.old[:3])
self.dis_ori = math.sqrt(np.linalg.norm(self.goal[3:7] - self.old[3:7]) + np.linalg.norm(-1*self.goal[3:7] - self.old[3:7]) - 2)
# self.angle_ori = self.quat_angle()
self.state = np.append(self.state, self.dis_pos)
self.state = np.append(self.state, self.dis_ori)
self.state = np.append(self.state, self.joint_pos[6:12])
self.state = np.append(self.state, self.goal_angle)
# print("aaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaa")
# print(self.images_)
# time.sleep(1)
if self.__name == '/right_':
self.aa_box_x = random.uniform(0.1,0.3)
self.aa_box_y = random.uniform(-0.5,0.5)
self.set_object('table_box', (0.55,0,0.345), (0, 0, 0, 0))
self.set_object('aa_box', (self.aa_box_x,self.aa_box_y,0.8), (0, 0, 0, 0))
# aa_box = aa_box_pos_msg()
# aa_box.x = 1.
# aa_box.y = 2.
# self.set_aabox_pub.publish(aa_box)
# rospy.Publisher('aa_box_pos',self.aa_box_x,)
# print(self.aa_box_x,"III",self.aa_box_x,self.aa_box_y)
self.img_suckkkkkkkkkkkkk = np.reshape(self.images_,-1)
print("aaa")
print(self.img_suckkkkkkkkkkkkk.shape)
print("bbb")
self.collision = False
self.done = False
self.success = False
return self.state , self.img_suckkkkkkkkkkkkk
def set_goal(self):
self.goal = self.np_random.uniform(low=-0.5, high=0.5, size=(8,))
# self.goal[1] = self.np_random.uniform(low=0., high=0.5)
rpy = self.np_random.uniform(low=-1*self.rpy_range, high=self.rpy_range, size=(4,))
# print('self.goal = ', self.goal)
# if self.goal[0]>0.5:
# if self.goal[0]>0.75:
# self.goal[2] /= -3
# else:
# self.goal[2] /= -2
# self.goal[2]+=1
self.goal[0] = 0
# self.goal[3] = self.range_cnt/2
self.goal = np.append(self.goal, self.range_cnt)
res = self.set_goal_client(self.goal, rpy, self.__name)
res_ = self.get_state_client(self.__obname)
goal_pos = np.array(res.state)
if not res.success:
return self.set_goal()
else:
return goal_pos[:7], res.joint_angle, res.joint_pos, res_.joint_pos
def set_old(self):
self.start = self.np_random.uniform(low=-0.5, high=0.5, size=(8,))
rpy = self.np_random.uniform(low=-1*self.rpy_range, high=self.rpy_range, size=(4,))
# if self.start[0]>0.5:
# if self.start[0]>0.75:
# self.start[2] /= -3
# else:
# self.start[2] /= -2
# self.start[2] +=1
self.start[0] = 0
# self.start[3] = self.range_cnt/2
self.start = np.append(self.start, self.range_cnt)
res = self.set_start_client(self.start, rpy, self.__name)
res_ = self.get_state_client(self.__obname)
old_pos = np.array(res.state)
if not res.success:
return self.set_old()
else:
return old_pos, res.joint_pos, res_.joint_pos, res.joint_angle, res.limit, res.quaterniond, res.quat_inv
def collision_init(self):
linkPosM = np.array(self.joint_pos[:15])
linkPosS = np.array(self.joint_pos[15:])
linkPosM = np.append([0.,0.,-0.8], linkPosM)
linkPosS = np.append([0.,0.,-0.8], linkPosS)
linkPosM = linkPosM.reshape(6,3)
linkPosS = linkPosS.reshape(6,3)
return linkPosM, linkPosS
def step(self, a):
alarm = []
Link_dis = []
s = self.state
suck = []
self.collision = False
action_vec = a[:3]*self.ACTION_VEC_TRANS
action_ori = a[3:7]*self.ACTION_ORI_TRANS
action_phi = a[7]*self.ACTION_PHI_TRANS
self.action = np.append(action_vec, action_ori)
self.action = np.append(self.action, action_phi)
self.cmd = np.add(s[:8], self.action)
self.cmd[3:7] /= np.linalg.norm(self.cmd[3:7])
res = self.move_cmd_client(self.cmd, self.__name)
res_ = self.get_state_client(self.__obname)
self.image_input = np.reshape(self.images_,-1)
# print(self.image_input.shape)
# print(self.image_cnn[10])
suck = np.append(suck, self.image_input)
if res.success:
self.old, self.joint_pos[:15], self.joint_angle = np.array(res.state), res.joint_pos, res.joint_angle
self.limit, self.goal_quat, self.quat_inv, self.joint_pos[15:30] = res.limit, res.quaterniond, res.quat_inv, res_.joint_pos
linkPosM, linkPosS = self.collision_init()
alarm, Link_dis = self.cc.checkCollision(linkPosM, linkPosS)
s = np.append(self.old, np.subtract(self.goal[:3], self.old[:3]))
s = np.append(s, self.goal_quat[:4])
# s = np.append(s, np.subtract(self.goal[3:7], self.old[3:7]))
# s = np.append(s, np.subtract(-1*self.goal[3:7], self.old[3:7]))
s = np.append(s, Link_dis)
s = np.append(s, self.joint_angle)
s = np.append(s, self.limit)
self.dis_pos = np.linalg.norm(self.goal[:3] - s[:3])
self.dis_ori = math.sqrt(np.linalg.norm(self.goal[3:7] - s[3:7]) + np.linalg.norm(-1*self.goal[3:7] - s[3:7]) - 2)
# self.angle_ori = self.quat_angle()
s = np.append(s, self.dis_pos)
s = np.append(s, self.dis_ori)
s = np.append(s, self.joint_pos[6:12])
s = np.append(s, self.goal_angle)
# print(s.shape)
# print(suck)
# print(s)
terminal = self._terminal(s, res.success, alarm)
reward = self.get_reward(s, res.success, terminal, res.singularity)
# self.images_ = suck
if (not self.collision) and math.fabs(s[7])<0.9:
self.state = s
# rospy.Subscriber('aa_box_pos_msg',float32,self.aa_suck)
# print(self.aa_box_x,"III",self.aa_box_y)
# if self.__name == '/right_':
# self.set_object('table_box', (0.55,0,0.345), (0, 0, 0, 0))
# self.set_object('aa_box', (self.aa_box_x,self.aa_box_y,0.8), (0, 0, 0, 0))
## see
# if self.workers == 'arm':
# if self.object_pub == 0:
# self.set_object2(self.__name, (self.goal[0]-0.08, self.goal[1], self.goal[2]+1.45086), (0, 0, 0, 0))
# self.object_pub = 1
# else:
# self.set_object2(self.__name+'q', (self.goal[0]-0.08, self.goal[1], self.goal[2]+1.45086), self.goal[3:7])
# self.object_pub = 0
fail = False
if not res.success or self.collision or res.singularity:
fail = True
return self.state, reward, terminal, self.success, fail, suck
# , self.images_
def _terminal(self, s, ik_success, alarm):
alarm_cnt = 0
for i in alarm:
alarm_cnt += i
if alarm_cnt>0.4:
self.collision = True
if ik_success and not self.collision:
if self.dis_pos < self.goal_err and self.dis_ori < self.ori_err:
self.success = True
if not self.done:
self.done = True
self.s_cnt += 1
self.range_cnt = self.range_cnt + 0.001 if self.range_cnt < 0.85 else 0.85 #0.004
self.rpy_range = self.rpy_range + 0.001 if self.rpy_range < 0.8 else 0.8 #0.002
self.goal_err = self.goal_err*0.993 if self.goal_err > 0.015 else 0.015
self.ori_err = self.ori_err*0.993 if self.ori_err > 0.2 else 0.2
return True
else:
self.success = False
return False
else:
self.success = False
return False
def get_reward(self, s, ik_success, terminal, singularity):
reward = 0.
if not ik_success:
return -5
if self.collision:
return -5
if math.fabs(s[7])>0.9:
return -5
if terminal:
return 3
reward -= self.dis_pos*5
reward -= self.dis_ori*5
reward += 0.5
if reward > 0:
reward *= 2
cos_vec = np.dot(self.action[:3], self.state[8:11])/(np.linalg.norm(self.action[:3]) *np.linalg.norm(self.state[8:11]))
reward += (cos_vec*self.dis_pos - self.dis_pos)/8
reward -= 2
if singularity:
reward -= 3
return reward
def __init__(self, name, workers):
self.__name = self.NAME[name%2]
self.__obname = self.NAME[name%2 + 1]
if workers == 0:
self.workers = 'arm'
else:
self.workers = str(workers)
high = np.array([1.,1.,1.,1.,1.,1.,1.,1., #8
1.,1.,1.,1.,1.,1.,1.,1.,1.,1.,1., #7
0.,0.,0.,0.,0.,0., #6
0.,0.,0.,0.,0.,0.,0.,0.,0.,#9
0.,0.,0.,0.,0.,0.,0.,0.,0., #2
0.,0.,0.,0.,0.,0.]) #1
#24
low = -1*high
# ox,oy,oz,oa,ob,oc,od,of,
# vx,vy,vz,va,vb,vc,vd,vf
# dis of Link(15)
#
# joint_angle(7),
# limit(1), rate(3)
self.observation_space = spaces.Box(low=low, high=high, dtype=np.float32)
self.action_space = spaces.Discrete(3)
self.act_dim=8
self.obs_dim=57
self.state = []
self.action = []
self.cmd = []
self.point = []
self.goal = []
self.goal_pos = []
self.goal_quat = []
self.goal_phi = 0
self.goal_rpy = []
self.old = []
self.old_pos = []
self.old_quat = []
self.old_phi = 0
self.joint_pos = []
self.joint_angle = []
self.limit = []
self.s_jointpos = []
# self.dis_pos
self.cc = CheckCollision()
self.collision = False
self.range_cnt = 0.6#+(workers*0.1)
self.rpy_range = 0.2*(workers+1)
self.done = True
self.s_cnt = 0
self.goal_err = 0.08
self.ori_err = 0.4
self.quat_inv = False
self.goal_angle = []
self.object_pub = 0
self.set_model_pub = rospy.Publisher(
'/gazebo/set_model_state',
ModelState,
queue_size=1,
latch=True
)
self.set_mode_pub = rospy.Publisher(
self.__name+self.workers+'/set_mode_msg',
String,
queue_size=1,
latch=True
)
# self.set_mode_pub.publish('set_mode')
self.seed(345*(workers+1) + 467*(name+1))
self.reset()
def __init__(self, name, workers):
self.__name = self.NAME[name % 2]
self.__obname = self.NAME[name % 2 + 1]
# if workers == 0:
self.workers = 'arm'
# else:
# self.workers = str(workers)
high = np.array([
1.,
1.,
1.,
1.,
1.,
1.,
1.,
1., #8
1.,
1.,
1.,
1.,
1.,
1.,
1.,
1.,
1.,
1.,
1., #7
0.,
0.,
0.,
0.,
0.,
0., #6
0.,
0.,
0.,
0.,
0.,
0.,
0.,
0.,
0., #9
0.,
0.,
0.,
0.,
0.,
0.,
0.,
0.,
0., #2
0.,
0.,
0.,
0.,
0.,
0.
]) #1
#24
low = -1 * high
# ox,oy,oz,oa,ob,oc,od,of,
# vx,vy,vz,va,vb,vc,vd,vf
# dis of Link(15)
#
# joint_angle(7),
# limit(1), rate(3)
self.observation_space = spaces.Box(low=low,
high=high,
dtype=np.float32)
self.action_space = spaces.Discrete(3)
self.act_dim = 8
self.obs_dim = 57 + 24 + 2
self.state = []
self.action = []
self.cmd = []
self.point = []
self.goal = []
self.goal_pos = []
self.goal_quat = []
self.goal_phi = 0
self.goal_rpy = []
self.old = []
self.old_pos = []
self.old_quat = []
self.old_phi = 0
self.joint_pos = []
self.joint_angle = []
self.limit = []
self.s_jointpos = []
# self.dis_pos
self.cc = CheckCollision()
self.collision = False
self.range_cnt = 0.6 #+(workers*0.1)
self.rpy_range = 0.2 * (workers + 1)
self.done = True
self.s_cnt = 0
self.goal_err = 0.08
self.ori_err = 0.3
self.quat_inv = False
self.goal_angle = []
self.object_pub = 0
self.set_model_pub = rospy.Publisher('/gazebo/set_model_state',
ModelState,
queue_size=1,
latch=True)
self.set_mode_pub = rospy.Publisher(self.__name + self.workers +
'/set_mode_msg',
String,
queue_size=1,
latch=True)
self.set_aabox_pub = rospy.Publisher('aa_box_pos',
aa_box_pos,
queue_size=1,
latch=True)
self.bridge = CvBridge()
self.depth_dim = 76800
self.depth_image = None
self.image_input = None
self.__bumper = None
self.__robot = "_arm"
self.images_ = []
self.aa_box_x = 0
self.aa_box_y = 0
# self.set_mode_pub.publish('set_mode')
rospy.Subscriber('/ir_depth/depth/image_raw', Image, self.callback)
self.seed(9)
# self.seed(345*(workers+1) + 467*(name+1))
self.reset(True)
## image (gazebo)
# rospy.Subscriber("odom", Odometry,self.get_aa_box_position)
# rospy.Subscriber("/bumper",ContactsState,self.Sub_Bumper)
self.arm_move = rospy.Publisher(self.__name + 'arm_move',
Float32MultiArray,
queue_size=1,
latch=True)
class Test(core.Env):
# ACTION_VEC_TRANS = 1/600
# ACTION_ORI_TRANS = 1/100
# ACTION_PHI_TRANS = 1/100
ACTION_VEC_TRANS = 1 / 240
ACTION_ORI_TRANS = 1 / 60
ACTION_PHI_TRANS = 1 / 60
NAME = ['/right_', '/left_', '/right_']
def __init__(self, name, workers):
self.__name = self.NAME[name % 2]
self.__obname = self.NAME[name % 2 + 1]
# if workers == 0:
self.workers = 'arm'
# else:
# self.workers = str(workers)
high = np.array([
1.,
1.,
1.,
1.,
1.,
1.,
1.,
1., #8
1.,
1.,
1.,
1.,
1.,
1.,
1.,
1.,
1.,
1.,
1., #7
0.,
0.,
0.,
0.,
0.,
0., #6
0.,
0.,
0.,
0.,
0.,
0.,
0.,
0.,
0., #9
0.,
0.,
0.,
0.,
0.,
0.,
0.,
0.,
0., #2
0.,
0.,
0.,
0.,
0.,
0.
]) #1
#24
low = -1 * high
# ox,oy,oz,oa,ob,oc,od,of,
# vx,vy,vz,va,vb,vc,vd,vf
# dis of Link(15)
#
# joint_angle(7),
# limit(1), rate(3)
self.observation_space = spaces.Box(low=low,
high=high,
dtype=np.float32)
self.action_space = spaces.Discrete(3)
self.act_dim = 8
self.obs_dim = 57 + 24 + 2
self.state = []
self.action = []
self.cmd = []
self.point = []
self.goal = []
self.goal_pos = []
self.goal_quat = []
self.goal_phi = 0
self.goal_rpy = []
self.old = []
self.old_pos = []
self.old_quat = []
self.old_phi = 0
self.joint_pos = []
self.joint_angle = []
self.limit = []
self.s_jointpos = []
# self.dis_pos
self.cc = CheckCollision()
self.collision = False
self.range_cnt = 0.6 #+(workers*0.1)
self.rpy_range = 0.2 * (workers + 1)
self.done = True
self.s_cnt = 0
self.goal_err = 0.08
self.ori_err = 0.3
self.quat_inv = False
self.goal_angle = []
self.object_pub = 0
self.set_model_pub = rospy.Publisher('/gazebo/set_model_state',
ModelState,
queue_size=1,
latch=True)
self.set_mode_pub = rospy.Publisher(self.__name + self.workers +
'/set_mode_msg',
String,
queue_size=1,
latch=True)
self.set_aabox_pub = rospy.Publisher('aa_box_pos',
aa_box_pos,
queue_size=1,
latch=True)
self.bridge = CvBridge()
self.depth_dim = 76800
self.depth_image = None
self.image_input = None
self.__bumper = None
self.__robot = "_arm"
self.images_ = []
self.aa_box_x = 0
self.aa_box_y = 0
# self.set_mode_pub.publish('set_mode')
rospy.Subscriber('/ir_depth/depth/image_raw', Image, self.callback)
self.seed(9)
# self.seed(345*(workers+1) + 467*(name+1))
self.reset(True)
## image (gazebo)
# rospy.Subscriber("odom", Odometry,self.get_aa_box_position)
# rospy.Subscriber("/bumper",ContactsState,self.Sub_Bumper)
self.arm_move = rospy.Publisher(self.__name + 'arm_move',
Float32MultiArray,
queue_size=1,
latch=True)
@property
def get_goal(self):
return self.goal
@property
def is_success(self):
return self.done
@property
def success_cnt(self):
return self.s_cnt
## image (gazebo)
# def get_aa_box_position(self,msg = None):
# print(msg.pose.pose.position.x)
# print(msg.pose.pose.position.y)
def aa_suck(self, data):
try:
self.aa_box_x = data.x
self.aa_box_y = data.y
except CvBridgeError as e:
print(e)
# def set_aa_box_vel(self, goal_x, goal_y):
def callback(self, data):
try:
# if len(data.data) == 0:
# print("data.data == 0")
tmp = self.bridge.imgmsg_to_cv2(data, "32FC1")
tmp = cv2.resize(tmp, (320, 240))
where_are_nan = np.isnan(tmp)
where_are_inf = np.isinf(tmp)
tmp[where_are_nan] = 10
tmp[where_are_inf] = 10
# print(tmp)
self.images_ = tmp / 10
# print("aaas")
# if self.images_.shape == (0,):
# rospy.logwarn("self.images_ = 0")
# print("aaaaaaaaaaa")
# print(self.images_)
# cv2.imshow('My Image', self.images_)
# cv2.waitKey(1)
except CvBridgeError as e:
print(e)
def get_state_client(self, name):
service = name + self.workers + '/get_state'
try:
rospy.wait_for_service(service, timeout=1.)
except rospy.ROSException as e:
rospy.logwarn('wait_for_service timeout')
self.get_state_client(name)
client = rospy.ServiceProxy(service, get_state)
# res = client(cmd)
res = client.call()
return res
def move_cmd_client(self, cmd, name):
service = name + self.workers + '/move_cmd'
try:
rospy.wait_for_service(service, timeout=1.)
except rospy.ROSException as e:
rospy.logwarn('wait_for_service timeout')
self.move_cmd_client(cmd, name)
client = rospy.ServiceProxy(service, move_cmd)
# res = client(cmd)
res = client.call(cmd)
return res
def set_start_client(self, cmd, rpy, name):
service = name + self.workers + '/set_start'
try:
rospy.wait_for_service(service, timeout=1.)
except rospy.ROSException as e:
rospy.logwarn('wait_for_service timeout')
self.set_start_client(cmd, rpy, name)
client = rospy.ServiceProxy(service, set_start)
# res = client(cmd)
res = client(action=cmd, rpy=rpy)
return res
def set_goal_client(self, cmd, rpy, name):
service = name + self.workers + '/set_goal'
try:
rospy.wait_for_service(service, timeout=1.)
except rospy.ROSException as e:
rospy.logwarn('wait_for_service timeout')
self.set_goal_client(cmd, rpy, name)
client = rospy.ServiceProxy(service, set_goal)
# res = client(cmd)
res = client(action=cmd, rpy=rpy)
return res
def set_object(self, name, pos, ori):
msg = ModelState()
msg.model_name = name
msg.pose.position.x = pos[0]
msg.pose.position.y = pos[1]
msg.pose.position.z = pos[2]
msg.pose.orientation.w = ori[0]
msg.pose.orientation.x = ori[1]
msg.pose.orientation.y = ori[2]
msg.pose.orientation.z = ori[3]
msg.reference_frame = 'world'
self.set_model_pub.publish(msg)
def set_object2(self, name, pos, ori):
msg = ModelState()
msg.model_name = name + self.workers
msg.pose.position.x = pos[0]
msg.pose.position.y = pos[1]
msg.pose.position.z = pos[2]
msg.pose.orientation.w = ori[0]
msg.pose.orientation.x = ori[1]
msg.pose.orientation.y = ori[2]
msg.pose.orientation.z = ori[3]
msg.reference_frame = 'world'
self.set_model_pub.publish(msg)
def seed(self, seed=None):
self.np_random, seed = seeding.np_random(seed)
return [seed]
def quat_angle(self):
cos_q = np.dot(self.goal[3:7], self.old[3:7])
if (self.quat_inv and cos_q > 0) or ((not self.quat_inv)
and cos_q <= 0):
cos_q = np.dot(-1 * self.goal[3:7], self.old[3:7])
return math.acos(cos_q) / pi
def move(self, goal):
self.goal = goal
res = self.get_state_client(self.__name)
res_ = self.get_state_client(self.__obname)
self.old, self.joint_pos[:15], self.joint_angle = np.array(
res.state), res.joint_pos, res.joint_angle
self.limit, self.goal_quat, self.quat_inv, self.joint_pos[
15:30] = res.limit, res.quaterniond, res.quat_inv, res_.joint_pos
linkPosM, linkPosS = self.collision_init()
alarm, Link_dis = self.cc.checkCollision(linkPosM, linkPosS)
s = np.append(self.old[:3], np.subtract(self.goal[:3], self.old[:3]))
s = np.append(s, Link_dis)
s = np.append(s, self.joint_angle)
s = np.append(s, self.limit[0])
self.dis_pos = np.linalg.norm(self.goal[:3] - s[:3])
s = np.append(s, self.dis_pos)
self.state = s
return s
def reset(self, reset_start):
# self.old, self.joint_pos[:15], self.joint_pos[15:30], self.joint_angle, self.limit, self.goal_quat, self.quat_inv = self.set_old()
# linkPosM, linkPosS = self.collision_init()
# alarm, Link_dis = self.cc.checkCollision(linkPosM, linkPosS)
# alarm_cnt = 0
# for i in alarm:
# alarm_cnt += i
# if alarm_cnt>0:
# return self.reset()
# joint_pos_tmp = self.joint_pos
# self.goal, self.goal_angle, self.joint_pos[:15], self.joint_pos[15:30]= self.set_goal()
# linkPosM, linkPosS = self.collision_init()
# alarm, Link_dis = self.cc.checkCollision(linkPosM, linkPosS)
# alarm_cnt = 0
# for i in alarm:
# alarm_cnt += i
# if alarm_cnt>0:
# return self.reset()
if reset_start:
self.old, self.joint_pos[:15], self.joint_pos[
15:
30], self.joint_angle, self.limit, self.goal_quat, self.quat_inv = self.set_old(
)
linkPosM, linkPosS = self.collision_init()
alarm, Link_dis = self.cc.checkCollision(linkPosM, linkPosS)
alarm_cnt = 0
for i in alarm:
alarm_cnt += i
if alarm_cnt > 0:
return self.reset(reset_start)
joint_pos_tmp = self.joint_pos
self.goal, self.goal_angle, self.joint_pos[:15], self.joint_pos[
15:30] = self.set_goal()
linkPosM, linkPosS = self.collision_init()
alarm, Link_dis = self.cc.checkCollision(linkPosM, linkPosS)
alarm_cnt = 0
for i in alarm:
alarm_cnt += i
self.joint_pos = joint_pos_tmp
self.state = np.append(self.old,
np.subtract(self.goal[:3], self.old[:3]))
self.state = np.append(self.state, self.goal_quat[:4])
# self.state = np.append(self.state, np.subtract(self.goal[3:7], self.old[3:7]))
# self.state = np.append(self.state, np.subtract(-1*self.goal[3:7], self.old[3:7]))
self.state = np.append(self.state, Link_dis)
self.state = np.append(self.state, self.joint_angle)
self.state = np.append(self.state, self.limit)
self.dis_pos = np.linalg.norm(self.goal[:3] - self.old[:3])
self.dis_ori = math.sqrt(
np.linalg.norm(self.goal[3:7] - self.old[3:7]) +
np.linalg.norm(-1 * self.goal[3:7] - self.old[3:7]) - 2)
# self.angle_ori = self.quat_angle()
self.state = np.append(self.state, self.dis_pos)
self.state = np.append(self.state, self.dis_ori)
self.state = np.append(self.state, self.joint_pos[6:12])
self.state = np.append(self.state, self.goal_angle)
# print("aaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaa")
# print(self.images_)
# self.img_suckkkkkkkkkkkkk = np.reshape(self.images_,-1)
# print("aaa")
# print(self.img_suckkkkkkkkkkkkk.shape)
# print("bbb")
# self.state = np.append(self.state, self.img_suckkkkkkkkkkkkk)
if self.__name == '/right_':
# self.aa_box_x = random.uniform(0.15,0.3)
# self.aa_box_y = random.uniform(-0.5,0.5)
self.aa_box_x = random.uniform(0.1, 0.3)
self.aa_box_y = random.uniform(-0.5, 0.5)
# self.aa_box_y = random.uniform(-0.5,-0.1)
self.set_object('table_box', (0.55, 0, 0.345), (0, 0, 0, 0))
self.set_object('aa_box', (self.aa_box_x, self.aa_box_y, 0.8),
(0, 0, 0, 0))
aa_box = aa_box_pos()
aa_box.x = self.aa_box_x
aa_box.y = self.aa_box_y
self.set_aabox_pub.publish(aa_box)
# rospy.Publisher('aa_box_pos',aa_box_pos)
# print(self.aa_box_x,"III",self.aa_box_x,self.aa_box_y)
# self.set_object2(self.__name, (self.goal[0]-0.08, self.goal[1], self.goal[2]+1.45086), (0, 0, 0, 0))
time.sleep(0.5)
linkPosM, linkPosS = self.collision_init()
alarm, Link_dis = self.cc.checkCollision(linkPosM, linkPosS)
alarm_cnt = 0
for i in alarm:
alarm_cnt += i
if alarm_cnt > 0:
return self.reset(reset_start)
arm_x = 0.08
arm_r_y = 1
arm_l_y = -1
arm_z = -1.4125
if self.goal[
0] < self.aa_box_x + 0.05 + arm_x and self.aa_box_x - 0.05 + arm_x < self.goal[
0] and self.goal[1] < (
self.aa_box_y + 0.05) * arm_r_y and (
self.aa_box_y + 0.05) * arm_r_y < self.goal[1]:
return self.reset(reset_start)
rospy.Subscriber('aa_box_pos', aa_box_pos, self.aa_suck)
if self.__name == '/right_':
self.aa_box_possition = [
self.aa_box_x + 0.05 + arm_x, (self.aa_box_y + 0.05) * arm_r_y,
0.975 + arm_z + 0.125, self.aa_box_x + 0.05 + arm_x,
(self.aa_box_y - 0.05) * arm_r_y, 0.975 + arm_z + 0.125,
self.aa_box_x - 0.05 + arm_x, (self.aa_box_y + 0.05) * arm_r_y,
0.975 + arm_z + 0.125, self.aa_box_x - 0.05 + arm_x,
(self.aa_box_y - 0.05) * arm_r_y, 0.975 + arm_z + 0.125,
self.aa_box_x + 0.05 + arm_x, (self.aa_box_y + 0.05) * arm_r_y,
0.625 + arm_z, self.aa_box_x + 0.05 + arm_x,
(self.aa_box_y - 0.05) * arm_r_y, 0.625 + arm_z,
self.aa_box_x - 0.05 + arm_x, (self.aa_box_y + 0.05) * arm_r_y,
0.625 + arm_z, self.aa_box_x - 0.05 + arm_x,
(self.aa_box_y - 0.05) * arm_r_y, 0.625 + arm_z
]
if self.__name == '/left_':
self.aa_box_possition = [
self.aa_box_x + 0.05 + arm_x, (self.aa_box_y + 0.05) * arm_l_y,
0.975 + arm_z + 0.125, self.aa_box_x + 0.05 + arm_x,
(self.aa_box_y - 0.05) * arm_l_y, 0.975 + arm_z + 0.125,
self.aa_box_x - 0.05 + arm_x, (self.aa_box_y + 0.05) * arm_l_y,
0.975 + arm_z + 0.125, self.aa_box_x - 0.05 + arm_x,
(self.aa_box_y - 0.05) * arm_l_y, 0.975 + arm_z + 0.125,
self.aa_box_x + 0.05 + arm_x, (self.aa_box_y + 0.05) * arm_l_y,
0.625 + arm_z, self.aa_box_x + 0.05 + arm_x,
(self.aa_box_y - 0.05) * arm_l_y, 0.625 + arm_z,
self.aa_box_x - 0.05 + arm_x, (self.aa_box_y + 0.05) * arm_l_y,
0.625 + arm_z, self.aa_box_x - 0.05 + arm_x,
(self.aa_box_y - 0.05) * arm_l_y, 0.625 + arm_z
]
self.state = np.append(self.state, self.aa_box_possition)
aa_box_possssss = np.array([self.aa_box_x, self.aa_box_y, 0.8])
self.dis_obstacle_1 = np.linalg.norm(aa_box_possssss[:3] -
self.joint_pos[6:9])
self.dis_obstacle_2 = np.linalg.norm(aa_box_possssss[:3] -
self.joint_pos[9:12])
self.state = np.append(self.state, self.dis_obstacle_1)
self.state = np.append(self.state, self.dis_obstacle_2)
self.collision = False
self.done = False
self.success = False
return self.state
def set_goal(self):
self.goal = self.np_random.uniform(low=-0.5, high=0.5, size=(8, ))
self.goal[1] = self.np_random.uniform(low=0.1, high=0.5)
# self.goal = self.np_random.uniform(low=-0.5, high=0.5, size=(8,))
# self.goal[1] = self.np_random.uniform(low=0.1, high=0.35)
# self.goal[1] = 0.3
if self.__name == '/left_':
self.goal[1] = self.np_random.uniform(low=-0.5, high=-0.1)
self.goal[2] = self.np_random.uniform(low=-0.3, high=0.5)
rpy = self.np_random.uniform(low=-1 * self.rpy_range,
high=self.rpy_range,
size=(4, ))
# if self.__name == '/left_':
# self.goal[1] = self.np_random.uniform(low=-0.35, high=-0.1)
# self.goal[2] = self.np_random.uniform(low=-0.2, high=0.5)
# rpy = self.np_random.uniform(low=-1*self.rpy_range, high=self.rpy_range, size=(4,))
# print('self.goal = ', self.goal)
# if self.goal[0]>0.5:
# if self.goal[0]>0.75:
# self.goal[2] /= -3
# else:
# self.goal[2] /= -2
# self.goal[2]+=1
self.goal[0] = 0
# self.goal[3] = self.range_cnt/2
self.goal = np.append(self.goal, self.range_cnt)
res = self.set_goal_client(self.goal, rpy, self.__name)
res_ = self.get_state_client(self.__obname)
goal_pos = np.array(res.state)
if not res.success:
return self.set_goal()
else:
return goal_pos[:7], res.joint_angle, res.joint_pos, res_.joint_pos
def set_old(self):
# self.start = self.np_random.uniform(low=-0.5, high=0.5, size=(8,))
# rpy = self.np_random.uniform(low=-1*self.rpy_range, high=self.rpy_range, size=(4,))
self.start = self.np_random.uniform(low=-0.5, high=0.5, size=(8, ))
self.start[1] = self.np_random.uniform(low=0.1, high=0.5)
if self.__name == '/left_':
self.start[1] = self.np_random.uniform(low=-0.5, high=-0.1)
self.start[2] = self.np_random.uniform(low=-0.3, high=0.5)
rpy = self.np_random.uniform(low=-1 * self.rpy_range,
high=self.rpy_range,
size=(4, ))
# if self.start[0]>0.5:
# if self.start[0]>0.75:
# self.start[2] /= -3
# else:
# self.start[2] /= -2
# self.start[2] +=1
self.start[0] = 0
# self.start[3] = self.range_cnt/2
self.start = np.append(self.start, self.range_cnt)
res = self.set_start_client(self.start, rpy, self.__name)
res_ = self.get_state_client(self.__obname)
old_pos = np.array(res.state)
if not res.success:
return self.set_old()
else:
return old_pos, res.joint_pos, res_.joint_pos, res.joint_angle, res.limit, res.quaterniond, res.quat_inv
def collision_init(self):
linkPosM = np.array(self.joint_pos[:15])
linkPosS = np.array(self.joint_pos[15:])
linkPosM = np.append([0., 0., -0.8], linkPosM)
linkPosS = np.append([0., 0., -0.8], linkPosS)
linkPosM = linkPosM.reshape(6, 3)
linkPosS = linkPosS.reshape(6, 3)
return linkPosM, linkPosS
def move_arm(self, cmd):
self.move_cmd_client(cmd, self.__name)
def check_collision(self):
res = self.get_state_client(self.__name)
res_ = self.get_state_client(self.__obname)
self.joint_pos[:15], self.joint_pos[
15:30] = res.joint_pos, res_.joint_pos
linkPosM, linkPosS = self.collision_init()
alarm, _ = self.cc.checkCollision(linkPosM, linkPosS)
alarm_cnt = 0
for i in alarm:
alarm_cnt += i
if alarm_cnt > 0.4:
return True
else:
return False
def step(self, a):
alarm = []
Link_dis = []
bumpalarm = []
s = self.state
suck = self.image_input
self.collision = False
action_vec = a[:3] * self.ACTION_VEC_TRANS
action_ori = a[3:7] * self.ACTION_ORI_TRANS
action_phi = a[7] * self.ACTION_PHI_TRANS
self.action = np.append(action_vec, action_ori)
self.action = np.append(self.action, action_phi)
self.cmd = np.add(s[:8], self.action)
self.cmd[3:7] /= np.linalg.norm(self.cmd[3:7])
res = self.move_cmd_client(self.cmd, self.__name)
res_ = self.get_state_client(self.__obname)
if res.success:
self.old, self.joint_pos[:15], self.joint_angle = np.array(
res.state), res.joint_pos, res.joint_angle
self.limit, self.goal_quat, self.quat_inv, self.joint_pos[
15:
30] = res.limit, res.quaterniond, res.quat_inv, res_.joint_pos
linkPosM, linkPosS = self.collision_init()
alarm, Link_dis = self.cc.checkCollision(linkPosM, linkPosS)
s = np.append(self.old, np.subtract(self.goal[:3], self.old[:3]))
s = np.append(s, self.goal_quat[:4])
# s = np.append(s, np.subtract(self.goal[3:7], self.old[3:7]))
# s = np.append(s, np.subtract(-1*self.goal[3:7], self.old[3:7]))
s = np.append(s, Link_dis)
s = np.append(s, self.joint_angle)
s = np.append(s, self.limit)
self.dis_pos = np.linalg.norm(self.goal[:3] - s[:3])
self.dis_ori = math.sqrt(
np.linalg.norm(self.goal[3:7] - s[3:7]) +
np.linalg.norm(-1 * self.goal[3:7] - s[3:7]) - 2)
# self.angle_ori = self.quat_angle()
s = np.append(s, self.dis_pos)
s = np.append(s, self.dis_ori)
s = np.append(s, self.joint_pos[6:12])
s = np.append(s, self.goal_angle)
rospy.Subscriber('aa_box_pos', aa_box_pos, self.aa_suck)
# print(self.aa_box_x,"III",self.aa_box_y)
# if self.__name == '/right_':
self.set_object('table_box', (0.55, 0, 0.345), (0, 0, 0, 0))
self.set_object('aa_box', (self.aa_box_x, self.aa_box_y, 0.82691),
(0, 0, 0, 0))
# self.image_input = np.reshape(self.images_,-1)
# print(self.image_cnn[10])
arm_x = 0.08
arm_r_y = 1
arm_l_y = -1
arm_z = -1.4125
if self.__name == '/right_':
self.aa_box_possition = [
self.aa_box_x + 0.05 + arm_x,
(self.aa_box_y + 0.05) * arm_r_y,
0.975 + arm_z + 0.125, self.aa_box_x + 0.05 + arm_x,
(self.aa_box_y - 0.05) * arm_r_y,
0.975 + arm_z + 0.125, self.aa_box_x - 0.05 + arm_x,
(self.aa_box_y + 0.05) * arm_r_y,
0.975 + arm_z + 0.125, self.aa_box_x - 0.05 + arm_x,
(self.aa_box_y - 0.05) * arm_r_y,
0.975 + arm_z + 0.125, self.aa_box_x + 0.05 + arm_x,
(self.aa_box_y + 0.05) * arm_r_y,
0.625 + arm_z, self.aa_box_x + 0.05 + arm_x,
(self.aa_box_y - 0.05) * arm_r_y,
0.625 + arm_z, self.aa_box_x - 0.05 + arm_x,
(self.aa_box_y + 0.05) * arm_r_y,
0.625 + arm_z, self.aa_box_x - 0.05 + arm_x,
(self.aa_box_y - 0.05) * arm_r_y, 0.625 + arm_z
]
if self.__name == '/left_':
self.aa_box_possition = [
self.aa_box_x + 0.05 + arm_x,
(self.aa_box_y + 0.05) * arm_l_y,
0.975 + arm_z, self.aa_box_x + 0.05 + arm_x,
(self.aa_box_y - 0.05) * arm_l_y,
0.975 + arm_z, self.aa_box_x - 0.05 + arm_x,
(self.aa_box_y + 0.05) * arm_l_y,
0.975 + arm_z, self.aa_box_x - 0.05 + arm_x,
(self.aa_box_y - 0.05) * arm_l_y,
0.975 + arm_z, self.aa_box_x + 0.05 + arm_x,
(self.aa_box_y + 0.05) * arm_l_y,
0.625 + arm_z, self.aa_box_x + 0.05 + arm_x,
(self.aa_box_y - 0.05) * arm_l_y,
0.625 + arm_z, self.aa_box_x - 0.05 + arm_x,
(self.aa_box_y + 0.05) * arm_l_y,
0.625 + arm_z, self.aa_box_x - 0.05 + arm_x,
(self.aa_box_y - 0.05) * arm_l_y, 0.625 + arm_z
]
s = np.append(s, self.aa_box_possition)
aa_box_possssss = np.array([self.aa_box_x, self.aa_box_y, 0.8])
self.dis_obstacle_1 = np.linalg.norm(aa_box_possssss[:3] -
self.joint_pos[6:9])
self.dis_obstacle_2 = np.linalg.norm(aa_box_possssss[:3] -
self.joint_pos[9:12])
# suck = np.append(suck, self.image_input)
s = np.append(s, self.dis_obstacle_1)
s = np.append(s, self.dis_obstacle_2)
# s = np.append(s, self.image_input)
# print(suck)
# print(s)
terminal = self._terminal(s, res.success, alarm)
alarm_cnt = 0
# for i in bumpalarm:
# alarm_cnt += i
# if alarm_cnt>0.4:
# terminal = True
# reward = self.get_reward(s, res.success, terminal, res.singularity)
# self.images_ = suck
if (not self.collision) and math.fabs(s[7]) < 0.9:
self.state = s
## see
if self.workers == 'arm':
if self.object_pub == 0:
self.set_object2(self.__name,
(self.goal[0] - 0.08, self.goal[1],
self.goal[2] + 1.45086), (0, 0, 0, 0))
self.object_pub = 1
else:
self.set_object2(self.__name + 'q',
(self.goal[0] - 0.08, self.goal[1],
self.goal[2] + 1.45086), self.goal[3:7])
self.object_pub = 0
fail = False
# if not res.success or self.collision or res.singularity:
# fail = True
linkPosM, linkPosS = self.collision_init()
alarm, Link_dis = self.cc.checkCollision(linkPosM, linkPosS)
alarm_cnt = 0
for i in alarm:
alarm_cnt += i
if alarm_cnt > 0.4:
fail = True
self.collision = True
# return self.state, reward, terminal, self.success, fail
return self.state, terminal, self.collision, res.success, res.singularity
def _terminal(self, s, ik_success, alarm):
alarm_cnt = 0
for i in alarm:
alarm_cnt += i
if alarm_cnt > 0.4:
self.collision = True
if ik_success and not self.collision:
if self.dis_pos < 0.05:
if not self.done:
self.done = True
self.s_cnt += 1
return True
else:
return False
else:
return False
class Test(core.Env):
dt = .2
LINK_LENGTH_1 = 1. # [m]
LINK_LENGTH_2 = 1. # [m]
LINK_MASS_1 = 1. #: [kg] mass of link 1
LINK_MASS_2 = 1. #: [kg] mass of link 2
LINK_COM_POS_1 = 0.5 #: [m] position of the center of mass of link 1
LINK_COM_POS_2 = 0.5 #: [m] position of the center of mass of link 2
LINK_MOI = 1. #: moments of inertia for both links
MAX_VEL_1 = 2.
MAX_VEL_2 = 2.
MAX_VEL_3 = 2.
ACTION_VEC_TRANS = 1/240
ACTION_ORI_TRANS = 1/240
ACTION_PHI_TRANS = 1/240
NAME = ['/right_arm', '/left_arm', '/right_arm']
torque_noise_max = 0.
#: use dynamics equations from the nips paper or the book
book_or_nips = "book"
action_arrow = None
domain_fig = None
actions_num = 8
def __init__(self, name):
self.__name = self.NAME[name%2]
self.__obname = self.NAME[name%2 + 1]
self.viewer = None
high = np.array([1.,1.,1.,1.,1.,1.,1.,1., #8
1.,1.,1.,1.,1.,1.,1., #7
0.,0.,0.,0.,0.,0., #6
0.,0.,0.,0.,0.,0.,0.,0.,0.,#9
0.,0.,0., #2
0.,0.,0.]) #1
#24
low = -1*high
# ox,oy,oz,oa,ob,oc,od,of,
# vx,vy,vz,va,vb,vc,vd,vf
# dis of Link(15)
#
# joint_angle(7),
# limit(1), rate(3)
self.observation_space = spaces.Box(low=low, high=high, dtype=np.float32)
self.action_space = spaces.Discrete(3)
self.act_dim=7
self.obs_dim=39
self.state = []
self.action = []
self.cmd = []
self.point = []
self.goal = []
self.goal_pos = []
self.goal_quat = []
self.goal_phi = 0
self.goal_rpy = []
self.old = []
self.old_pos = []
self.old_quat = []
self.old_phi = 0
self.joint_pos = []
self.joint_angle = []
self.limit = []
self.s_jointpos = []
# self.dis_pos
self.cc = CheckCollision()
self.collision = False
self.range_cnt = 0.8
self.rpy_range = 0.1
self.s_cnt = 0
self.done = True
self.goal_err = 0.08
self.ori_err = 0.24
self.quat_inv = False
self.set_mode_pub = rospy.Publisher(
'/gazebo/set_model_state',
ModelState,
queue_size=1,
latch=True
)
self.seed()
self.reset()
def get_state_client(self, name):
service = name+'/get_state'
try:
rospy.wait_for_service(service, timeout=1.)
except rospy.ROSException as e:
rospy.logwarn('wait_for_service timeout')
self.get_state_client(name)
client = rospy.ServiceProxy(
service,
get_state
)
# res = client(cmd)
res = client.call()
return res
def move_cmd_client(self, cmd, name):
service = name+'/move_cmd'
try:
rospy.wait_for_service(service, timeout=1.)
except rospy.ROSException as e:
rospy.logwarn('wait_for_service timeout')
self.move_cmd_client(cmd, name)
client = rospy.ServiceProxy(
service,
move_cmd
)
# res = client(cmd)
res = client.call(cmd)
return res
def set_start_client(self, cmd, rpy, name):
service = name+'/set_start'
try:
rospy.wait_for_service(service, timeout=1.)
except rospy.ROSException as e:
rospy.logwarn('wait_for_service timeout')
self.set_start_client(cmd, rpy, name)
client = rospy.ServiceProxy(
service,
set_start
)
# res = client(cmd)
res = client(action=cmd, rpy=rpy)
return res
def set_goal_client(self, cmd, rpy, name):
service = name+'/set_goal'
try:
rospy.wait_for_service(service, timeout=1.)
except rospy.ROSException as e:
rospy.logwarn('wait_for_service timeout')
self.set_goal_client(cmd, rpy, name)
client = rospy.ServiceProxy(
service,
set_goal
)
# res = client(cmd)
res = client(action=cmd, rpy=rpy)
return res
def set_object(self, name, pos, ori):
msg = ModelState()
msg.model_name = name
msg.pose.position.x = pos[0]
msg.pose.position.y = pos[1]
msg.pose.position.z = pos[2]
msg.pose.orientation.w = ori[0]
msg.pose.orientation.x = ori[1]
msg.pose.orientation.y = ori[2]
msg.pose.orientation.z = ori[3]
msg.reference_frame = 'world'
self.set_mode_pub.publish(msg)
def seed(self, seed=None):
self.np_random, seed = seeding.np_random(seed)
return [seed]
def quat_angle(self):
cos_q = np.dot(self.goal[3:7], self.old[3:7])
if (self.quat_inv and cos_q>0) or ((not self.quat_inv) and cos_q<=0):
cos_q = np.dot(-1*self.goal[3:7], self.old[3:7])
return math.acos(cos_q)/pi
def move(self, goal):
self.goal = goal
res = self.get_state_client(self.__name)
res_ = self.get_state_client(self.__obname)
self.old, self.joint_pos[:15], self.joint_angle = np.array(res.state), res.joint_pos, res.joint_angle
self.limit, self.goal_quat, self.quat_inv, self.joint_pos[15:30] = res.limit, res.quaterniond, res.quat_inv, res_.joint_pos
linkPosM, linkPosS = self.collision_init()
_, Link_dis = self.cc.checkCollision(linkPosM, linkPosS)
s = np.append(self.old[:3], np.subtract(self.goal[:3], self.old[:3]))
s = np.append(s, Link_dis)
s = np.append(s, self.joint_angle)
s = np.append(s, self.limit[0])
self.dis_pos = np.linalg.norm(self.goal[:3] - s[:3])
s = np.append(s, self.dis_pos)
self.state = s
return s
def reset(self):
# if self.done:
self.goal = self.set_goal()
self.old, self.joint_pos[:15], self.joint_pos[15:30], self.joint_angle, self.limit, self.goal_quat, self.quat_inv = self.set_old()
linkPosM, linkPosS = self.collision_init()
alarm, Link_dis = self.cc.checkCollision(linkPosM, linkPosS)
alarm_cnt = 0
for i in alarm:
alarm_cnt += i
if alarm_cnt>0:
print('fuckfuck')
return self.reset()
self.state = np.append(self.old, np.subtract(self.goal[:3], self.old[:3]))
self.state = np.append(self.state, self.goal_quat)
self.state = np.append(self.state, Link_dis)
self.state = np.append(self.state, self.joint_angle)
self.state = np.append(self.state, self.limit)
self.dis_pos = np.linalg.norm(self.goal[:3] - self.old[:3])
# self.dis_ori = np.linalg.norm(self.goal[3:7] - self.old[3:7])
self.angle_ori = self.quat_angle()
self.state = np.append(self.state, self.dis_pos)
self.state = np.append(self.state, self.angle_ori)
self.state = np.append(self.state, self.joint_pos[9:12])
self.collision = False
self.done = False
return self.state
def set_goal(self):
self.goal = self.np_random.uniform(low=0., high=self.range_cnt, size=(8,))
rpy = self.np_random.uniform(low=-1*self.rpy_range, high=self.rpy_range, size=(3,))
# print('self.goal = ', self.goal)
self.goal[0] = 0
self.goal[3] = self.range_cnt/2
self.goal = np.append(self.goal, self.range_cnt)
res = self.set_goal_client(self.goal, rpy, self.__name)
goal_pos = np.array(res.state)
if not res.success:
return self.set_goal()
if np.linalg.norm(goal_pos[:2])>0.2:
return goal_pos[:7]
else:
return self.set_goal()
def set_old(self):
self.start = self.np_random.uniform(low=0., high=self.range_cnt, size=(8,))
rpy = self.np_random.uniform(low=-1*self.rpy_range, high=self.rpy_range, size=(3,))
self.start[0] = 0
self.start[3] = self.range_cnt/2
self.start = np.append(self.start, self.range_cnt)
res = self.set_start_client(self.start, rpy, self.__name)
res_ = self.get_state_client(self.__obname)
old_pos = np.array(res.state)
if not res.success:
return self.set_old()
if np.linalg.norm(np.subtract(old_pos[:3], self.goal[:3])) > 0.1:
return old_pos, res.joint_pos, res_.joint_pos, res.joint_angle, res.limit, res.quaterniond, res.quat_inv
else:
return self.set_old()
def collision_init(self):
linkPosM = np.array(self.joint_pos[:15])
linkPosS = np.array(self.joint_pos[15:])
linkPosM = np.append([0.,0.,-0.8], linkPosM)
linkPosS = np.append([0.,0.,-0.8], linkPosS)
linkPosM = linkPosM.reshape(6,3)
linkPosS = linkPosS.reshape(6,3)
return linkPosM, linkPosS
def step(self, a):
alarm = []
Link_dis = []
s = self.state
self.collision = False
action_vec = a[:3]*self.ACTION_VEC_TRANS
action_ori = a[3:7]*self.ACTION_ORI_TRANS
# action_phi = a[7]*self.ACTION_PHI_TRANS
self.action = np.append(action_vec, action_ori)
self.action = np.append(self.action, [0])
self.cmd = np.add(s[:8], self.action)
self.cmd[3:7] /= np.linalg.norm(self.cmd[3:7])
res = self.move_cmd_client(self.cmd, self.__name)
res_ = self.get_state_client(self.__obname)
if res.success:
self.old, self.joint_pos[:15], self.joint_angle = np.array(res.state), res.joint_pos, res.joint_angle
self.limit, self.goal_quat, self.quat_inv, self.joint_pos[15:30] = res.limit, res.quaterniond, res.quat_inv, res_.joint_pos
linkPosM, linkPosS = self.collision_init()
alarm, Link_dis = self.cc.checkCollision(linkPosM, linkPosS)
s = np.append(self.old, np.subtract(self.goal[:3], self.old[:3]))
s = np.append(s, self.goal_quat)
s = np.append(s, Link_dis)
s = np.append(s, self.joint_angle)
s = np.append(s, self.limit)
self.dis_pos = np.linalg.norm(self.goal[:3] - s[:3])
self.angle_ori = self.quat_angle()
s = np.append(s, self.dis_pos)
s = np.append(s, self.angle_ori)
s = np.append(s, self.joint_pos[9:12])
terminal = self._terminal(s, res.success, alarm)
reward = self.get_reward(s, res.success, terminal)
if not self.collision:
self.state = s
self.set_object(self.__name, (self.goal[0]-0.08, self.goal[1], self.goal[2]+1.45086), (0, 0, 0, 0))
return self.state, reward, terminal, self.collision
def _terminal(self, s, ik_success, alarm):
alarm_cnt = 0
for i in alarm:
alarm_cnt += i
if alarm_cnt>0.5:
self.collision = True
if ik_success and not self.collision:
if self.dis_pos < self.goal_err and self.angle_ori < self.ori_err:
if not self.done:
self.done = True
self.s_cnt += 1
self.range_cnt = self.range_cnt + 0.003 if self.range_cnt < 0.95 else 0.95
self.rpy_range = self.rpy_range + 0.001 if self.rpy_range < 0.45 else 0.45
self.goal_err = self.goal_err*0.995 if self.goal_err > 0.01 else 0.01
self.ori_err = self.ori_err*0.995 if self.ori_err > 0.03 else 0.03
print('ssssssuuuuuuccccccccceeeeeeeesssssssssss' , self.s_cnt, self.goal_err)
return True
else:
return False
else:
return False
def get_reward(self, s, ik_success, terminal):
reward = 0.
if not ik_success:
return -3
if self.collision:
return -5
reward -= self.dis_pos
reward -= self.angle_ori
reward += 0.4
if reward > 0:
reward *= 3
cos_vec = np.dot(self.action[:3], self.state[8:11])/(np.linalg.norm(self.action[:3]) *np.linalg.norm(self.state[8:11]))
reward += (cos_vec*self.dis_pos - self.dis_pos)
# reward += (cos_ori*self.dis_ori/2 - self.dis_ori/2)
reward -= 2
# if terminal and ik_success and not self.collision:
# reward += 1
# if self.dis_pos < 0.04:
# reward += 1
# if self.dis_pos < 0.1:
# reward += 1
return reward
class Test(core.Env):
ACTION_VEC_TRANS = 1 / 300
ACTION_ORI_TRANS = 1 / 100
ACTION_PHI_TRANS = 1 / 100
NAME = ['/right_', '/left_', '/right_']
def __init__(self, name, workers):
self.__name = self.NAME[name % 2]
self.__obname = self.NAME[name % 2 + 1]
if workers == 0:
self.workers = 'arm'
else:
self.workers = str(workers)
high = np.array([
1.,
1.,
1.,
1.,
1.,
1.,
1.,
1., #8
1.,
1.,
1.,
1.,
1.,
1.,
1.,
1.,
1.,
1.,
1., #7
0.,
0.,
0.,
0.,
0.,
0., #6
0.,
0.,
0.,
0.,
0.,
0.,
0.,
0.,
0., #9
0.,
0.,
0.,
0.,
0.,
0.,
0.,
0.,
0., #2
0.,
0.,
0.,
0.,
0.,
0.
]) #1
#24
low = -1 * high
# ox,oy,oz,oa,ob,oc,od,of,
# vx,vy,vz,va,vb,vc,vd,vf
# dis of Link(15)
#
# joint_angle(7),
# limit(1), rate(3)
self.observation_space = spaces.Box(low=low,
high=high,
dtype=np.float32)
self.action_space = spaces.Discrete(3)
self.act_dim = 8
self.obs_dim = 61
self.state = []
self.action = []
self.cmd = []
self.point = []
self.goal = []
self.goal_pos = []
self.goal_quat = []
self.goal_phi = 0
self.goal_rpy = []
self.old = []
self.old_pos = []
self.old_quat = []
self.old_phi = 0
self.joint_pos = []
self.joint_angle = []
self.limit = []
self.s_jointpos = []
# self.dis_pos
self.cc = CheckCollision()
self.collision = False
self.range_cnt = 0.7
self.rpy_range = 1
self.done = True
self.s_cnt = 0
self.goal_err = 0.03
self.ori_err = 0.25
self.quat_inv = False
self.goal_angle = []
self.object_pub = 0
self.curr_angle = []
self.move_speed = 0.1
self.set_mode_pub = rospy.Publisher('/gazebo/set_model_state',
ModelState,
queue_size=1,
latch=True)
self.seed(254 * (name + 1))
@property
def is_success(self):
return self.done
@property
def success_cnt(self):
return self.s_cnt
@property
def get_goal(self):
return self.goal
def get_state_client(self, name):
service = name + self.workers + '/get_state'
try:
rospy.wait_for_service(service, timeout=1.)
except rospy.ROSException as e:
rospy.logwarn('wait_for_service timeout')
self.get_state_client(name)
client = rospy.ServiceProxy(service, get_state)
# res = client(cmd)
res = client.call()
return res
def move_cmd_client(self, cmd, name):
service = name + self.workers + '/move_cmd'
try:
rospy.wait_for_service(service, timeout=1.)
except rospy.ROSException as e:
rospy.logwarn('wait_for_service timeout')
self.move_cmd_client(cmd, name)
client = rospy.ServiceProxy(service, move_cmd)
# res = client(cmd)
res = client.call(cmd)
return res
def move_init_client(self, cmd, name):
service = name + self.workers + '/move_init'
try:
rospy.wait_for_service(service, timeout=1.)
except rospy.ROSException as e:
rospy.logwarn('wait_for_service timeout')
self.move_cmd_client(cmd, name)
client = rospy.ServiceProxy(service, move_init)
# res = client(cmd)
res = client.call(cmd)
return res
def set_start_client(self, cmd, rpy, name):
service = name + self.workers + '/set_start'
try:
rospy.wait_for_service(service, timeout=1.)
except rospy.ROSException as e:
rospy.logwarn('wait_for_service timeout')
self.set_start_client(cmd, rpy, name)
client = rospy.ServiceProxy(service, set_start)
# res = client(cmd)
res = client(action=cmd, rpy=rpy)
return res
def set_goal_client(self, cmd, rpy, name):
service = name + self.workers + '/set_goal'
try:
rospy.wait_for_service(service, timeout=1.)
except rospy.ROSException as e:
rospy.logwarn('wait_for_service timeout')
self.set_goal_client(cmd, rpy, name)
client = rospy.ServiceProxy(service, set_goal)
# res = client(cmd)
res = client(action=cmd, rpy=rpy)
return res
def set_object(self, name, pos, ori):
msg = ModelState()
msg.model_name = name + self.workers
msg.pose.position.x = pos[0]
msg.pose.position.y = pos[1]
msg.pose.position.z = pos[2]
msg.pose.orientation.w = ori[0]
msg.pose.orientation.x = ori[1]
msg.pose.orientation.y = ori[2]
msg.pose.orientation.z = ori[3]
msg.reference_frame = 'world'
self.set_mode_pub.publish(msg)
def seed(self, seed=None):
self.np_random, seed = seeding.np_random(seed)
return [seed]
def quat_angle(self):
cos_q = np.dot(self.goal[3:7], self.old[3:7])
if (self.quat_inv and cos_q > 0) or ((not self.quat_inv)
and cos_q <= 0):
cos_q = np.dot(-1 * self.goal[3:7], self.old[3:7])
return math.acos(cos_q) / pi
def reset(self, cmd):
self.move_speed = cmd[8] / 100
cmd[7] = cmd[7] * 2 / pi
res = self.move_init_client(cmd, self.__name)
if not res.enable:
return [], False, True, res.enable
_res = self.get_state_client(self.__name)
res_ = self.get_state_client(self.__obname)
self.goal, self.goal_angle, = np.array(res.state)[:7], res.joint_angle
self.old, self.joint_pos[:15], self.joint_pos[
15:30] = _res.state, _res.joint_pos, res_.joint_pos
self.joint_angle, self.limit, self.goal_quat = res.joint_angle, res.limit, res.quaterniond
linkPosM, linkPosS = self.collision_init()
alarm, Link_dis = self.cc.checkCollision(linkPosM, linkPosS)
alarm_cnt = 0
for i in alarm:
alarm_cnt += i
self.state = np.append(self.old,
np.subtract(self.goal[:3], self.old[:3]))
self.state = np.append(self.state, self.goal_quat)
self.state = np.append(self.state, Link_dis)
self.state = np.append(self.state, self.joint_angle)
self.state = np.append(self.state, self.limit)
self.dis_pos = np.linalg.norm(self.goal[:3] - self.old[:3])
self.dis_ori = math.sqrt(
np.linalg.norm(self.goal[3:7] - self.old[3:7]) +
np.linalg.norm(-1 * self.goal[3:7] - self.old[3:7]) - 2)
self.state = np.append(self.state, self.dis_pos)
self.state = np.append(self.state, self.dis_ori)
self.state = np.append(self.state, self.joint_pos[6:12])
self.state = np.append(self.state, self.goal_angle)
self.collision = False
self.done = False
if alarm_cnt > 0:
self.collision = True
return self.state, res.success, self.collision, res.enable
def collision_init(self):
linkPosM = np.array(self.joint_pos[:15])
linkPosS = np.array(self.joint_pos[15:])
linkPosM = np.append([0., 0., -0.8], linkPosM)
linkPosS = np.append([0., 0., -0.8], linkPosS)
linkPosM = linkPosM.reshape(6, 3)
linkPosS = linkPosS.reshape(6, 3)
return linkPosM, linkPosS
def move_arm(self, cmd):
self.move_cmd_client(cmd, self.__name)
def step(self, a):
alarm = []
Link_dis = []
s = self.state
self.collision = False
action_vec = a[:3] * self.ACTION_VEC_TRANS * self.move_speed
action_ori = a[3:7] * self.ACTION_ORI_TRANS * self.move_speed
action_phi = a[7] * self.ACTION_PHI_TRANS * self.move_speed
self.action = np.append(action_vec, action_ori)
self.action = np.append(self.action, action_phi)
self.cmd = np.add(s[:8], self.action)
self.cmd[3:7] /= np.linalg.norm(self.cmd[3:7])
res = self.move_cmd_client(self.cmd, self.__name)
res_ = self.get_state_client(self.__obname)
if res.success:
self.old, self.joint_pos[:15], self.joint_angle = np.array(
res.state), res.joint_pos, res.joint_angle
self.limit, self.goal_quat, self.quat_inv, self.joint_pos[
15:
30] = res.limit, res.quaterniond, res.quat_inv, res_.joint_pos
linkPosM, linkPosS = self.collision_init()
alarm, Link_dis = self.cc.checkCollision(linkPosM, linkPosS)
s = np.append(self.old, np.subtract(self.goal[:3], self.old[:3]))
s = np.append(s, self.goal_quat)
s = np.append(s, Link_dis)
s = np.append(s, self.joint_angle)
s = np.append(s, self.limit)
self.dis_pos = np.linalg.norm(self.goal[:3] - s[:3])
self.dis_ori = math.sqrt(
np.linalg.norm(self.goal[3:7] - s[3:7]) +
np.linalg.norm(-1 * self.goal[3:7] - s[3:7]) - 2)
s = np.append(s, self.dis_pos)
s = np.append(s, self.dis_ori)
s = np.append(s, self.joint_pos[6:12])
s = np.append(s, self.goal_angle)
terminal = self._terminal(s, res.success, alarm)
if (not self.collision) and math.fabs(s[7]) < 0.9:
self.state = s
if self.workers == 'arm':
if self.object_pub == 0:
self.set_object(self.__name,
(self.goal[0] - 0.08, self.goal[1],
self.goal[2] + 1.45086), (0, 0, 0, 0))
self.object_pub = 1
else:
self.set_object(self.__name + 'q',
(self.goal[0] - 0.08, self.goal[1],
self.goal[2] + 1.45086), self.goal[3:7])
self.object_pub = 0
return self.state, terminal, self.collision, res.success, res.singularity
def _terminal(self, s, ik_success, alarm):
alarm_cnt = 0
for i in alarm:
alarm_cnt += i
if alarm_cnt > 0.4:
self.collision = True
if ik_success and not self.collision:
if self.dis_pos < self.goal_err and self.dis_ori < self.ori_err:
if not self.done:
self.done = True
self.s_cnt += 1
return True
else:
return False
else:
return False
