def __init__(self):
rospy.init_node('pioneer_cross_arm', anonymous=False)
rospy.loginfo("[CA] Pioneer Main Cross Arm- Running")
self.kinematics = Kinematics()
self.motion = Motion()
self.motor = Motor()
self.prev_arm = None
def __init__(self):
print('start')
self.kinematics = Kinematics()
self.motor = Motor("Thormang3_Wolf")
self.rate = rospy.Rate(10)
self.speech = rospy.Publisher("/robotis/sensor/text_to_speech",String,queue_size=10)
# self.speech = rospy.Publisher("/robotis/sensor/update_tts",String,queue_size=10)
self.music = rospy.Publisher("/robotis/sensor/music",mp3,queue_size=10)
self.music_stop = rospy.Publisher('/robotis/sensor/stop_music', Bool,queue_size = 10)
self.sensor_list = None
self.pub_gripper = rospy.Publisher('/robotis/gripper/joint_pose_msg',JointState,queue_size=10)
#base_ini
self.base_ini = rospy.Publisher("/robotis/base/ini_pose",String,queue_size=10)
for _ in range(4):
self.base_ini.publish('ini_pose')
self.rate.sleep()
self.end_action()
sleep(1)
#print("start")
#ini_arm
self.kinematics.publisher_(self.kinematics.module_control_pub, "manipulation_module", latch=True)
self.kinematics.publisher_(self.kinematics.module_control_pub, "gripper_module", latch=True)
self.kinematics.publisher_(self.kinematics.send_ini_pose_msg_pub, "ini_pose", latch=True)
self.end_action()
self.left_arm_ini_pos = self.kinematics.get_kinematics_pose('left_arm')
self.kinematics.publisher_(self.kinematics.module_control_pub, "head_control_module", latch=True)
#sleep(0.5)
#move head
self.head_postition = rospy.Publisher('/robotis/head_control/set_joint_states',JointState,queue_size=10)
sleep(0.5)
#print('moved')
self.sensor_list = None
self.deg = float(60)
self.deg_30 = float(30 *math.pi /180)
self.head_pan = np.radians(12)
# self.head_pan = float(23 * math.pi / 180)
self.head_tilt = float(36 * math.pi / 180)
self.student_pan = np.radians(36)
self.student_tilt = np.radians(-12)
self.J_pan = np.radians(36)
self.J_tilt = np.radians(-28)
def __init__(self):
np.set_printoptions(suppress=True)
rospy.init_node('pioneer_placement_keyboard', anonymous=False)
rospy.loginfo("[Main] Pioneer Placement Keyboard - Running")
rospack = rospkg.RosPack()
self.ws_path = rospack.get_path(
"pioneer_main") + "/config/thormang3_align_keyboard_ws.yaml"
# Subscriber
rospy.Subscriber("/pioneer/placement/left_arm_point", Point32,
self.left_arm_pos_callback)
rospy.Subscriber("/pioneer/placement/right_arm_point", Point32,
self.right_arm_pos_callback)
rospy.Subscriber("/pioneer/placement/approach_keyboard", Bool,
self.approach_keyboard_callback)
rospy.Subscriber("/pioneer/placement/placement_trigger", Bool,
self.placement_trigger_callback)
rospy.Subscriber("/pioneer/placement/grasp_keyboard", Bool,
self.grip_key_callback)
rospy.Subscriber("/pioneer/placement/init_pose", Bool,
self.ini_pose_callback)
rospy.Subscriber("/pioneer/placement/left_arm_arr_points", Pose2DArray,
self.left_arm_arr_points_callback)
rospy.Subscriber("/pioneer/placement/right_arm_arr_points",
Pose2DArray, self.right_arm_arr_points_callback)
rospy.Subscriber("/pioneer/placement/shutdown_signal", Bool,
self.shutdown_callback)
# Publisher
self.finish_placement_pub = rospy.Publisher(
"/pioneer/placement/finish_placement", Bool, queue_size=1)
# Variables
self.kinematics = Kinematics()
self.main_rate = rospy.Rate(20)
self.state = None
self.shutdown = False
self.zl, self.rl, self.pl, self.yl = 0.62, 150, -1, -29 # 0.63
self.zr, self.rr, self.pr, self.yr = 0.62, -150, -1, 29 # 0.63
self.left_points, self.right_points = None, None
self.ik_l_trajectory, self.ik_r_trajectory = None, None
self.ik_debug = False
def __init__(self):
rospy.init_node('pioneer_cross_arm') #, disable_signals=True)
rospy.loginfo("[CA] Pioneer Main Cross Arm- Running")
rospack = rospkg.RosPack()
self.video_file = "vlc " + rospack.get_path(
"pioneer_main") + "/data/cross_arm/robot_dream.mp4"
self.kinematics = Kinematics()
self.motion = Motion()
self.action = Action("Thormang3_Bear")
self.main_rate = rospy.Rate(10)
self.arm = None
self.state = None
self.shutdown = False
self.object = False
self.failed = False
self.left_clicked = False
self.scan_offset = 50
self.init_head_p = 15
self.volume_value = 60 # 60
self.fail_counter = 0
## Publisher
self.play_sound_pub = rospy.Publisher('/play_sound_file',
String,
queue_size=10)
self.set_volume_pub = rospy.Publisher('/set_volume',
Int16,
queue_size=10)
self.face_pub = rospy.Publisher('/pioneer/face', Bool, queue_size=10)
# self.shutdown_pub = rospy.Publisher("/pioneer/shutdown_signal", Bool, queue_size=10)
## Subscriber
rospy.Subscriber("/pioneer/shutdown_signal", Bool,
self.shutdown_callback)
rospy.Subscriber("/pioneer/cross_arm/final_decision", String,
self.final_decision_callback)
rospy.Subscriber("/pioneer/cross_arm/object", Bool,
self.object_callback)
def main():
rospy.init_node('pioneer_trajectory', anonymous=False)
rospy.loginfo("[Tra] Pioneer Demo Trajectory - Running")
## Trajectory
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)
# mode = 'full'
mode = 'left_arm'
if mode == "full":
# trajectory simulation circle
kinematics.set_kinematics_pose("left_arm" , 2.0, **{ 'x': 0.20, 'y': 0.30, 'z': 0.80, 'roll': 0.00, 'pitch': 0.00, 'yaw': 0.00 })
kinematics.set_kinematics_pose("right_arm" , 2.0, **{ 'x': 0.20, 'y': -0.30, 'z': 0.80, 'roll': 0.00, 'pitch': 0.00, 'yaw': 0.00 })
sleep(0.1)
while kinematics.left_tra == True and kinematics.right_tra == True:
pass
sleep(1)
iter = 0
while not rospy.is_shutdown():
if mode == "full":
kinematics.trajectory_sin(group="left_arm", x=0.30, y=0.20, z=0.90, roll=0.0, pitch=0.0, yaw=0.0, xc=-0.1, yc=-0.1, zc=-0.1, time=2, res=0.01)
kinematics.trajectory_sin(group="right_arm", x=0.30, y=-0.20, z=0.90, roll=0.0, pitch=0.0, yaw=0.0, xc=-0.1, yc=0.1, zc=-0.1, time=2, res=0.01)
sleep(1) # because pubslishing array msg using for loop
while kinematics.left_arr == True and kinematics.right_arr == True:
pass # no action
kinematics.trajectory_sin(group="left_arm", x=0.20, y=0.30, z=0.85, roll=0.0, pitch=0.0, yaw=0.0, xc=0.1, yc=0.1, zc=0.1, time=2, res=0.01)
kinematics.trajectory_sin(group="right_arm", x=0.20, y=-0.30, z=0.85, roll=0.0, pitch=0.0, yaw=0.0, xc=0.1, yc=-0.1, zc=0.1, time=2, res=0.01)
sleep(1) # because pubslishing array msg using for loop
while kinematics.left_arr == True and kinematics.right_arr == True:
pass # no action
iter += 1
rospy.loginfo("[Tra] Iteration : {}".format(iter))
if iter >= 2:
mode = "none"
elif mode == "left_arm":
left_arm = kinematics.get_kinematics_pose("left_arm")
print(left_arm)
kinematics.set_kinematics_pose("left_arm" , 2.0, **{ 'x': 0.40, 'y': 0.50, 'z': 0.80, 'roll': 0.00, 'pitch': 0.00, 'yaw': 0.00 })
while kinematics.left_arr == True
pass
mode = "none"
# pass
elif mode == "none":
break
kinematics.publisher_(kinematics.send_ini_pose_msg_pub, "ini_pose", latch=True)
kinematics.kill_threads()
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(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', 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():
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()
def main():
rospy.init_node('pioneer_main', anonymous=False)
rospy.loginfo("Pioneer Main - Running")
# camera = Camera()
# print(sys.version)
##################
## Kinematics
##################
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)
# trajectory simulation circle
# kinematics.set_kinematics_pose("left_arm" , 1.0, **{ 'x': 0.20, 'y': 0.30, 'z': 0.80, 'roll': 0.00, 'pitch': 0.00, 'yaw': 0.00 })
# kinematics.set_kinematics_pose("right_arm" , 1.0, **{ 'x': 0.20, 'y': -0.30, 'z': 0.80, 'roll': 0.00, 'pitch': 0.00, 'yaw': 0.00 })
# sleep(1)
# a = 0
# while not rospy.is_shutdown():
# kinematics.trajectory_sin(group="left_arm", x=0.30, y=0.20, z=0.90, roll=0.0, pitch=0.0, yaw=0.0, xc=-0.1, yc=-0.1, zc=-0.1, time=2, res=0.01)
# kinematics.trajectory_sin(group="right_arm", x=0.30, y=-0.20, z=0.90, roll=0.0, pitch=0.0, yaw=0.0, xc=-0.1, yc=0.1, zc=-0.1, time=2, res=0.01)
# sleep(1) # because pubslishing array msg using for loop
# while kinematics.left_arr == True and kinematics.right_arr == True:
# pass # no action
# # input("Press enter")
# kinematics.trajectory_sin(group="left_arm", x=0.20, y=0.30, z=0.80, roll=0.0, pitch=0.0, yaw=0.0, xc=0.1, yc=0.1, zc=0.1, time=2, res=0.01)
# kinematics.trajectory_sin(group="right_arm", x=0.20, y=-0.30, z=0.80, roll=0.0, pitch=0.0, yaw=0.0, xc=0.1, yc=-0.1, zc=0.1, time=2, res=0.01)
# sleep(1) # because pubslishing array msg using for loop
# while kinematics.left_arr == True and kinematics.right_arr == True:
# pass # no action
# # input("Press enter1")
# a = a+1
# if a >= 3:
# break
# while not rospy.is_shutdown():
# kinematics.set_gripper("left_arm", 2, 5)
# kinematics.set_gripper("right_arm", 2, 5)
# # sleep(2)
# break
# left_arm = kinematics.get_kinematics_pose("left_arm")
# right_arm = kinematics.get_kinematics_pose("right_arm")
# sleep(1)
# while not rospy.is_shutdown():
# kinematics.set_kinematics_pose("left_arm" , 2.0, **{ 'x': left_arm.get('x'), 'y': left_arm.get('y'), \
# 'z': left_arm.get('z') + 0.1, 'roll': left_arm.get('roll'), 'pitch': left_arm.get('pitch'), 'yaw': left_arm.get('yaw') })
# kinematics.set_kinematics_pose("right_arm" , 2.0, **{ 'x': right_arm.get('x'), 'y': right_arm.get('y'), \
# 'z': right_arm.get('z') + 0.1, 'roll': right_arm.get('roll'), 'pitch': right_arm.get('pitch'), 'yaw': right_arm.get('yaw') })
# break
##################
## Motor
##################
# motor = Motor()
# motor.publisher_(motor.module_control_pub, "none", latch=True)
# rate = rospy.Rate(60)
# while not rospy.is_shutdown():
# rospy.loginfo("motor")
# # motor.set_joint_states(["l_arm_wr_p", "l_arm_wr_y", "r_arm_wr_y", "r_arm_wr_p"], \
# # [0, 0, 0, 0], [0, 0, 0, 0], [0, 0, 0, 0])
# # sleep(2)
# # motor.set_joint_states(["l_arm_wr_p", "l_arm_wr_y", "r_arm_wr_y", "r_arm_wr_p"], \
# # [45, 45, 45, 45], [0, 0, 0, 0], [0, 0, 0, 0])
# # sleep(2)
# # motor.set_joint_states(["l_arm_wr_p", "l_arm_wr_y", "l_arm_wr_r", "r_arm_wr_y", "r_arm_wr_p", "r_arm_wr_r"], False)
# # motor.set_joint_states(["l_arm_thumb_y", "l_arm_thumb_p", "l_arm_index_p", "l_arm_middle_p", "l_arm_finger45_p",
# # "r_arm_thumb_y", "r_arm_thumb_p", "r_arm_index_p", "r_arm_middle_p", "r_arm_finger45_p"], False)
# motor.set_joint_states(["all"], False)
# rate.sleep()
##################
## Walk
##################
# walk = Walking()
# walk.latching_publish(walk.walking_pub, "ini_pose")
# sleep(4)
# print("walking")
# walk.latching_publish(walk.walking_pub, "set_mode") # <-- Enable walking mode
# sleep(4)
# print("walking balance on")
# walk.latching_publish(walk.walking_pub, "balance_on")
# sleep(4)
# walk.walk_command("forward", 1, 1.0, 0.01, 0.05, 5)
# while (walk.status_msg == "Walking_Started"):
# rospy.loginfo("walking forward 1")
# walk.walk_command("forward", 1, 1.0, 0.1, 0.05, 5)
# while (walk.status_msg == "Walking_Started"):
# rospy.loginfo("walking forward 2")
# walk.walk_command("forward", 1, 1.0, 0.01, 0.05, 5)
# while (walk.status_msg == "Walking_Started"):
# rospy.loginfo("walking forward 3")
# sleep(5)
# walk.kill_threads()
##################
## Sensor
##################
# sensor = Sensor()
# sleep(5)
# sensor.kill_threads()
##################
## Motion
##################
# motion = Motion()
# motion.init_motion()
# print(motion.init_joint)
# print(motion.init_pose)
# sleep(3)
# rospy.loginfo(motion.init_pose)
# # motor.set_joint_states(motion.init_joint, motion.init_pose)
# # sleep(5)
# motor.kill_threads()
# motor.set_joint_states(["l_arm_el_y"], [0.0], [0.0], [0.0])
# motor.set_joint_states(["l_arm_el_y"], [0.5])
# while not rospy.is_shutdown():
# # rospy.loginfo("infinite")
# # print(camera.source_image)
# cv2.imshow('img',camera.source_image)
# cv2.waitKey(1)
# # rate.sleep()
kinematics.kill_threads()