def main():
rospy.init_node('pioneer_main_typing') #, disable_signals=True)
rospy.loginfo("[TY] Pioneer Main Typing - Running")
rospy.Subscriber("/pioneer/aruco/keyboard_position", Pose2D,
keyboard_pos_callback)
rospy.Subscriber("/pioneer/shutdown_signal", Bool, shutdown_callback)
# Kinematics
kinematics = Kinematics()
kinematics.publisher_(kinematics.module_control_pub,
"manipulation_module",
latch=True) # <-- Enable Manipulation mode
kinematics.publisher_(kinematics.send_ini_pose_msg_pub,
"typing_pose",
latch=True)
kinematics.publisher_(kinematics.en_typing_pub, True,
latch=False) # <-- Enable Typing mode
wait_robot(kinematics, "End Init Trajectory")
# Typing Init Pose
kinematics.set_joint_pos(['head_p', 'head_y', 'torso_y'], [30, 0, 0])
kinematics.set_gripper("left_arm", 0, 0)
kinematics.set_gripper("right_arm", 0, 0)
sleep(2)
# Set Finger
kinematics.set_joint_pos(['l_arm_index_p', 'l_arm_thumb_p'], [-86, -70])
kinematics.set_joint_pos(['r_arm_index_p', 'r_arm_thumb_p'], [-86, -70])
rospy.loginfo('[TY] Finish Init Head & Hand')
main_rate = rospy.Rate(20)
global keyboard_pos, zl_typing, zr_typing
keyboard_pos = None
zl_typing = 0.033
zr_typing = 0.035
global total_word
total_word = 0
# waiting aruco position from recorder
while not rospy.is_shutdown():
while keyboard_pos == None:
pass
sleep(1)
break
# load & calculate keyboard frame position
calc_keyboard_frame_position()
# load IK config file
left_ws = load_ws_config('left_arm')
right_ws = load_ws_config('right_arm')
ik_reference(left_ws, right_ws)
# convert keyboard frame to ik position
calc_keyboard_ik_position()
while not rospy.is_shutdown():
key = input("Input key : ")
key = key.lower()
if key == 'exit':
rospy.loginfo('[TY] Pioneer Typing Exit')
break
elif key == 'init':
kinematics.publisher_(kinematics.send_ini_pose_msg_pub,
"typing_pose",
latch=False)
elif key == 'reload':
calc_keyboard_frame_position()
calc_keyboard_ik_position()
elif key == '':
rospy.loginfo('[TY] Exitting..')
else:
if len(key) == 1:
process(kinematics, key)
elif key == 'typing':
typing(kinematics, last_arm)
elif 'zl_typing' in key:
mylist = key.split(" ")
if len(mylist) == 3:
temp = float(mylist[2])
if temp >= 0.05:
rospy.logwarn('[TY] zl_typing input over limit')
else:
zl_typing = float(mylist[2])
rospy.loginfo('[TY] zl_typing : {}'.format(zl_typing))
else:
rospy.logwarn('[TY] Len zl_typing is wrong')
elif 'zr_typing' in key:
mylist = key.split(" ")
if len(mylist) == 3:
temp = float(mylist[2])
if temp >= 0.05:
rospy.logwarn('[TY] zr_typing input over limit')
else:
zr_typing = float(mylist[2])
rospy.loginfo('[TY] zr_typing : {}'.format(zr_typing))
else:
rospy.logwarn('[TY] Len zr_typing is wrong')
else:
# rospy.logwarn('[TY] Wrong input')
# going to standby position
homing = False
if check_arm(kinematics, 'left_arm', x=0.25, y=0.15) == False:
move_arm(kinematics,
'left_arm',
2.0,
x=0.25,
y=0.15,
z=0.75)
homing = True
if check_arm(kinematics, 'right_arm', x=0.25,
y=-0.15) == False:
move_arm(kinematics,
'right_arm',
2.0,
x=0.25,
y=-0.15,
z=0.77)
homing = True
if homing:
rospy.loginfo('[TY] Homing Init...')
sleep(3)
if "\\n" in key:
total_word += len(key)
rospy.loginfo('[TY] Length of word: {}'.format(total_word))
words = key.split('\\n')
print(words)
for word in words:
for alphabet in word:
rospy.loginfo('[TY] Typing: {}'.format(alphabet))
process(kinematics, alphabet)
typing(kinematics, last_arm)
rospy.loginfo('[TY] Typing: {}'.format('enter'))
process(kinematics, "\n")
typing(kinematics, last_arm)
elif "\\b" in key:
rospy.loginfo('[TY] Deleting: {}'.format(key))
process(kinematics, "\b")
typing(kinematics, last_arm, delete=True)
total_word = 0
else:
# typing for one word w/o enter
total_word += len(key)
rospy.loginfo('[TY] Length of word: {}'.format(total_word))
for alphabet in key:
rospy.loginfo('[TY] Typing: {}'.format(alphabet))
process(kinematics, alphabet)
typing(kinematics, last_arm)
move_arm(kinematics, 'left_arm', 2.0, x=0.25, y=0.15, z=0.75)
move_arm(kinematics, 'right_arm', 2.0, x=0.25, y=-0.15, z=0.77)
rospy.loginfo('[TY] Return Homing ...')
main_rate.sleep()
kinematics.kill_threads()
def main():
rospy.init_node('pioneer_main', anonymous=False)
rospy.loginfo("Pioneer Main Demo Pick Up Keyboard- Running")
kinematics = Kinematics()
kinematics.publisher_(kinematics.module_control_pub,
"manipulation_module",
latch=True) # <-- Enable Manipulation mode
kinematics.publisher_(kinematics.send_ini_pose_msg_pub,
"ini_pose",
latch=True)
sleep(2)
kinematics.set_gripper("left_arm", 0, 5)
kinematics.set_gripper("right_arm", 0, 5)
while not rospy.is_shutdown():
kinematics.set_kinematics_pose(
"right_arm", 1.0, **{
'x': 0.20,
'y': -0.30,
'z': 0.60,
'roll': 90.00,
'pitch': 40.00,
'yaw': 0.00
})
kinematics.set_kinematics_pose(
"left_arm", 1.0, **{
'x': 0.20,
'y': 0.30,
'z': 0.60,
'roll': -90.00,
'pitch': 40.00,
'yaw': 0.00
})
wait(3)
kinematics.set_kinematics_pose(
"right_arm", 2.0, **{
'x': 0.40,
'y': -0.4,
'z': 0.62,
'roll': 90.00,
'pitch': 40.00,
'yaw': 0.00
})
kinematics.set_kinematics_pose(
"left_arm", 2.0, **{
'x': 0.40,
'y': 0.15,
'z': 0.62,
'roll': -90.00,
'pitch': 40.00,
'yaw': 0.00
})
wait(3)
kinematics.set_kinematics_pose(
"right_arm", 2.0, **{
'x': 0.40,
'y': -0.33,
'z': 0.62,
'roll': 90.00,
'pitch': 40.00,
'yaw': 0.00
})
kinematics.set_kinematics_pose(
"left_arm", 2.0, **{
'x': 0.40,
'y': 0.08,
'z': 0.62,
'roll': -90.00,
'pitch': 40.00,
'yaw': 0.00
})
wait(3)
kinematics.set_gripper("left_arm", 3, 5)
kinematics.set_gripper("right_arm", 3, 5)
wait(0.3)
kinematics.set_kinematics_pose(
"right_arm", 2.0, **{
'x': 0.30,
'y': -0.2,
'z': 0.8,
'roll': 90.00,
'pitch': 30.00,
'yaw': 0.00
})
kinematics.set_kinematics_pose(
"left_arm", 2.0, **{
'x': 0.30,
'y': 0.2,
'z': 0.8,
'roll': -90.00,
'pitch': 30.00,
'yaw': 0.00
})
wait(6)
kinematics.set_kinematics_pose(
"right_arm", 2.0, **{
'x': 0.40,
'y': -0.33,
'z': 0.62,
'roll': 90.00,
'pitch': 40.00,
'yaw': 0.00
})
kinematics.set_kinematics_pose(
"left_arm", 2.0, **{
'x': 0.40,
'y': 0.08,
'z': 0.62,
'roll': -90.00,
'pitch': 40.00,
'yaw': 0.00
})
wait(3)
kinematics.set_gripper("left_arm", 0, 5)
kinematics.set_gripper("right_arm", 0, 5)
kinematics.set_kinematics_pose(
"right_arm", 2.0, **{
'x': 0.40,
'y': -0.4,
'z': 0.62,
'roll': 90.00,
'pitch': 40.00,
'yaw': 0.00
})
kinematics.set_kinematics_pose(
"left_arm", 2.0, **{
'x': 0.40,
'y': 0.15,
'z': 0.62,
'roll': -90.00,
'pitch': 40.00,
'yaw': 0.00
})
wait(3)
kinematics.publisher_(kinematics.send_ini_pose_msg_pub, "ini_pose")
break
kinematics.kill_threads()
def main(mode):
# global variables
global state, config_path
global lmarker_x, lmarker_y, rmarker_x, rmarker_y
global left_tar_x, left_tar_y, right_tar_x, right_tar_y
left_tar_x, left_tar_y = None, None
right_tar_x, right_tar_y = None, None
state = None
scan_workspace = False
save_config = False
lx_ik, ly_ik = None, None
rx_ik, ry_ik = None, None
lik_xmin, lik_xmax = None, None
prev_lik, prev_rik = (), ()
left_ws, right_ws = {}, {}
prev_l_tar = (None, None)
prev_r_tar = (None, None)
if mode == "align_keyboard":
config_path = rospack.get_path(
"pioneer_main") + "/config/thormang3_align_keyboard_ws.yaml"
elif mode == "typing":
config_path = rospack.get_path(
"pioneer_main") + "/config/thormang3_typing_ws.yaml"
# Subscriber
rospy.Subscriber("/pioneer/aruco/left_position", Point32,
left_aruco_pos_callback)
rospy.Subscriber("/pioneer/aruco/right_position", Point32,
right_aruco_pos_callback)
rospy.Subscriber("/pioneer/target/left_arm_point", Point32,
left_arm_pos_callback)
rospy.Subscriber("/pioneer/target/right_arm_point", Point32,
right_arm_pos_callback)
rospy.Subscriber("/pioneer/target/start", Bool, arm_start_callback)
rospy.Subscriber("/pioneer/target/sync_arm", Bool, arm_sync_callback)
rospy.Subscriber("/pioneer/init_pose", Bool, ini_pose_callback)
rospy.Subscriber("/pioneer/typing", Bool, typing_pose_callback)
# Publisher
l_sync_point_pub = rospy.Publisher("/pioneer/aruco/lsync_position",
Point32,
queue_size=1)
main_rate = rospy.Rate(20)
# Kinematics
kinematics = Kinematics()
kinematics.publisher_(kinematics.module_control_pub,
"manipulation_module",
latch=True) # <-- Enable Manipulation mode
if mode == "align_keyboard":
kinematics.publisher_(kinematics.send_ini_pose_msg_pub,
"align_keyboard_pose",
latch=True)
kinematics.publisher_(kinematics.en_align_key_pub, True,
latch=False) # <-- Enable Align Keboard mode
elif mode == "typing":
kinematics.publisher_(kinematics.send_ini_pose_msg_pub,
"typing_pose",
latch=True)
kinematics.publisher_(kinematics.en_typing_pub, True,
latch=False) # <-- Enable Typing mode
wait_robot(kinematics, "End Init Trajectory")
# set init head, torso, gripper
kinematics.set_joint_pos(['head_p', 'head_y', 'torso_y'], [30, 0, 0])
kinematics.set_gripper("left_arm", 0, 0)
kinematics.set_gripper("right_arm", 0, 0)
sleep(1)
rospy.loginfo('[MC] Finish Init Head & Hand')
# set finger
if mode == "align_keyboard":
kinematics.set_joint_pos(['l_arm_finger45_p', 'r_arm_finger45_p'],
[180, 180])
lik_ymin_lower = 0.00 #0.1
lik_ymax_lower = 0.28 #0.26
lik_ymin_upper = 0.00 #0.05
lik_ymax_upper = 0.28 #0.34
lik_xmin = 0.25 #0.2
lik_xmax = 0.55 #0.45
zl, rl, pl, yl = 0.65, 150, -1, -29
zr, rr, pr, yr = 0.65, -150, -1, 29
elif mode == "typing":
kinematics.set_joint_pos(['l_arm_index_p', 'l_arm_thumb_p'],
[-86, -70])
kinematics.set_joint_pos(['r_arm_index_p', 'r_arm_thumb_p'],
[-86, -70])
lik_ymin_lower = -0.02
lik_ymax_lower = 0.28
lik_ymin_upper = -0.05
lik_ymax_upper = 0.34
lik_xmin = 0.25
lik_xmax = 0.55
zl, rl, pl, yl = 0.72, 10, 0, 0
zr, rr, pr, yr = 0.74, -10, 0, 0
lp1 = (lik_xmin, lik_ymax_lower)
lp2 = (lik_xmax, lik_ymax_upper)
lp3 = (lik_xmax, lik_ymin_upper)
lp4 = (lik_xmin, lik_ymin_lower)
left_arm_pts = [lp1, lp2, lp3, lp4]
rik_xmin = lik_xmin
rik_xmax = lik_xmax
rik_ymin_lower = lik_ymin_lower * -1
rik_ymax_lower = lik_ymax_lower * -1
rik_ymin_upper = lik_ymin_upper * -1
rik_ymax_upper = lik_ymax_upper * -1
rp1 = (rik_xmin, rik_ymax_lower)
rp2 = (rik_xmax, rik_ymax_upper)
rp3 = (rik_xmax, rik_ymin_upper)
rp4 = (rik_xmin, rik_ymin_lower)
right_arm_pts = [rp1, rp2, rp3, rp4]
if scan_workspace:
# Left Arm
for idx, pts in enumerate(left_arm_pts):
x, y = pts
kinematics.set_kinematics_pose(
"left_arm", 4.0, **{
'x': x,
'y': y,
'z': zl,
'roll': rl,
'pitch': pl,
'yaw': yl
})
logging(kinematics, 'Left Arm', x, y, idx + 1, left_ws)
# move to P1
kinematics.set_kinematics_pose(
"left_arm", 4.0, **{
'x': left_arm_pts[0][0],
'y': left_arm_pts[0][1],
'z': zl,
'roll': rl,
'pitch': pl,
'yaw': yl
})
# Right Arm
for idx, pts in enumerate(right_arm_pts):
x, y = pts
kinematics.set_kinematics_pose(
"right_arm", 4.0, **{
'x': x,
'y': y,
'z': zr,
'roll': rr,
'pitch': pr,
'yaw': yr
})
logging(kinematics, 'Right Arm', x, y, idx + 1, right_ws)
# move to P1
kinematics.set_kinematics_pose(
"right_arm", 4.0, **{
'x': right_arm_pts[0][0],
'y': right_arm_pts[0][1],
'z': zr,
'roll': rr,
'pitch': pr,
'yaw': yr
})
if save_config:
aruco_ws = {}
aruco_ws['left_arm'] = left_ws
aruco_ws['right_arm'] = right_ws
with open(config_path, 'w') as f:
yaml.dump(aruco_ws, f, default_flow_style=False)
else:
left_ws = load_config('left_arm')
right_ws = load_config('right_arm')
if left_ws != None:
yleft_data = parse_Yframe(left_ws)
ly_frame_min = yleft_data[0]
ly_frame_max = yleft_data[1]
ly_ws = range(ly_frame_min, ly_frame_max + 1)
lik_xmin = yleft_data[2]
lik_xmax = yleft_data[3]
xleft_data = parse_Xframe(left_ws)
lx_min_b = xleft_data[0][0]
lx_min_a = xleft_data[0][1]
lx_max_a = xleft_data[1][0]
lx_max_b = xleft_data[1][1]
lik_ymax_lower = xleft_data[2] #0.24
lik_ymax_upper = xleft_data[3] #0.34
lik_ymin_lower = xleft_data[4] #0.1
lik_ymin_upper = xleft_data[5] #0.05
if right_ws != None:
yright_data = parse_Yframe(right_ws)
ry_frame_min = yright_data[0]
ry_frame_max = yright_data[1]
ry_ws = range(ry_frame_min, ry_frame_max + 1)
rik_xmin = yright_data[2]
rik_xmax = yright_data[3]
xright_data = parse_Xframe(right_ws)
rx_max_b = xright_data[0][0]
rx_max_a = xright_data[0][1]
rx_min_a = xright_data[1][0]
rx_min_b = xright_data[1][1]
rik_ymax_lower = xright_data[2] #0.24
rik_ymax_upper = xright_data[3] #0.34
rik_ymin_lower = xright_data[4] #0.1
rik_ymin_upper = xright_data[5] #0.05
while not rospy.is_shutdown():
# lx_tar, ly_tar = left_tar_x, left_tar_y
# rx_tar, ry_tar= right_tar_x, right_tar_y
l_tar = (left_tar_x, left_tar_y)
r_tar = (right_tar_x, right_tar_y)
if state == 'init_pose':
rospy.loginfo('[MC] Robot State : {}'.format(state))
if mode == "align_keyboard":
kinematics.publisher_(kinematics.send_ini_pose_msg_pub,
"align_keyboard_pose",
latch=False)
kinematics.publisher_(
kinematics.en_align_key_pub, True,
latch=False) # <-- Enable Align Keboard mode
elif mode == "typing":
kinematics.publisher_(kinematics.send_ini_pose_msg_pub,
"typing_pose",
latch=False)
kinematics.publisher_(kinematics.en_typing_pub,
True,
latch=False) # <-- Enable Typing mode
lx_ik = ly_ik = rx_ik = ry_ik = None
prev_lik = prev_rik = ()
state = None
elif state == 'move_arm':
rospy.loginfo('[MC] Robot State : {}'.format(state))
if lx_ik != None and ly_ik != None:
kinematics.set_kinematics_pose(
"left_arm", 2.0, **{
'x': lx_ik,
'y': ly_ik,
'z': zl,
'roll': rl,
'pitch': pl,
'yaw': yl
})
prev_lik = (lx_ik, ly_ik)
last_arm = "left_arm"
if rx_ik != None and ry_ik != None:
kinematics.set_kinematics_pose(
"right_arm", 2.0, **{
'x': rx_ik,
'y': ry_ik,
'z': zr,
'roll': rr,
'pitch': pr,
'yaw': yr
})
prev_rik = (rx_ik, ry_ik)
last_arm = "right_arm"
state = None
elif state == "typing":
if last_arm == 'left_arm':
zl_typing = 0.03
kinematics.set_kinematics_pose(
"left_arm", 0.4, **{
'x': lx_ik,
'y': ly_ik,
'z': zl - zl_typing,
'roll': rl,
'pitch': pl,
'yaw': yl
})
wait_robot(kinematics, "End Left Arm Trajectory")
kinematics.set_kinematics_pose(
"left_arm", 0.4, **{
'x': lx_ik,
'y': ly_ik,
'z': zl + zl_typing,
'roll': rl,
'pitch': pl,
'yaw': yl
})
elif last_arm == 'right_arm':
zr_typing = 0.03
kinematics.set_kinematics_pose(
"right_arm", 0.4, **{
'x': rx_ik,
'y': ry_ik,
'z': zr - zr_typing,
'roll': rr,
'pitch': pr,
'yaw': yr
})
wait_robot(kinematics, "End Right Arm Trajectory")
kinematics.set_kinematics_pose(
"right_arm", 0.4, **{
'x': rx_ik,
'y': ry_ik,
'z': zr + zr_typing,
'roll': rr,
'pitch': pr,
'yaw': yr
})
state = None
elif state == 'sync_move_arms':
rospy.loginfo('[MC] Robot State : {}'.format(state))
if prev_rik and prev_lik:
rospy.loginfo("[MC] Finish Sync Calculating")
diff_ikr = np.array([rx_ik, ry_ik]) - np.array(prev_rik)
ikl_synch = prev_lik + diff_ikr
lx_ik = ikl_synch[0]
ly_ik = ikl_synch[1]
ly_p = np.interp(lx_ik, [lik_xmin, lik_xmax],
[ly_frame_max, ly_frame_min])
# Mapping CX Frame
lx_frame_min = np.interp(ly_p, [ly_frame_min, ly_frame_max],
[lx_min_a, lx_min_b])
lx_frame_min = int(np.round(lx_frame_min, 0))
lx_frame_max = np.interp(ly_p, [ly_frame_min, ly_frame_max],
[lx_max_a, lx_max_b])
lx_frame_max = int(np.round(lx_frame_max, 0))
# Mapping IK_Y
lik_ymax = np.interp(ly_p, [ly_frame_min, ly_frame_max],
[lik_ymax_upper, lik_ymax_lower])
lik_ymax = np.round(lik_ymax, 4)
lik_ymin = np.interp(ly_p, [ly_frame_min, ly_frame_max],
[lik_ymin_upper, lik_ymin_lower])
lik_ymin = np.round(lik_ymin, 4)
lx_p = np.interp(ly_ik, [lik_ymin, lik_ymax],
[lx_frame_max, lx_frame_min])
lp = Point32()
lp.x = lx_p
lp.y = ly_p
l_sync_point_pub.publish(lp)
state = None
else:
if prev_l_tar != l_tar:
if l_tar[1] in ly_ws:
# Mapping CY Frame
# ly_frame_min = np.interp( l_tar[1], [ly_frame_min, ly_frame_max], [ly_min_a, ly_min_b] )
# ly_frame_min = int( np.round(ly_frame_min, 0) )
# Left IK X Target
lx_ik = np.interp(l_tar[1], [ly_frame_min, ly_frame_max],
[lik_xmax, lik_xmin])
# Mapping CX Frame
lx_frame_min = np.interp(l_tar[1],
[ly_frame_min, ly_frame_max],
[lx_min_a, lx_min_b])
lx_frame_min = int(np.round(lx_frame_min, 0))
lx_frame_max = np.interp(l_tar[1],
[ly_frame_min, ly_frame_max],
[lx_max_a, lx_max_b])
lx_frame_max = int(np.round(lx_frame_max, 0))
# Mapping IK_Y
lik_ymax = np.interp(l_tar[1],
[ly_frame_min, ly_frame_max],
[lik_ymax_upper, lik_ymax_lower])
lik_ymax = np.round(lik_ymax, 4)
lik_ymin = np.interp(l_tar[1],
[ly_frame_min, ly_frame_max],
[lik_ymin_upper, lik_ymin_lower])
lik_ymin = np.round(lik_ymin, 4)
lx_ws = range(lx_frame_min, lx_frame_max + 1)
if l_tar[0] in lx_ws:
# Left IK Y Target
ly_ik = np.interp(l_tar[0],
[lx_frame_min, lx_frame_max],
[lik_ymax, lik_ymin])
print()
rospy.loginfo(
'[Left Arm] Input Coor X: {0}, Y: {1}'.format(
l_tar[0], l_tar[1]))
rospy.loginfo(
'[Left Arm] X_IK: {0:.2f}, Y_IK: {1:.2f}'.format(
lx_ik, ly_ik))
else:
rospy.logerr(
'[Left Arm] X Frame target is out of range')
else:
rospy.logerr('[Left Arm] Y Frame target is out of range')
prev_l_tar = l_tar
if prev_r_tar != r_tar:
if r_tar[1] in ry_ws:
# Left IK X Target
rx_ik = np.interp(r_tar[1], [ry_frame_min, ry_frame_max],
[rik_xmax, rik_xmin])
# Mapping CX Frame
rx_frame_min = np.interp(r_tar[1],
[ry_frame_min, ry_frame_max],
[rx_min_a, rx_min_b])
rx_frame_min = int(np.round(rx_frame_min, 0))
rx_frame_max = np.interp(r_tar[1],
[ry_frame_min, ry_frame_max],
[rx_max_a, rx_max_b])
rx_frame_max = int(np.round(rx_frame_max, 0))
# Mapping IK_Y
rik_ymax = np.interp(r_tar[1],
[ry_frame_min, ry_frame_max],
[rik_ymax_upper, rik_ymax_lower])
rik_ymax = np.round(rik_ymax, 4)
rik_ymin = np.interp(r_tar[1],
[ry_frame_min, ry_frame_max],
[rik_ymin_upper, rik_ymin_lower])
rik_ymin = np.round(rik_ymin, 4)
rx_ws = range(rx_frame_min, rx_frame_max + 1)
if r_tar[0] in rx_ws:
# Left IK Y Target
ry_ik = np.interp(r_tar[0],
[rx_frame_min, rx_frame_max],
[rik_ymin, rik_ymax])
print()
rospy.loginfo(
'[Right Arm] Input Coor X: {0}, Y: {1}'.format(
r_tar[0], r_tar[1]))
rospy.loginfo(
'[Right Arm] X_IK: {0:.2f}, Y_IK: {1:.2f}'.format(
rx_ik, ry_ik))
else:
rospy.logerr(
'[Right Arm] X Frame target is out of range')
else:
rospy.logerr('[Right Arm] Y Frame target is out of range')
prev_r_tar = r_tar
main_rate.sleep()
kinematics.kill_threads()
def main():
rospy.init_node('pioneer_main_align_keyboard', anonymous=False)
rospy.loginfo("[AK] Pioneer Align Keyboard - Running")
rospy.Subscriber("/pioneer/target/left_arm_point", Point32,
left_arm_pos_callback)
rospy.Subscriber("/pioneer/target/right_arm_point", Point32,
right_arm_pos_callback)
rospy.Subscriber("/pioneer/target/start", Bool, arm_start_callback)
rospy.Subscriber("/pioneer/target/sync_arm", Bool, arm_sync_callback)
rospy.Subscriber("/pioneer/target/grasp_keyboard", Bool, grip_key_callback)
rospy.Subscriber("/pioneer/init_pose", Bool, ini_pose_callback)
l_sync_point_pub = rospy.Publisher("/pioneer/aruco/lsync_position",
Point32,
queue_size=1)
main_rate = rospy.Rate(20)
# global variables
global start_ik, start_sync, grasp_keyboard, init_pose
start_sync = False
start_ik = False
grasp_keyboard = False
init_pose = False
global state
global left_tar_x, left_tar_y, right_tar_x, right_tar_y
global prev_lik, prev_rik
left_tar_x, left_tar_y = None, None
right_tar_x, right_tar_y = None, None
state = None
lx_ik, ly_ik = None, None
rx_ik, ry_ik = None, None
lik_xmin, lik_xmax = None, None
prev_lik, prev_rik = (), ()
left_ws, right_ws = {}, {}
prev_l_tar = (None, None)
prev_r_tar = (None, None)
##################
## Kinematics
##################
kinematics = Kinematics()
kinematics.publisher_(kinematics.module_control_pub,
"manipulation_module",
latch=True) # <-- Enable Manipulation mode
kinematics.publisher_(kinematics.send_ini_pose_msg_pub,
"align_keyboard_pose",
latch=True)
kinematics.publisher_(kinematics.en_align_key_pub, True,
latch=False) # <-- Enable Align Keboard mode
wait_robot(kinematics, "End Init Trajectory")
# set init head, torso, gripper
kinematics.set_joint_pos(['head_p', 'head_y', 'torso_y'], [30, 0, 0])
kinematics.set_gripper("left_arm", 0, 0)
kinematics.set_gripper("right_arm", 0, 0)
sleep(1)
kinematics.set_joint_pos(['l_arm_finger45_p', 'r_arm_finger45_p'],
[180, 180])
sleep(1)
rospy.loginfo('[AK] Finish Init Head & Hand')
# load config file
left_ws = load_ws_config('left_arm')
right_ws = load_ws_config('right_arm')
if left_ws != None:
yleft_data = parse_Yframe(left_ws)
ly_frame_min = yleft_data[0]
ly_frame_max = yleft_data[1]
ly_ws = range(ly_frame_min, ly_frame_max + 1)
lik_xmin = yleft_data[2]
lik_xmax = yleft_data[3]
xleft_data = parse_Xframe(left_ws)
lx_min_b = xleft_data[0][0]
lx_min_a = xleft_data[0][1]
lx_max_a = xleft_data[1][0]
lx_max_b = xleft_data[1][1]
lik_ymax_lower = xleft_data[2] #0.24
lik_ymax_upper = xleft_data[3] #0.34
lik_ymin_lower = xleft_data[4] #0.1
lik_ymin_upper = xleft_data[5] #0.05
if right_ws != None:
yright_data = parse_Yframe(right_ws)
ry_frame_min = yright_data[0]
ry_frame_max = yright_data[1]
ry_ws = range(ry_frame_min, ry_frame_max + 1)
rik_xmin = yright_data[2]
rik_xmax = yright_data[3]
xright_data = parse_Xframe(right_ws)
rx_max_b = xright_data[0][0]
rx_max_a = xright_data[0][1]
rx_min_a = xright_data[1][0]
rx_min_b = xright_data[1][1]
rik_ymax_lower = xright_data[2] #0.24
rik_ymax_upper = xright_data[3] #0.34
rik_ymin_lower = xright_data[4] #0.1
rik_ymin_upper = xright_data[5] #0.05
while not rospy.is_shutdown():
if left_tar_x == -1 and left_tar_y == -1:
lx_ik = ly_ik = None
else:
l_tar = (left_tar_x, left_tar_y)
if right_tar_x == -1 and right_tar_y == -1:
rx_ik = ry_ik = None
else:
r_tar = (right_tar_x, right_tar_y)
if state == 'init_pose':
rospy.loginfo('[AK] Robot State : {}'.format(state))
if prev_lik and prev_rik:
lx_ik = prev_lik[0]
ly_ik = prev_lik[1] + 0.062
move_arm(kinematics, "left_arm", lx_ik, ly_ik)
rx_ik = prev_rik[0]
ry_ik = prev_rik[1] - 0.062
move_arm(kinematics, "right_arm", rx_ik, ry_ik)
sleep(2.5)
kinematics.publisher_(kinematics.send_ini_pose_msg_pub,
"align_keyboard_pose",
latch=False)
kinematics.publisher_(
kinematics.en_align_key_pub, True,
latch=False) # <-- Enable Align Keboard mode
else:
kinematics.publisher_(kinematics.send_ini_pose_msg_pub,
"align_keyboard_pose",
latch=False)
kinematics.publisher_(
kinematics.en_align_key_pub, True,
latch=False) # <-- Enable Align Keboard mode
lx_ik = ly_ik = rx_ik = ry_ik = None
prev_lik = prev_rik = ()
state = None
elif state == 'move_arm':
rospy.loginfo('[AK] Robot State : {}'.format(state))
if lx_ik != None and ly_ik != None \
and rx_ik != None and ry_ik != None:
print('move arm now')
ly_ik += 0.05
lx_ik -= 0.0 #0.025
move_arm(kinematics, "left_arm", lx_ik, ly_ik)
ry_ik -= 0.05
rx_ik -= 0.0
move_arm(kinematics, "right_arm", rx_ik, ry_ik)
else:
print('LX_IK: {}, LY_IK: {}'.format(lx_ik, ly_ik))
print('RX_IK: {}, RY_IK: {}'.format(rx_ik, ry_ik))
rospy.logwarn(
'[AK] Robot arm singularities \n Please move keyboard to workspace'
)
state = None
elif state == 'grip_keyboard':
rospy.loginfo('[AK] Robot State : {}'.format(state))
if prev_lik and prev_rik:
lx_ik = prev_lik[0]
ly_ik = prev_lik[1] - 0.06
move_arm(kinematics, "left_arm", lx_ik, ly_ik)
rx_ik = prev_rik[0]
ry_ik = prev_rik[1] + 0.06
move_arm(kinematics, "right_arm", rx_ik, ry_ik)
state = None
elif state == 'sync_move_arms':
rospy.loginfo('[AK] Robot State : {}'.format(state))
diff_ikr = np.array([rx_ik, ry_ik]) - np.array(prev_rik)
ikl_synch = prev_lik + diff_ikr
lx_ik = ikl_synch[0]
ly_ik = ikl_synch[1]
ly_p = np.interp(lx_ik, [lik_xmin, lik_xmax],
[ly_frame_max, ly_frame_min])
# Mapping CX Frame
lx_frame_min = np.interp(ly_p, [ly_frame_min, ly_frame_max],
[lx_min_a, lx_min_b])
lx_frame_min = int(np.round(lx_frame_min, 0))
lx_frame_max = np.interp(ly_p, [ly_frame_min, ly_frame_max],
[lx_max_a, lx_max_b])
lx_frame_max = int(np.round(lx_frame_max, 0))
# Mapping IK_Y
lik_ymax = np.interp(ly_p, [ly_frame_min, ly_frame_max],
[lik_ymax_upper, lik_ymax_lower])
lik_ymax = np.round(lik_ymax, 4)
lik_ymin = np.interp(ly_p, [ly_frame_min, ly_frame_max],
[lik_ymin_upper, lik_ymin_lower])
lik_ymin = np.round(lik_ymin, 4)
lx_p = np.interp(ly_ik, [lik_ymin, lik_ymax],
[lx_frame_max, lx_frame_min])
lp = Point32()
lp.x = lx_p
lp.y = ly_p
l_sync_point_pub.publish(lp)
sleep(0.2)
move_arm(kinematics, "right_arm", rx_ik, ry_ik)
move_arm(kinematics, "left_arm", lx_ik, ly_ik)
state = None
else:
if prev_l_tar != l_tar:
if l_tar[1] in ly_ws:
# Mapping CY Frame
# ly_frame_min = np.interp( l_tar[1], [ly_frame_min, ly_frame_max], [ly_min_a, ly_min_b] )
# ly_frame_min = int( np.round(ly_frame_min, 0) )
# Left IK X Target
lx_ik = np.interp(l_tar[1], [ly_frame_min, ly_frame_max],
[lik_xmax, lik_xmin])
# Mapping CX Frame
lx_frame_min = np.interp(l_tar[1],
[ly_frame_min, ly_frame_max],
[lx_min_a, lx_min_b])
lx_frame_min = int(np.round(lx_frame_min, 0))
lx_frame_max = np.interp(l_tar[1],
[ly_frame_min, ly_frame_max],
[lx_max_a, lx_max_b])
lx_frame_max = int(np.round(lx_frame_max, 0))
# Mapping IK_Y
lik_ymax = np.interp(l_tar[1],
[ly_frame_min, ly_frame_max],
[lik_ymax_upper, lik_ymax_lower])
lik_ymax = np.round(lik_ymax, 4)
lik_ymin = np.interp(l_tar[1],
[ly_frame_min, ly_frame_max],
[lik_ymin_upper, lik_ymin_lower])
lik_ymin = np.round(lik_ymin, 4)
lx_ws = range(lx_frame_min, lx_frame_max + 1)
if l_tar[0] in lx_ws:
# Left IK Y Target
ly_ik = np.interp(l_tar[0],
[lx_frame_min, lx_frame_max],
[lik_ymax, lik_ymin])
print()
rospy.loginfo(
'[Left Arm] Input Coor X: {0}, Y: {1}'.format(
l_tar[0], l_tar[1]))
rospy.loginfo(
'[Left Arm] X_IK: {0:.2f}, Y_IK: {1:.2f}'.format(
lx_ik, ly_ik))
else:
rospy.logerr(
'[Left Arm] X Frame target is out of range')
else:
rospy.logerr('[Left Arm] Y Frame target is out of range')
prev_l_tar = l_tar
if prev_r_tar != r_tar:
if r_tar[1] in ry_ws:
# Left IK X Target
rx_ik = np.interp(r_tar[1], [ry_frame_min, ry_frame_max],
[rik_xmax, rik_xmin])
# Mapping CX Frame
rx_frame_min = np.interp(r_tar[1],
[ry_frame_min, ry_frame_max],
[rx_min_a, rx_min_b])
rx_frame_min = int(np.round(rx_frame_min, 0))
rx_frame_max = np.interp(r_tar[1],
[ry_frame_min, ry_frame_max],
[rx_max_a, rx_max_b])
rx_frame_max = int(np.round(rx_frame_max, 0))
# Mapping IK_Y
rik_ymax = np.interp(r_tar[1],
[ry_frame_min, ry_frame_max],
[rik_ymax_upper, rik_ymax_lower])
rik_ymax = np.round(rik_ymax, 4)
rik_ymin = np.interp(r_tar[1],
[ry_frame_min, ry_frame_max],
[rik_ymin_upper, rik_ymin_lower])
rik_ymin = np.round(rik_ymin, 4)
rx_ws = range(rx_frame_min, rx_frame_max + 1)
if r_tar[0] in rx_ws:
# Left IK Y Target
ry_ik = np.interp(r_tar[0],
[rx_frame_min, rx_frame_max],
[rik_ymin, rik_ymax])
print()
rospy.loginfo(
'[Right Arm] Input Coor X: {0}, Y: {1}'.format(
r_tar[0], r_tar[1]))
rospy.loginfo(
'[Right Arm] X_IK: {0:.2f}, Y_IK: {1:.2f}'.format(
rx_ik, ry_ik))
else:
rospy.logerr(
'[Right Arm] X Frame target is out of range')
else:
rospy.logerr('[Right Arm] Y Frame target is out of range')
prev_r_tar = r_tar
main_rate.sleep()
kinematics.kill_threads()