def Obj_Data_Calculation(center_X,center_Y,height_Z): #Enter the number of objects that have been picked and place
global objects_picked_num,target_base_avoidance,target_base_above_avoidance
baseRequest = eye2baseRequest()
center_X = center_X*100
center_Y = center_Y*100
height_Z = height_Z*100
baseRequest.ini_pose = [center_X,center_Y,height_Z]
target_base = pixel_z_to_base_client(baseRequest) #[x,y,z]
## Increase four sides obstacle avoidance, posture conversion 0918
#general A posture set
A_posture = 0
### Avoid singularities for pushpin mission
# if target_base[0] >= 30.5 and target_base[1] > 19.5 and target_base[1] <= 39.5:
# A_posture = 20
# if target_base[0] >= 30.5 and target_base[1] >= 19.5 and target_base[1] <= 39.5:
# A_posture = 20
if target_base[0] >= 30.5 and target_base[1] > 30 and target_base[1] <= 45:
A_posture = 20
if target_base[0] >= 30.5 and target_base[1] <= 30 and target_base[1] >= 9:
A_posture = -20
A_posture = 180 + A_posture
## ------0918
avoidRequest_above = collision_avoidRequest()
avoidRequest_above.ini_pose = [target_base[0],target_base[1],target_base[2],A_posture,0,0] ##0919
avoidRequest_above.limit = 0.1 # test
avoidRequest_above.dis = 15 # test
target_base_above_avoidance = base_avoidance_client(avoidRequest_above)
avoidRequest = collision_avoidRequest()
avoidRequest.ini_pose = [target_base[0],target_base[1],target_base[2],A_posture,0,0] ##0919
avoidRequest.limit = 0.1 # test
avoidRequest.dis = 0 # test
target_base_avoidance = base_avoidance_client(avoidRequest)
def Mission_Trigger(self):
if self.arm_move == True and robot_ctr.get_robot_motion_state(
) == Arm_status.Isbusy:
self.arm_move = False
# if Arm_state_flag == Arm_status.Idle and Sent_data_flag == 1:
if robot_ctr.get_robot_motion_state(
) == Arm_status.Idle and self.arm_move == False:
if self.state == State.move:
print('ffffffffffffffffffffffffffffffffffffffffffffffff')
pos = self.pos[0]
# position = [pos[0], pos[1], pos[2], pos[3], pos[4], pos[5]]
req = collision_avoidRequest()
req.ini_pose = pos
req.limit = 2
req.dis = 0
position = np.array(self.easy_CA_client(req).tar_pose)
print(position)
robot_ctr.Set_ptp_speed(30)
robot_ctr.Step_AbsLine_PosCmd(position)
self.pos = np.delete(self.pos, 0, 0)
self.state = State.take_pic
self.arm_move = True
elif self.state == State.take_pic:
robot_ctr.Set_ptp_speed(80)
robot_ctr.Step_AbsPTPCmd([16.5, 29.2, 3, -180, 0, 0])
if len(self.pos) > 0:
self.state = State.move
else:
self.state = State.finish
def Obj_Data_Calculation():
baseRequest = eye2baseRequest()
baseRequest.ini_pose = [boxes.x, boxes.y, camera_z]
target_base = pixel_z_to_base_client(baseRequest) #[x,y,z]
avoidRequest = collision_avoidRequest()
avoidRequest.ini_pose = [
target_base[0], target_base[1], target_base[2], 180, 0, 0
]
avoidRequest.limit = 1 # test
avoidRequest.dis = 10 # test
target_base_avoidance = base_avoidance_client(avoidRequest)
# print("trans_pixel_to_base:",baseRequest.ini_pose)
print("target_base:", target_base)
print("target_base_avoidance:", target_base_avoidance)
def Obj_Data_Calculation(
): #Enter the number of objects that have been picked and place
global objects_picked_num, target_base_avoidance
baseRequest = eye2baseRequest()
baseRequest.ini_pose = [boxes.x, boxes.y, camera_z]
target_base = pixel_z_to_base_client(baseRequest) #[x,y,z]
avoidRequest = collision_avoidRequest()
avoidRequest.ini_pose = [
target_base[0], target_base[1], target_base[2], 180, 0, 0
]
avoidRequest.limit = 0.1 # test
avoidRequest.dis = 0 # test
target_base_avoidance = base_avoidance_client(avoidRequest)
print("target_base:", target_base)
print("target_base_avoidance:", target_base_avoidance)
def Mission_Trigger(self):
if self.arm_move == True and robot_ctr.get_robot_motion_state(
) == Arm_status.Isbusy:
self.arm_move = False
# if Arm_state_flag == Arm_status.Idle and Sent_data_flag == 1:
if robot_ctr.get_robot_motion_state(
) == Arm_status.Idle and self.arm_move == False:
if self.state == State.move:
pos = self.pos[0]
# position = [pos[0], pos[1], pos[2], pos[3], pos[4], pos[5]]
req = collision_avoidRequest()
req.ini_pose = pos
req.limit = 5
req.dis = 3
res = self.CA_client(req)
position = np.array(res.tar_pose)
req = tool_angleRequest()
req.angle = str(res.suc_angle)
self.tool_angle_client(res.suc_angle)
robot_ctr.Set_ptp_speed(30)
robot_ctr.Step_AbsPTPCmd(position)
self.pos = np.delete(self.pos, 0, 0)
self.state = State.take_pic
self.arm_move = True
elif self.state == State.take_pic:
robot_ctr.Set_ptp_speed(30)
robot_ctr.Step_AbsPTPCmd([16.5, 29.2, 3, -180, 0, 0])
if len(self.pos) > 0:
self.state = State.move
else:
self.state = State.finish
req = ax_caliRequest()
req.cmd = 'dis'
self.tool_cali_client(req)
return
req = tool_angleRequest()
req.angle = 0
self.tool_angle_client(req)
def MotionItem(ItemNo):
global SpeedValue, PushFlag, MissionEndFlag, CurrentMissionType, MotionStep, objects_picked_num, obj_num, MissionType_Flag
global target_base_avoidance, target_base_above_avoidance, arm_down_pick_flag, Stop_motion_flag, Absort_fail_to_Get_image
global LineDown_Speed, ArmGernel_Speed
for case in switch(ItemNo):
if case(Arm_cmd.Arm_Stop):
print("Arm_Stop")
break
if case(Arm_cmd.MoveToObj_Pick1):
positon = [
target_base_above_avoidance[0], target_base_above_avoidance[1],
target_base_above_avoidance[2], target_base_above_avoidance[3],
target_base_above_avoidance[4], target_base_above_avoidance[5]
] ###target obj position
robot_ctr.Step_AbsPTPCmd(positon)
print("MoveToObj_Pick1")
MotionStep += 1
break
if case(Arm_cmd.MoveToObj_Pick2):
positon = [
target_base_avoidance[0], target_base_avoidance[1], -26,
target_base_avoidance[3], target_base_avoidance[4],
target_base_avoidance[5]
] ###target obj position
robot_ctr.Step_AbsLine_PosCmd(positon, 0, 10)
robot_ctr.Set_override_ratio(LineDown_Speed) ##speed low
arm_down_pick_flag = True
robot_ctr.Set_digital_output(1, True) # Absort_ON
print("MoveToObj_Pick2")
MotionStep += 1
break
if case(Arm_cmd.Absort_Check):
robot_inputs_state = robot_ctr.Get_current_robot_inputs(
) # Determine whether the object is sucked
if robot_inputs_state[0] == True: # is digital IO input 1 pin
print("Absort success check and mission continue")
MotionStep += 1
else:
print("Absort fail and mission continue to Get image")
'''''
1.Suck next object
MissionType_Flag = pick
2.If there is no next object, take another photo
''' ''
robot_ctr.Set_digital_output(1, False) # Absort_OFF
MissionType_Flag = MissionType.Get_Img
GetKeyFlag = True
ExecuteFlag = False
MotionStep += 1 # tmp
break
if case(Arm_cmd.MoveToObj_PickUp):
# time.sleep(0.3) # pause pick for check sucker ready
if Stop_motion_flag == True: #There are early pick up items
# Stop_motion_flag = False
print("Absort success fucccccckkkkk")
# MotionStep += 1
# else: # Did not pick up items early
positon = [
target_base_above_avoidance[0], target_base_above_avoidance[1],
target_base_above_avoidance[2], target_base_above_avoidance[3],
target_base_above_avoidance[4], target_base_above_avoidance[5]
] ###target obj position
robot_ctr.Step_AbsLine_PosCmd(positon, 0, 10)
robot_ctr.Set_override_ratio(ArmGernel_Speed)
arm_down_pick_flag = False #Initialize the flag to determine the next action
print("MoveToObj_PickUp")
MotionStep += 1
break
if case(Arm_cmd.MoveToTarget_Place):
positon = [16.2, 10, 10, -180, 0, 0]
robot_ctr.Step_AbsPTPCmd(positon)
positon = [16.2, -11.3, 10, -180, 0, 0]
robot_ctr.Step_AbsPTPCmd(positon)
print("MoveToTarget_Place")
MotionStep += 1
break
if case(Arm_cmd.Absort_OFF):
robot_ctr.Set_digital_output(1, False)
time.sleep(0.1)
print("Absort_OFF")
MotionStep += 1
break
if case(Arm_cmd.MoveToTarget_PlaceUp):
positon = [16.2, 10, 10, -180, 0, 0]
robot_ctr.Step_AbsPTPCmd(positon)
MotionStep += 1
break
if case(Arm_cmd.Go_Image1):
CurrentMissionType = MissionType.Get_Img
### test take pic point(1)
positon = [11.3440, 26.4321, 11.23, 179.994, 10.002, -0.488]
robot_ctr.Step_AbsPTPCmd(positon)
# time.sleep(20) ### test 9/16
MotionStep += 1
break
if case(Arm_cmd.Go_Image2):
CurrentMissionType = MissionType.Get_Img2
baseRequest = eye2baseRequest()
baseRequest.ini_pose = [boxes.x, boxes.y, camera_z]
Get_Image_Point_Base = pixel_z_to_base_client(
baseRequest) #[x,y,z]
avoidRequest_Get_Image_Point = collision_avoidRequest()
avoidRequest_Get_Image_Point.ini_pose = [
Get_Image_Point_Base[0] - 7, Get_Image_Point_Base[1],
Get_Image_Point_Base[2], 180, 10, 0
]
avoidRequest_Get_Image_Point.limit = 0.1 # test
avoidRequest_Get_Image_Point.dis = 30 # test
Get_Image_Point_base_avoidance = base_avoidance_client(
avoidRequest_Get_Image_Point)
positon = [
Get_Image_Point_base_avoidance[0],
Get_Image_Point_base_avoidance[1],
Get_Image_Point_base_avoidance[2],
Get_Image_Point_base_avoidance[3],
Get_Image_Point_base_avoidance[4],
Get_Image_Point_base_avoidance[5]
]
robot_ctr.Step_AbsPTPCmd(positon)
MotionStep += 1
break
if case(Arm_cmd.Get_Image):
CurrentMissionType = MissionType.Get_Img
### test take pic
time.sleep(0.3) # Delayed time to see
Obj_Data_Calculation()
if obj_num == 0: # If you don't see the object,Mission End
MissionType_Flag = MissionType.Mission_End
# robot_ctr.Stop_motion() #That is, it is sucked and started to place
print("mission end")
else: # Have seen the object, take the action
MissionType_Flag = MissionType.Pick
# print("Get_Image success")
'''''' '''''
1.If the area object is not finished
2.If there is no next object, take next photo spot
MissionType_Flag = Get_Img2
3.If next photo spot is elso noing, end of mission
If ob_num == 0
''' '''''' ''
MotionStep += 1
break
if case(Arm_cmd.Go_back_home):
robot_ctr.Set_operation_mode(0)
robot_ctr.Go_home()
print("MissionEnd")
MotionStep += 1
break
if case():
print("something else!")
def Obj_Data_Calculation(
): #Enter the number of objects that have been picked and place
global objects_picked_num, target_base_avoidance, target_base_above_avoidance
baseRequest = eye2baseRequest()
baseRequest.ini_pose = [boxes.x, boxes.y, camera_z]
target_base = pixel_z_to_base_client(baseRequest) #[x,y,z]
## Increase four sides obstacle avoidance, posture conversion 0918
## The inner length and width of the box 59*39cm
# Limit 8 cm
#general B posture set ,C posture set
C_posture = 0
A_posture = 0
# tool right
# if target_base[1] >= 48.5 and target_base[0] >= 30.5: #ahead #left
# C_posture = -45
# elif target_base[1] <= 9 and target_base[0] >= 30.5: #ahead #right
# C_posture = -135
# elif target_base[0] <= 9 and target_base[1] >= 48.5: #left #rear
# C_posture = 45
# elif target_base[0] <= 9 and target_base[1] <= 9: #right#rear
# C_posture = 135
# elif target_base[1] >= 48.5: #left
# C_posture = 0
# elif target_base[1] <= 9: #right
# C_posture = -180
# elif target_base[0] >= 30.5: #ahead
# C_posture = -90
# elif target_base[0] <= 9: #rear
# C_posture = 90
# ### Avoid singularities for pushpin mission
# if target_base[0] >= 30.5 and target_base[1] > 19.5 and target_base[1] <= 39.5:
# B_posture = -10
# if target_base[0] >= 30.5 and target_base[1] >= 19.5 and target_base[1] <= 39.5:
# B_posture = -10
# tool left
if target_base[1] >= 48.5 and target_base[0] >= 30.5: #ahead #left
C_posture = 135
elif target_base[1] <= 9 and target_base[0] >= 30.5: #ahead #right
C_posture = 45
elif target_base[0] <= 9 and target_base[1] >= 48.5: #left #rear
C_posture = -135
elif target_base[0] <= 9 and target_base[1] <= 9: #right#rear
C_posture = -45
elif target_base[1] >= 48.5: #left
C_posture = -180
elif target_base[1] <= 9: #right
C_posture = 0
elif target_base[0] >= 30.5: #ahead
C_posture = 90
elif target_base[0] <= 9: #rear
C_posture = -90
### Avoid singularities for pushpin mission
if target_base[0] >= 30.5 and target_base[1] > 19.5 and target_base[
1] <= 39.5:
A_posture = 20
if target_base[0] >= 30.5 and target_base[1] >= 19.5 and target_base[
1] <= 39.5:
A_posture = 20
A_posture = 180 + A_posture
## ------0918
avoidRequest_above = collision_avoidRequest()
avoidRequest_above.ini_pose = [
target_base[0], target_base[1], target_base[2], A_posture, 0, C_posture
] ##0919
avoidRequest_above.limit = 0.1 # test
avoidRequest_above.dis = 15 # test
target_base_above_avoidance = base_avoidance_client(avoidRequest_above)
avoidRequest = collision_avoidRequest()
avoidRequest.ini_pose = [
target_base[0], target_base[1], target_base[2], A_posture, 0, C_posture
] ##0919
avoidRequest.limit = 0.1 # test
avoidRequest.dis = 0 # test
target_base_avoidance = base_avoidance_client(avoidRequest)
def Mission_Trigger(self):
if self.arm_move == True and robot_ctr.get_robot_motion_state() == Arm_status.Isbusy:
self.arm_move = False
# if Arm_state_flag == Arm_status.Idle and Sent_data_flag == 1:
if self.monitor_suc == True:
robot_inputs_state = robot_ctr.Get_current_robot_inputs()
if robot_inputs_state[0] == True:
robot_ctr.Stop_motion() #That is, it is sucked and started to place
time.sleep(0.2)
self.monitor_suc = False
self.state = State.pick_obj
# dig_inputs = robot_ctr.Get_current_digital_inputs()
# if dig_inputs[2] == True:
# self.stop_flg = True
# return
# elif dig_inputs[2] == False and self.stop_flg == True:
# self.stop_flg = False
# self.state = State.move2pic
if robot_ctr.get_robot_motion_state() == Arm_status.Idle and self.arm_move == False:
if self.state == State.move2pic:
pos = self.pic_pos[self.pic_pos_indx]
# self.pic_pos_indx += 1
position = [pos[0], pos[1]-3, pos[2], pos[3], pos[4], pos[5]]
# pos[1] -= 3
robot_ctr.Set_ptp_speed(10)
robot_ctr.Step_AbsPTPCmd(position)
self.state = State.get_objinfo
self.arm_move = True
# time.sleep(10)
elif self.state == State.take_pic:
time.sleep(0.2)
req = eye2baseRequest()
req.ini_pose = np.array(np.identity(4)).reshape(-1)
trans = self.hand_eye_client(req).tar_pose
req = save_pcdRequest()
req.curr_trans = np.array(trans)
req.name = 'mdfk'
if self.pic_pos_indx < len(self.pic_pos):
self.state = State.move2pic
req.save_mix = False
else:
self.state = State.get_objinfo
req.save_mix = True
self.get_pcd_client(req)
elif self.state == State.get_objinfo:
time.sleep(0.2)
req = snapshotRequest()
req.call = 0
res = self.get_obj_client(req)
# res = snapshotResponse()
# res.doit = True
# res.type = 0
# t = tf.transformations.rotation_matrix(radians(90), [0, 1, 0], point=None)
# res.trans = np.array([1,0,0,0,0,1,0,0,0,0,1,0.59,0,0,0,1])
# t = np.array([0.746, -0.665, -0.011, 0.172, -0.663, -0.745, 0.066, -0.089, -0.053, -0.042, -0.997, 0.58, 0,0,0,1])
# t = [0.108, 0.977, -0.180, -0.089, -0.993, 0.103, -0.040, -0.183, -0.020, 0.183, 0.982,0.58, 0,0,0,1]
# t = [-0.040, -0.998, 0.033, -0.047, -0.999, 0.040, 0.012, -0.034, -0.013, -0.033, -0.999, 0.58, 0,0,0,1]
# t = [0.906, -0.388, -0.168, -0.243, 0.334, 0.900, -0.278, -0.029, 0.259, 0.196, 0.945, 0.58, 0, 0, 0, 1]
# t = [0.660, 0.750, -0.022, -0.282, -0.750, 0.660, 0.006, 0.146, 0.019, 0.012, 0.999, 0.58,0, 0, 0, 1]
# res.trans = np.array(t).reshape(-1)
# res.trans[11] = 0.58
if res.doit == True:
trans = np.mat(np.asarray(res.trans)).reshape(4,4)
trans[2,3] = 0.59
if res.type == 1:
trans = np.array(trans).reshape(-1)
elif res.type == 2: # y 90
# pre_trans = np.mat([[1., 0, 0, 0],
# [0, 1, 0, 0],
# [0, 0, 1, 0],
# [0, 0, 0, 1]])
pre_trans = tf.transformations.euler_matrix(0, radians(90), 0, axes='sxyz')
# trans = trans * pre_trans
trans = pre_trans * trans
trans = np.array(trans).reshape(-1)
elif res.type == 3: # -90
# pre_trans = np.mat([[1., 0, 0, 0],
# [0, 1, 0, 0],
# [0, 0, 1, 0],
# [0, 0, 0, 1]])
pre_trans = tf.transformations.euler_matrix(0, radians(-90), 0, axes='sxyz')
# trans = trans * pre_trans
trans = pre_trans * trans
print('fucktrans', trans)
trans = np.array(trans).reshape(-1)
req = eye2baseRequest()
req.ini_pose = trans ##
self.target_obj = self.hand_eye_client(req).tar_pose
self.target_obj = np.mat(self.target_obj).reshape(4,4)
self.right_side = self.check_side(self.target_obj)
x, y, z = np.array(np.multiply(self.target_obj[0:3, 3:], 100)).reshape(-1)
a, b, c = [degrees(abc) for abc in tf.transformations.euler_from_matrix(self.target_obj, axes='sxyz')]
self.target_obj = [x, y, z, a, b, c]
print('self.target_obj\n ', self.target_obj)
self.state = State.move2objup
self.pic_pos_indx = 0
else:
# self.pic_pos_indx += 1
self.pic_pos_indx = self.pic_pos_indx if self.pic_pos_indx < len(self.pic_pos) else 0
self.state = State.move2pic
elif self.state == State.move2objup:
req = collision_avoidRequest()
req.ini_pose = np.array(self.target_obj).reshape(-1) ####
req.limit = 1.5
req.dis = 6
res = self.CA_client(req)
pose = np.array(res.tar_pose)
self.dis_trans = np.mat(res.dis_trans).reshape(4,4)
self.suc_angle = res.suc_angle
robot_ctr.Set_ptp_speed(10)
robot_ctr.Step_AbsPTPCmd(pose)
req = tool_angleRequest()
req.angle = self.suc_angle
res = self.tool_client(req)
self.state = State.move2obj
self.arm_move = True
elif self.state == State.move2obj:
robot_ctr.Set_digital_output(1,True)
req = collision_avoidRequest()
req.ini_pose = np.array(self.target_obj).reshape(-1) ####
req.limit = 2
req.dis = -1
res = self.CA_client(req)
pose = np.array(res.tar_pose)
robot_ctr.Set_ptp_speed(10)
robot_ctr.Step_AbsPTPCmd(pose)
self.state = State.pick_obj
self.monitor_suc = True
self.arm_move = True
elif self.state == State.pick_obj:
req = collision_avoidRequest()
req.ini_pose = np.array(self.target_obj).reshape(-1) ####
req.limit = 2
req.dis = 6
res = self.CA_client(req)
pose = np.array(res.tar_pose)
robot_ctr.Set_ptp_speed(10)
robot_ctr.Step_AbsPTPCmd(pose)
self.state = State.move2binup
self.arm_move = True
elif self.state == State.move2binup:
if self.monitor_suc == True:
time.sleep(0.3)
self.monitor_suc = False
pose = [15,15,10,180,0,0]
robot_ctr.Set_ptp_speed(10)
robot_ctr.Step_AbsPTPCmd(pose)
self.state = State.move2placeup
self.arm_move = True
elif self.state == State.move2placeup:
req = tool_angleRequest()
req.angle = 0
res = self.tool_client(req)
robot_inputs_state = robot_ctr.Get_current_robot_inputs()
if robot_inputs_state[0] == False:
robot_ctr.Set_digital_output(1,False)
self.state = State.move2pic
return
pose = [7.7,-18,5.5714,180,0,0]
robot_ctr.Set_ptp_speed(10)
robot_ctr.Step_AbsPTPCmd(pose)
self.state = State.move2placeup1
self.arm_move = True
elif self.state == State.move2placeup1:
pose = [7.7,-18,-22,180,0,0]
trans = tf.transformations.euler_matrix(radians(pose[3]), radians(pose[4]), radians(pose[5]), axes='sxyz')
trans = np.mat(trans) * self.dis_trans
pose[3:] = [degrees(abc) for abc in tf.transformations.euler_from_matrix(trans, axes='sxyz')]
print('pose:\,n', pose)
# robot_ctr.Set_ptp_speed(10)
robot_ctr.Step_AbsPTPCmd(pose)
self.state = State.place
self.arm_move = True
elif self.state == State.place:
if self.right_side:
self.place_pos_right[0] += 6
if self.place_pos_right[0] > 18:
self.place_pos_right[0] = 18
self.place_pos_right[2] += 1.5
pose = copy.deepcopy(self.place_pos_right) ##
else:
self.place_pos_left[0] += 6
if self.place_pos_left[0] > 18:
self.place_pos_left[0] = 18
self.place_pos_left[2] += 1.5
pose = copy.deepcopy(self.place_pos_left) ##
print('pose:\,n', pose)
trans = tf.transformations.euler_matrix(radians(pose[3]), radians(pose[4]), radians(pose[5]), axes='sxyz')
print(trans)
print(self.dis_trans)
trans = np.mat(trans) * self.dis_trans
pose[3:] = [degrees(abc) for abc in tf.transformations.euler_from_matrix(trans, axes='sxyz')]
print('pose:\,n', pose)
robot_ctr.Set_ptp_speed(10)
robot_ctr.Step_AbsPTPCmd(pose)
self.state = State.placeup
self.arm_move = True
##
elif self.state == State.placeup:
robot_ctr.Set_digital_output(1,False)
pose = [7.7,-20,5.5714,180,0,0]
robot_ctr.Set_ptp_speed(10)
robot_ctr.Step_AbsPTPCmd(pose)
self.state = State.move2pic
self.arm_move = True