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()
class Placement_Keyboard:
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 left_arm_pos_callback(self, msg):
self.left_keyboard = (msg.x, msg.y)
rospy.loginfo('[Main] Left keyboard frame (X: {} Y: {})'.format(
msg.x, msg.y))
self.lx_ik, self.ly_ik = self.left_arm_ik(msg.x, msg.y)
def right_arm_pos_callback(self, msg):
self.right_keyboard = (msg.x, msg.y)
rospy.loginfo('[Main] Right keyboard frame (X: {} Y: {})'.format(
msg.x, msg.y))
self.rx_ik, self.ry_ik = self.right_arm_ik(msg.x, msg.y)
def approach_keyboard_callback(self, msg):
if msg.data == True:
self.state = 'approach_keyboard'
def placement_trigger_callback(self, msg):
if msg.data == True:
self.state = 'placement_trigger'
def ini_pose_callback(self, msg):
if msg.data == True:
self.state = 'init_pose'
def grip_key_callback(self, msg):
if msg.data == True:
self.state = 'grip_keyboard'
def shutdown_callback(self, msg):
self.shutdown = msg.data
rospy.signal_shutdown('Exit')
def wait_robot(self, obj, msg):
sleep(0.2)
if msg == "End Left Arm Trajectory":
while obj.left_tra != False:
if self.shutdown:
break
else:
pass # do nothing
elif msg == "End Right Arm Trajectory":
while obj.right_tra != False:
if self.shutdown:
break
else:
pass # do nothing
else:
while obj.status_msg != msg:
if self.shutdown:
break
else:
pass # do nothing
def left_arm_arr_points_callback(self, msg):
group = msg.name
self.l_num_points = len(msg.poses)
l_frame_tra = [(pose.x, pose.y) for pose in msg.poses]
l_frame_tra = np.array(l_frame_tra)
l_frame_tra[:, 1] = rospy.get_param(
"/uvc_camera_center_node/height") - l_frame_tra[:, 1]
diff_keyboard = np.array(self.left_keyboard) - np.array(
[l_frame_tra[0][0], l_frame_tra[0][1]])
self.l_frame_tra = [
self.translate_2D_point((pose[0], pose[1]), diff_keyboard)
for pose in l_frame_tra
]
self.ik_l_trajectory = [
self.left_arm_ik(pose[0], pose[1]) for pose in self.l_frame_tra
]
# rospy.loginfo('[PA] Group: {}, Total_Points: {}, IK_Point: {}'.format(group, self.l_num_points, self.ik_l_trajectory))
rospy.loginfo('[PA] {} Arr Received: {}'.format(
group, self.l_num_points))
def right_arm_arr_points_callback(self, msg):
group = msg.name
self.r_num_points = len(msg.poses)
r_frame_tra = [(pose.x, pose.y) for pose in msg.poses]
r_frame_tra = np.array(r_frame_tra)
r_frame_tra[:, 1] = rospy.get_param(
"/uvc_camera_center_node/height") - r_frame_tra[:, 1]
diff_keyboard = np.array(self.right_keyboard) - np.array(
[r_frame_tra[0][0], r_frame_tra[0][1]])
self.r_frame_tra = [
self.translate_2D_point((pose[0], pose[1]), diff_keyboard)
for pose in r_frame_tra
]
self.ik_r_trajectory = [
self.right_arm_ik(pose[0], pose[1]) for pose in self.r_frame_tra
]
# rospy.loginfo('[PA] Group: {}, Total_Points: {}, IK_Points: {}'.format(group, self.r_num_points, self.ik_r_trajectory))
rospy.loginfo('[PA] {} Arr Received: {}'.format(
group, self.r_num_points))
def translate_2D_point(self, point, diff_point):
return tuple(np.array(point) + diff_point)
def load_ws_config(self, arm):
try:
with open(self.ws_path, 'r') as f:
aruco_ws = yaml.safe_load(f)
rospy.loginfo('[Main] Loaded {} workspace'.format(arm))
return aruco_ws[arm]
except yaml.YAMLError as exc:
print(exc)
return None
def parse_Yframe(self, data):
y_frame_min = np.mean([data['P2']['cy'], data['P3']['cy']], dtype=int)
y_frame_max = np.mean([data['P1']['cy'], data['P4']['cy']], dtype=int)
ik_xmin = data['P1']['ik_x']
ik_xmax = data['P2']['ik_x']
return (y_frame_min, y_frame_max, ik_xmin, ik_xmax)
def parse_Xframe(self, data):
x_frame_min = (data['P1']['cx'], data['P2']['cx'])
x_frame_max = (data['P3']['cx'], data['P4']['cx'])
ik_ymin_lower = data['P4']['ik_y'] #0.1
ik_ymax_lower = data['P1']['ik_y'] #0.24
ik_ymin_upper = data['P3']['ik_y'] #0.05
ik_ymax_upper = data['P2']['ik_y'] #0.34
return (x_frame_min, x_frame_max, ik_ymax_lower, ik_ymax_upper,
ik_ymin_lower, ik_ymin_upper)
def move_arm(self, arm, time, x, y):
if arm == "left_arm":
self.kinematics.set_kinematics_pose(
arm, time, **{
'x': x,
'y': y,
'z': self.zl,
'roll': self.rl,
'pitch': self.pl,
'yaw': self.yl
})
# self.prev_lik = (x, y)
rospy.loginfo('[Main] Lx_ik : {:.2f} Ly_ik : {:.2f}'.format(x, y))
elif arm == "right_arm":
self.kinematics.set_kinematics_pose(
arm, time, **{
'x': x,
'y': y,
'z': self.zr,
'roll': self.rr,
'pitch': self.pr,
'yaw': self.yr
})
# self.prev_rik = (x, y)
rospy.loginfo('[Main] Rx_ik : {:.2f} Ry_ik : {:.2f}'.format(x, y))
def ik_reference(self, left_ws, right_ws):
if left_ws != None:
yleft_data = self.parse_Yframe(left_ws)
self.ly_frame_min = yleft_data[0]
self.ly_frame_max = yleft_data[1]
self.ly_ws = range(self.ly_frame_min, self.ly_frame_max + 1)
self.lik_xmin = yleft_data[2]
self.lik_xmax = yleft_data[3]
xleft_data = self.parse_Xframe(left_ws)
self.lx_min_b = xleft_data[0][0]
self.lx_min_a = xleft_data[0][1]
self.lx_max_a = xleft_data[1][0]
self.lx_max_b = xleft_data[1][1]
self.lik_ymax_lower = xleft_data[2] #0.24
self.lik_ymax_upper = xleft_data[3] #0.34
self.lik_ymin_lower = xleft_data[4] #0.1
self.lik_ymin_upper = xleft_data[5] #0.05
if right_ws != None:
yright_data = self.parse_Yframe(right_ws)
self.ry_frame_min = yright_data[0]
self.ry_frame_max = yright_data[1]
self.ry_ws = range(self.ry_frame_min, self.ry_frame_max + 1)
self.rik_xmin = yright_data[2]
self.rik_xmax = yright_data[3]
xright_data = self.parse_Xframe(right_ws)
self.rx_max_b = xright_data[0][0]
self.rx_max_a = xright_data[0][1]
self.rx_min_a = xright_data[1][0]
self.rx_min_b = xright_data[1][1]
self.rik_ymax_lower = xright_data[2] #0.24
self.rik_ymax_upper = xright_data[3] #0.34
self.rik_ymin_lower = xright_data[4] #0.1
self.rik_ymin_upper = xright_data[5] #0.05
def left_arm_ik(self, cx, cy):
if cy in self.ly_ws:
# Left IK X Target
lx_ik = np.interp(cy, [self.ly_frame_min, self.ly_frame_max],
[self.lik_xmax, self.lik_xmin])
# Mapping CX Frame
lx_frame_min = np.interp(cy,
[self.ly_frame_min, self.ly_frame_max],
[self.lx_min_a, self.lx_min_b])
lx_frame_min = int(np.round(lx_frame_min, 0))
lx_frame_max = np.interp(cy,
[self.ly_frame_min, self.ly_frame_max],
[self.lx_max_a, self.lx_max_b])
lx_frame_max = int(np.round(lx_frame_max, 0))
# Mapping IK_Y
lik_ymax = np.interp(cy, [self.ly_frame_min, self.ly_frame_max],
[self.lik_ymax_upper, self.lik_ymax_lower])
lik_ymax = np.round(lik_ymax, 4)
lik_ymin = np.interp(cy, [self.ly_frame_min, self.ly_frame_max],
[self.lik_ymin_upper, self.lik_ymin_lower])
lik_ymin = np.round(lik_ymin, 4)
lx_ws = range(lx_frame_min, lx_frame_max + 1)
if cx in lx_ws:
# Left IK Y Target
ly_ik = np.interp(cx, [lx_frame_min, lx_frame_max],
[lik_ymax, lik_ymin])
return lx_ik, ly_ik
# print()
# rospy.loginfo('[Left Arm] Input Coor X: {0}, Y: {1}'.format(cx, cy))
# rospy.loginfo('[Left Arm] X_IK: {0:.2f}, Y_IK: {1:.2f}'.format(self.lx_ik, self.ly_ik))
else:
return None, None
# rospy.logerr('[Left Arm] X Frame target is out of range')
else:
return None, None
# rospy.logerr('[Left Arm] Y Frame target is out of range')
def right_arm_ik(self, cx, cy):
if cy in self.ry_ws:
# Right IK X Target
rx_ik = np.interp(cy, [self.ry_frame_min, self.ry_frame_max],
[self.rik_xmax, self.rik_xmin])
# Mapping CX Frame
rx_frame_min = np.interp(cy,
[self.ry_frame_min, self.ry_frame_max],
[self.rx_min_a, self.rx_min_b])
rx_frame_min = int(np.round(rx_frame_min, 0))
rx_frame_max = np.interp(cy,
[self.ry_frame_min, self.ry_frame_max],
[self.rx_max_a, self.rx_max_b])
rx_frame_max = int(np.round(rx_frame_max, 0))
# Mapping IK_Y
rik_ymax = np.interp(cy, [self.ry_frame_min, self.ry_frame_max],
[self.rik_ymax_upper, self.rik_ymax_lower])
rik_ymax = np.round(rik_ymax, 4)
rik_ymin = np.interp(cy, [self.ry_frame_min, self.ry_frame_max],
[self.rik_ymin_upper, self.rik_ymin_lower])
rik_ymin = np.round(rik_ymin, 4)
rx_ws = range(rx_frame_min, rx_frame_max + 1)
if cx in rx_ws:
# Left IK Y Target
ry_ik = np.interp(cx, [rx_frame_min, rx_frame_max],
[rik_ymin, rik_ymax])
return rx_ik, ry_ik
# print()
# rospy.loginfo('[Right Arm] Input Coor X: {0}, Y: {1}'.format(cx, cy))
# rospy.loginfo('[Right Arm] X_IK: {0:.2f}, Y_IK: {1:.2f}'.format(self.rx_ik, self.ry_ik))
else:
return None, None
# rospy.logerr('[Right Arm] X Frame target is out of range')
else:
return None, None
# rospy.logerr('[Right Arm] Y Frame target is out of range')
def run(self):
kinematics = self.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
self.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('[Main] Finish Init Head & Hand')
# set hand joint torques
torque_lvl = 20
kinematics.set_joint_torque(['hand'], torque_lvl)
rospy.loginfo('[Main] Set Torque : {}%'.format(torque_lvl))
# load config file
left_ws = self.load_ws_config('left_arm')
right_ws = self.load_ws_config('right_arm')
self.ik_reference(left_ws, right_ws)
rospy.loginfo("[Main] Save Data: {}".format(
rospy.get_param("/pioneer/placement/save_data")))
while not rospy.is_shutdown():
if self.shutdown:
break
if self.state == 'init_pose':
rospy.loginfo('[Main] Robot State : {}'.format(self.state))
cur_left_arm = kinematics.get_kinematics_pose("left_arm")
cur_right_arm = kinematics.get_kinematics_pose("right_arm")
if cur_left_arm['z'] < 0.7 or cur_right_arm['z'] < 0.7:
self.lx_ik = cur_left_arm['x']
self.ly_ik = cur_left_arm['y'] + 0.07
self.move_arm("left_arm", 2.0, self.lx_ik, self.ly_ik)
self.rx_ik = cur_right_arm['x']
self.ry_ik = cur_right_arm['y'] - 0.07
self.move_arm("right_arm", 2.0, self.rx_ik, self.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
self.lx_ik = self.ly_ik = self.rx_ik = self.ry_ik = None
self.state = None
elif self.state == 'approach_keyboard':
rospy.loginfo('[Main] Robot State : {}'.format(self.state))
# sleep(1)
if self.lx_ik != None and self.ly_ik != None \
and self.rx_ik != None and self.ry_ik != None:
# print('move arm now')
self.ly_ik += 0.055
self.lx_ik -= 0.0
self.move_arm("left_arm", 2.0, self.lx_ik, self.ly_ik)
self.ry_ik -= 0.055
self.rx_ik -= 0.0
self.move_arm("right_arm", 2.0, self.rx_ik, self.ry_ik)
else:
rospy.logwarn(
'[Main] Robot arm singularities \n Please move keyboard to workspace'
)
self.state = None
elif self.state == 'grip_keyboard':
rospy.loginfo('[Main] Robot State : {}'.format(self.state))
# grip by offseting ik <-- not good when change object
cur_left_arm = kinematics.get_kinematics_pose("left_arm")
cur_right_arm = kinematics.get_kinematics_pose("right_arm")
self.lx_ik = cur_left_arm['x']
self.ly_ik = cur_left_arm['y'] - 0.06
self.rx_ik = cur_right_arm['x']
self.ry_ik = cur_right_arm['y'] + 0.06
self.move_arm("left_arm", 2.0, self.lx_ik, self.ly_ik)
self.move_arm("right_arm", 2.0, self.rx_ik, self.ry_ik)
##-----------------------------
## grip by offseting pixel
# offset_pixel_x = 35
# self.left_keyboard = (self.left_keyboard[0]+offset_pixel_x, self.left_keyboard[1]) # x. y
# self.lx_ik, self.ly_ik = self.left_arm_ik(self.left_keyboard[0], self.left_keyboard[1])
# self.right_keyboard = (self.right_keyboard[0]-offset_pixel_x, self.right_keyboard[1]) # x. y
# self.rx_ik, self.ry_ik = self.right_arm_ik(self.right_keyboard[0], self.right_keyboard[1])
# self.move_arm("left_arm" , 2.0, self.lx_ik, self.ly_ik)
# self.move_arm("right_arm" , 2.0, self.rx_ik, self.ry_ik)
self.state = None
elif self.state == 'placement_trigger':
rospy.loginfo('[Main] Robot State : {}'.format(self.state))
if self.ik_l_trajectory != None and self.ik_r_trajectory != None:
if self.ik_debug:
for i in range(self.l_num_points):
rospy.loginfo('frame_l: {}, {}'.format(
self.l_frame_tra[i][0],
self.l_frame_tra[i][1]))
for i in range(self.r_num_points):
rospy.loginfo('frame_r: {}, {}'.format(
self.r_frame_tra[i][0],
self.r_frame_tra[i][1]))
for i in range(self.l_num_points):
rospy.loginfo('ik_l: {}'.format(
self.ik_l_trajectory[i]))
for i in range(self.r_num_points):
rospy.loginfo('ik_r: {}'.format(
self.ik_r_trajectory[i]))
ikl_res = []
for val in self.ik_l_trajectory:
if val != (None, None):
ikl_res.append(val)
else:
break
ikr_res = []
for val in self.ik_r_trajectory:
if val != (None, None):
ikr_res.append(val)
else:
break
lim_trajectory = min(len(ikl_res), len(ikr_res))
if self.ik_debug:
for i in range(lim_trajectory):
# rospy.loginfo( 'ik_l: {}'.format(self.ik_l_trajectory[i]) )
# rospy.loginfo( 'ik_r: {}'.format(self.ik_r_trajectory[i]) )
# input("Press enter to continue..")
print()
self.move_arm("left_arm", 1.0,
self.ik_l_trajectory[i][0],
self.ik_l_trajectory[i][1])
self.move_arm("right_arm", 1.0,
self.ik_r_trajectory[i][0],
self.ik_r_trajectory[i][1])
sleep(1.5)
else:
ik_l_trajectory = np.array(self.ik_l_trajectory)
xl = ik_l_trajectory[:lim_trajectory, 0]
yl = ik_l_trajectory[:lim_trajectory, 1]
zl = np.full(lim_trajectory, self.zl)
r_l = np.full(lim_trajectory, self.rl)
p_l = np.full(lim_trajectory, self.pl)
y_l = np.full(lim_trajectory, self.yl)
ik_r_trajectory = np.array(self.ik_r_trajectory)
xr = ik_r_trajectory[:lim_trajectory, 0]
yr = ik_r_trajectory[:lim_trajectory, 1]
zr = np.full(lim_trajectory, self.zr)
r_r = np.full(lim_trajectory, self.rr)
p_r = np.full(lim_trajectory, self.pr)
y_r = np.full(lim_trajectory, self.yr)
# rospy.loginfo('[Main] ik_l_trajectory : {}'.format(ik_l_trajectory))
# rospy.loginfo('[Main] ik_r_trajectory : {}'.format(ik_r_trajectory))
# sleep(0.2)
kinematics.set_kinematics_arr_pose(
"left_arm", 0.01, **{
'total': lim_trajectory,
'x': xl,
'y': yl,
'z': zl,
'roll': r_l,
'pitch': p_l,
'yaw': y_l
})
kinematics.set_kinematics_arr_pose(
"right_arm", 0.01, **{
'total': lim_trajectory,
'x': xr,
'y': yr,
'z': zr,
'roll': r_r,
'pitch': p_r,
'yaw': y_r
})
sleep(1)
while kinematics.left_arr == True and kinematics.right_arr == True:
pass
sleep(2)
rospy.loginfo('[Main] Finish placement ...')
self.finish_placement_pub.publish(True)
self.state = None
self.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', 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)
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
##################
## 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()
class magic():
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)
#sleep(1)
#print('finished')
# nan = None
# self.calibration(ranges)
def run(self):
self.state = 'first_scene'
# self.state = 'first_step'
# self.state = ''
rospy.Subscriber('/robotis/sensor/scan_filtered', LaserScan, self.sensor_callback)
while (True):
if self.sensor_list is not None :
break
temp_mp3 = mp3()
temp_mp3.file_name = '100) open'
temp_mp3.volume = 10
self.music.publish(temp_mp3)
input('press to start')
# self.move_head()
# sleep(2)
while not rospy.is_shutdown():
if self.state == 'first_scene':
self.text_to_speech("1) Welcome everyone, I am Thormang.")
sleep(0.5)
self.text_to_speech("2) Today I am going to show you a magic trick that I have laerned recently")
sleep(0.5)
self.text_to_speech("3) As you can see, there are three paper bags in front of me.")
sleep(0.5)
self.text_to_speech("4) Later, I will put this wooden base with a sharp nail into one of the paper bags ")
sleep(0.5)
self.text_to_speech("74) and then swap around the position of the bags")
sleep(0.5)
self.text_to_speech("5) To make sure we wont find out the bag is empty by feeling the weight, so I prepared two wooden bases without nails.")
sleep(0.5)
input('press to look student')
self.move_head(self.student_pan,self.student_tilt)
sleep(0.5)
self.text_to_speech("6) Student, please help me put these two wooden bases into two paper bags and close it.")
sleep(0.5)
input("after student move")
self.move_head(self.head_pan,self.head_tilt)
self.state = 'second_scene'
elif self.state == 'second_scene':
input("press to look student again")
self.move_head(self.student_pan,self.student_tilt)
sleep(0.5)
self.text_to_speech("7) student, please ask an audience member to examine the nail and make sure it is real")
sleep(1)
self.move_head(0,0)
input('wait for check the nail')
self.text_to_speech("8) So, audience volunteer can you confirm to the rest of the audience that the nail is indeed real?")
input('after responced ,press to continue')
self.text_to_speech("9) Okay, thank you")
sleep(0.5)
self.move_head(self.student_pan,self.student_tilt)
sleep(0.5)
self.text_to_speech("77) student please place the nail inside the last bag and close it.")
sleep(1.5)
self.move_head(0,0)
self.text_to_speech("10) And everyone, please pay attention and remember which bag contains the nail.")
sleep(0.5)
sleep(1)
input('wait for student action , press to continue')
#self.move_head(0,0)
self.state = "third_scene"
elif self.state == "third_scene":
#input('finished swap ,press to continue')
sleep(1)
self.text_to_speech("11) Now, I need another volunteer.")
input('press to continue')
self.text_to_speech("12) Thank you for volunteering ")
sleep(0.5)
self.text_to_speech("79) now , do you remember which bag contains the nail ?")
input('after volunteer answered , press to continue')
self.move_head(self.head_pan,self.head_tilt)
sleep(2)
# ranges = self.sensor_list [:]
# self.calibration(ranges)
while (True):
ranges = self.sensor_list [:]
self.calibration(ranges)
det = input('detect well ? (y/n)')
if det == "y":
break
self.right_p1_up()
self.move_head(0,0)
self.text_to_speech("13) You mean this one , right?")
input('press to continue')
self.text_to_speech("14) To make sure that I wont be able to know which bag contains the nail")
sleep(0.5)
self.text_to_speech("15) the student will start swapping the bags around .")
sleep(0.5)
#input('press to continue')
self.text_to_speech("60) When I say START the student will begin, and volunteer, you can shout out STOP anytime you want.")
sleep(0.5)
self.kinematics.publisher_(self.kinematics.send_ini_pose_msg_pub, "ini_pose", latch=True)
self.end_action()
self.move_head(self.head_pan,self.head_tilt)
sleep(0.2)
self.text_to_speech("16) Lets START!")
self.music_stop.publish(True)
sleep(0.5)
temp_mp3 = mp3()
temp_mp3.file_name = '200) start game'
temp_mp3.volume = 15
self.music.publish(temp_mp3)
input('after swap ,press to detect the bag position')
self.text_to_speech("17) Okay, now I need some time to feel where the nail is.")
sleep(0.5)
self.state = 'fourth_scene'
elif self.state == 'fourth_scene':
sleep(2)
while (True):
ranges = self.sensor_list [:]
self.calibration(ranges)
det = input('detect well ? (y/n)')
if det == "y":
break
self.p2_up()
sleep(2)
self.p1_up()
sleep(2)
self.p3_up()
sleep(2)
self.p2_up()
point_back = self.kinematics.get_kinematics_pose('left_arm').copy()
point_back["x"] = 0.4
self.kinematics.set_kinematics_pose(group_name="left_arm",time=3,**point_back)
self.end_action()
self.kinematics.publisher_(self.kinematics.send_ini_pose_msg_pub, "ini_pose", latch=True)
self.end_action()
self.move_head(0,0)
self.text_to_speech("18) Ohh, I am 100 percent sure where the nail is. So I would like to play something much more interesting.")
sleep(2)
self.move_head(self.J_pan,0)
self.text_to_speech("19) Jacky")
sleep(0.5)
input('jacky come to the stage')
self.move_head(self.J_pan,self.J_tilt)
self.text_to_speech("20) as the supervisor of your lab, do you believe in my talent and your students ability?")
input('after Jacky worlds ,press to continue')
self.text_to_speech("61) oh , im happy to hear that !")
self.text_to_speech("21) Since you trust us so much ")
sleep(0.2)
self.text_to_speech("66) we can raise the stakes.")
sleep(0.7)
self.text_to_speech("65) How about we use your hand to press down on the paper bags? ")
input("jacky's time")
self.p2_up()
self.text_to_speech("80) Please give me your hand, Jacky")
sleep(0.5)
input('press to continue')
self.move_head(0,0)
self.text_to_speech("22) volunteer, choose a bag you think there's no nail inside? Left, Right, or Middle?")
sleep(0.5)
self.text_to_speech("62) Choose wisely, we wouldn’t want our dear professor to get hurt, right ?")
self.state = "first_step"
elif self.state == 'first_step':
situation = input('put the situation (p3_l , p3_r , p1_l , p1_r) >> (1,2,3,4) :')
self.move_head(self.head_pan,self.head_tilt)
# while (True):
# sleep(0.5)
# ranges = self.sensor_list [:]
# self.calibration(ranges)
# det = input('detect well ? (y/n)')
# if det == "y":
# break
if situation == '1' : #"p3_l":
self.text_to_speech("23) Okay, left.")
self.p3_up()
self.speech.publish("24) Safe!")
sleep(0.2)
self.DownAndUp()
self.last = 'p1'
elif situation == "2" : #"p3_r":
self.text_to_speech("25) Okay, right.")
self.p3_up()
self.speech.publish("24) Safe!")
sleep(0.2)
self.DownAndUp()
self.last = 'p1'
elif situation == "3" : #"p1_l":
self.text_to_speech("23) Okay,left.")
self.p1_up()
self.speech.publish("24) Safe!")
sleep(0.2)
self.DownAndUp()
self.last = 'p3'
elif situation == "4" : #"p1_r":
self.text_to_speech("25) Okay, right.")
self.p1_up()
self.speech.publish("24) Safe!")
sleep(0.2)
self.DownAndUp()
self.last = 'p3'
else:
print("input should be 1 , 2 , 3 ,4 >> (p3_l , p3_r , p1_l , p1_r) ")
continue
self.move_head(self.J_pan,self.J_tilt)
self.text_to_speech("28) Jacky, dont be that nervous, dont you trust me?")
sleep(0.5)
input('press to continue')
self.text_to_speech("29) I am the only hope of graduation for your students")
sleep(0.7)
self.text_to_speech("68) they won't disappoint you , trust me !")
sleep(0.5)
self.state = 'second_step'
# if self.sensor_list is not None:
# #print(self.sensor_list)
# ranges = self.sensor_list [:]
# self.state = 'second_step'
# #print('first finished')
# # break
elif self.state == 'second_step':
#input("press to continue")
#self.p2_up()
input('press to continue')
self.move_head(0,0)
sleep(0.3)
self.text_to_speech("30) Hey, volunteer, what's your next choice? Left or Right?")
l_r = input('left or right ( l , r ) : ')
if l_r == "l":
self.speech.publish("31) okay, left")
else :
self.speech.publish("32) okay, right")
if self.last == 'p1':
self.p1_up()
elif self.last == 'p3':
self.p3_up()
self.music_stop.publish(True)
sleep(0.2)
################################################################################################################################
###########thormang3_crash#############
temp_mp3 = mp3()
temp_mp3.file_name = '101) restart'
temp_mp3.volume = 25
self.music.publish(temp_mp3)
sleep(0.2)
self.motor.publisher_(self.motor.module_control_pub, "none", latch=True)
self.motor.set_joint_states(['all'], False)
rospy.loginfo('[Magic] Turn off torque')
###################################
sleep(1)
self.speech.publish("41) re re re staaar rrr t teng ")
sleep(2)
input ("trigger")
##################################
self.motor.set_joint_states(['all'], True)
rospy.loginfo('[Magic] Turn on torque')
###############
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.module_control_pub, "head_control_module", latch=True)
temp_mp3 = mp3()
temp_mp3.file_name = '201) start game'
temp_mp3.volume = 10
self.music.publish(temp_mp3)
sleep(4)
self.speech.publish("38) Finished rebooting ")
# self.kinematics.publisher_(self.kinematics.send_ini_pose_msg_pub, "ini_pose", latch=True)
# self.end_action()
################################################################################################################################
if self.last == 'p1':
self.p1_up()
elif self.last == 'p3':
self.p3_up()
self.speech.publish("39) Okay , this one ?")
sleep(0.5)
self.speech.publish("40) but i dont think this one is safe ")
sleep(2)
self.p2_up()
sleep(0.2)
self.move_head(self.J_pan,self.J_tilt)
self.speech.publish("33) Jacky , do you trust me? ")
sleep(2.5)
input('press to continue')
self.music_stop.publish(True)
temp_mp3 = mp3()
temp_mp3.file_name = '306) feel sad'
temp_mp3.volume = 15
self.music.publish(temp_mp3)
sleep(7)
self.text_to_speech("70) why? i only made one mistake ")
sleep(0.2)
self.text_to_speech("71) why dont you give me another chance")
self.speech.publish("50) well ")
sleep(0.5)
self.text_to_speech("72) but i dont care !")
sleep(0.2)
self.DownAndUp()
# self.music_stop.publish(True)
# temp_mp3 = mp3()
# temp_mp3.file_name = '300) start game'
# temp_mp3.volume = 15
# self.music.publish(temp_mp3)
sleep(0.5)
self.move_head(0,0)
self.state = 'third_step'
# # calculating position
# self.calibration(ranges)
# #print("second_finished")
# self.state = 'third_step'
# #break
elif self.state == 'third_step' :
sleep(0.5)
self.text_to_speech("34) Yeah, got you! HA HA HA , Don't be that scared")
sleep(0.5)
self.text_to_speech("69) I know what I am doing")
sleep(0.5)
input('grab the bag')
self.text_to_speech("35) So, you might be really curious where the actual nail is, right?")
sleep(0.5)
#input('press to continue')
if self.last == 'p1':
self.p1_up()
elif self.last == 'p3':
self.p3_up()
self.grab_bag()
sleep(0.5)
input('press to make thormann grip off')
self.grip_off('l_arm_grip')
# input('press to continue')
# self.text_to_speech("")
input('press to continue')
self.move_head(0,0)
self.kinematics.publisher_(self.kinematics.send_ini_pose_msg_pub, "ini_pose", latch=True)
self.end_action()
self.grip_on('l_arm_grip')
self.text_to_speech("37) Thanks for your attention, I am Thormang wolf from ERC lab, see you next time")
print("End Magic")
self.kinematics.kill_threads()
break
rospy.Rate(60).sleep()
def sensor_callback(self,msg):
self.sensor_list = np.array(msg.ranges)
def read_sensor(self):
rospy.Subscriber('/robotis/sensor/scan_filtered', LaserScan, self.sensor_callback)
while not rospy.is_shutdown():
if self.sensor_list is not None:
break
# # else:
# # print(self.sensor_list)
self.rate.sleep()
def calibration(self,ranges,deg = 60,threshold = 0.78):
nan = None
#print(len(ranges))
calibration = []
#print(self.deg_30)
per_deg = (self.deg/len(ranges) )* (math.pi/180)
#print(per_deg)
for i in range(len(ranges)):
if ranges[i] == None :
pass
else:
y = ranges[i] * math.cos(i*per_deg - (self.deg_30 - self.head_pan) ) * math.cos(self.head_tilt)
if y < threshold :
x = -ranges[i] * math.sin( (self.deg_30 - self.head_pan) - i*per_deg) * math.cos(self.head_tilt)
calibration.append([round(x,5),round(y,5),i])
calibration = np.array(calibration)
#print(len(calibration))
#print(calibration)
mean = sum(calibration)/len(calibration)
valid = []
for i in range(len(calibration)):
if calibration[i][1] < mean[1]:
valid.append(calibration[i])
valid = np.array(valid)
#print(valid)
##K-means n_clusters=3
estimator = KMeans(n_clusters=3)
res = estimator.fit_predict(valid[:,0:2])
#print(res)
lable_pred = estimator.labels_
#print (lable_pred)
entroids = estimator.cluster_centers_
data = entroids[:]
data = np.sort(data,axis = 0)
print (data)
self.point1 = {'x':0.52,'y':data[0,0]+0.02,'z':1.05,'roll':0,'pitch':0,'yaw':-25}
self.point2 = {'x':0.52,'y':data[1,0]+0.03,'z':1.05,'roll':0,'pitch':0,'yaw':0}
self.point3 = {'x':0.52,'y':data[2,0]+0.07,'z':1.05,'roll':0,'pitch':0,'yaw':30}
self.right_point ={'x':0.52,'y': (data[0,0]-0.06),'z':1.00,'roll':0,'pitch':20,'yaw':30}
if self.right_point["y"] > 0.05:
self.right_point["y"] = 0
print("point1 : y = ",self.point1["y"] )
print("point2 : y = ",self.point2["y"] )
print("point3 : y = ",self.point3["y"] )
print("right_y : ",self.right_point["y"])
# plt.figure(figsize=(12,6))
# plt.plot(calibration[:,0],calibration[:,1],'bo')
# plt.plot(valid[:,0],valid[:,1],'go')
# plt.plot(data[:,0],data[:,1],'ro')
# plt.show()
def text_to_speech(self,worlds):
self.speech.publish(worlds)
sleep(3.5)
def move_head(self,pan,tilt):
msg = JointState()
msg.name = ['head_y','head_p']
msg.position = [pan ,tilt]
msg.velocity = [0]
msg.effort = [0]
msg.header.frame_id = ''
msg.header.stamp.secs = 0
msg.header.stamp.nsecs= 0
for i in range(4):
msg.header.seq = i
self.head_postition.publish(msg)
self.rate.sleep()
def end_action(self):
sleep(0.5)
while not self.kinematics.status_msg == 'End Trajectory' :
sleep(1)
if self.kinematics.status_msg == 'Finish Init Pose':
break
sleep(0.5)
def right_p1_up(self):
self.kinematics.set_kinematics_pose(group_name="right_arm",time=3,**self.right_point) #x=0.52,y=0,z=1.1,roll=0,pitch=0,yaw=-40)
self.end_action()
def p1_up(self):
self.kinematics.set_kinematics_pose(group_name="left_arm",time=3,**self.point1) #x=0.52,y=0,z=1.1,roll=0,pitch=0,yaw=-40)
self.end_action()
def p2_up(self):
self.kinematics.set_kinematics_pose(group_name="left_arm",time=3,**self.point2) #x=0.52,y=0.2,z=1.1,roll=0,pitch=0,yaw=0)
self.end_action()
def p3_up(self):
self.kinematics.set_kinematics_pose(group_name="left_arm",time=3,**self.point3) #x=0.52,y=0.4,z=1.1,roll=0,pitch=0,yaw=40)
self.end_action()
def grab_bag(self):
#sleep(2)
self.grip_off('l_arm_grip')
self.end_action()
point = self.kinematics.get_kinematics_pose('left_arm').copy()
point_down = point.copy()
point_down["z"] = 0.85
point["x"] = 0.40
point["z"] = 1.05
point_ini = self.left_arm_ini_pos.copy()
point_ini["z"] = 0.92
left_side = {'x':0.28,'y': 0.55,'z':0.950,'roll':0,'pitch':0,'yaw':30.0}
# right_center = {'x':0.36,'y': -0.03,'z':0.820,'roll':0,'pitch':0,'yaw':60.0}
# right_split = {'x':0.32,'y': -0.16,'z':0.820,'roll':0,'pitch':0,'yaw':60.0}
# right_up = {'x':0.4,'y': -0.02,'z':0.82,'roll':0,'pitch':0,'yaw':20}
#print('point_down',point_down)
#print('point',point)
self.kinematics.set_kinematics_pose(group_name="left_arm",time=2,**point)
self.end_action()
point["z"] = 0.85
self.kinematics.set_kinematics_pose(group_name="left_arm",time=2,**point)
self.end_action()
self.kinematics.set_kinematics_pose(group_name="left_arm",time=2,**point_down)
self.end_action()
self.grip_on('l_arm_grip')
# sleep(0.2)
self.kinematics.set_kinematics_pose(group_name="left_arm",time=2,**point)
self.end_action()
# self.kinematics.set_kinematics_pose(group_name="left_arm",time=3,**point_ini)
# self.end_action()
self.move_head(self.J_pan,self.J_tilt)
self.text_to_speech("36) Jacky , please help me to check what is in the bag.")
##grab bag to the center
self.kinematics.set_kinematics_pose(group_name="left_arm",time=4,**left_side)
self.end_action()
##split
# self.grip_off("r_arm_grip")
# self.kinematics.set_kinematics_pose(group_name="right_arm",time=3,**right_center)
# self.end_action()
# self.grip_on("r_arm_grip")
# self.kinematics.set_kinematics_pose(group_name="right_arm",time=3,**right_split)
# self.end_action()
# self.grip_half()
# self.kinematics.set_kinematics_pose(group_name="right_arm",time=3,**right_up)
# self.end_action()
def DownAndUp(self):
#sleep(2)
point = self.kinematics.get_kinematics_pose('left_arm')
point_down = point.copy()
point_down["z"] = 0.82
#print('point_down',point_down)
#print('point',point)
self.kinematics.set_kinematics_pose(group_name="left_arm",time=2,**point_down)
self.end_action()
self.kinematics.set_kinematics_pose(group_name="left_arm",time=3,**point)
self.end_action()
def finish(self):
pass
def grip_off(self,group):
msg = JointState()
msg.name = [group]
msg.position = [0]
msg.velocity = [0]
msg.effort = [250]
msg.header.frame_id = ''
msg.header.stamp.secs = 0
msg.header.stamp.nsecs= 0
for i in range(4):
msg.header.seq = i
self.pub_gripper.publish(msg)
self.rate.sleep()
#print(i)
self.end_action()
def grip_half(self,group):
msg = JointState()
msg.name = [group]
msg.position = [0.6]
msg.velocity = [0]
msg.effort = [250]
msg.header.frame_id = ''
msg.header.stamp.secs = 0
msg.header.stamp.nsecs= 0
for i in range(4):
msg.header.seq = i
self.pub_gripper.publish(msg)
self.rate.sleep()
#print(i)
self.end_action()
def grip_on(self,group):
msg = JointState()
msg.name = [group]
msg.position = [1.15]
msg.velocity = [0]
msg.effort = [250]
msg.header.frame_id = ''
msg.header.stamp.secs = 0
msg.header.stamp.nsecs= 0
for i in range(4):
msg.header.seq = i
self.pub_gripper.publish(msg)
self.rate.sleep()
#print(i)
self.end_action()