class disposingTask:
def __init__(self, _name='/robotis'):
"""Initial object."""
self.en_sim = False
self.__set_pubsub()
if len(sys.argv) >= 2:
print(type(sys.argv[1]))
if sys.argv[1] == 'True':
rospy.set_param('self.en_sim', sys.argv[1])
self.en_sim = rospy.get_param('self.en_sim')
self.name = _name
self.state = initPose
self.nowState = initPose
self.nextState = idle
self.reGripCnt = 0
self.pickList = 0
self.pickListAll = 8
self.arm = ArmTask(self.name + '_arm')
self.pos = [0, 0, 0]
self.euler = [0, 0, 0]
self.phi = 0
self.sucAngle = 0
self.sandwitchPos = [0, 0, 0]
self.sandwitchEu = [0, 0, 0]
self.sandwitchSuc = 0
self.sandwitchVec = [0, 0, 0]
self.camMovePos = [0, 0]
self.Qrcode = ''
self.checkCnt = 0
self.moveCnt = 0
self.mode = 'p2p'
self.ROI_Pos = [0, 640, 0, 480]
self.Vision_pos = [0, 0, 0, 0, 0, 0]
if self.name == 'right':
self.is_right = 1
if self.name == 'left':
self.is_right = -1
if self.en_sim:
self.suction = SuctionTask(self.name + '_gazebo')
else:
self.suction = SuctionTask(self.name)
rospy.on_shutdown(self.suction.gripper_vaccum_off)
def __set_pubsub(self):
self.__realsense_sub = rospy.Subscriber('/object/normal',
coordinate_normal,
self.disposing_vision_callback,
queue_size=1)
self.__ROI_sub = rospy.Subscriber('/object/ROI',
ROI,
self.ROI_callback,
queue_size=1)
self.__Qrcode_sub = rospy.Subscriber('/barcode',
String,
self.Qrcode_callback,
queue_size=1)
#pub 2
self.mobileStade_pub = rospy.Publisher('/scan_black/strategy_behavior',
Int32,
queue_size=1)
self.moveWheel_pub = rospy.Publisher('/motion/cmd_vel',
Twist,
queue_size=1)
self.mobileMove_pub = rospy.Publisher('scan_black/strategy_forward',
Int32,
queue_size=1)
def ROI_callback(self, msg):
self.ROI_Pos = [msg.x_min, msg.x_Max, msg.y_min, msg.y_Max]
def Qrcode_callback(self, msg):
self.Qrcode = msg.data
def disposing_vision_callback(self, msg):
if msg.normal_z < -0.5:
self.Vision_pos = [
msg.x, msg.y, msg.z, msg.normal_x, msg.normal_y, msg.normal_z
]
def ROI_regulate(self):
Img_Obj_x = (self.ROI_Pos[0] + self.ROI_Pos[1]) / 2
Img_Obj_y = (self.ROI_Pos[2] + self.ROI_Pos[3]) / 2
# Img_Obj_Center = (Img_Obj_x,Img_Obj_y)
if Img_Obj_x < 180:
self.camMovePos[0] = 1
elif Img_Obj_x > 320:
self.camMovePos[0] = -1
else:
self.camMovePos[0] = 0
if Img_Obj_y < 180:
self.camMovePos[1] = -1
elif Img_Obj_y > 300:
self.camMovePos[1] = 1
else:
self.camMovePos[1] = 0
def VisiontoArm(self):
Img_Pos = np.mat([[self.Vision_pos[0]], [self.Vision_pos[1]],
[self.Vision_pos[2]], [1]])
Img_nVec = np.mat([[self.Vision_pos[3]], [self.Vision_pos[4]],
[self.Vision_pos[5]], [0]])
# Img_Pos = np.mat([[0], [0.01], [0.3]])
# Img_nVec = np.mat([[0.86], [0], [-0.5]])
dx = 0 # unit:meter
dy = 0.055 # unit:meter
dz = -0.12 # unit:meter
TransMat_EndToImg = np.mat([[1, 0, 0, dx], [0, 0, 1, dy],
[0, -1, 0, dz], [0, 0, 0, 1]])
ori = left.arm.get_fb().orientation
T0_7 = np.identity(4)
for i in range(0, 4):
for j in range(0, 4):
T0_7[i][j] = ori[i * 4 + j]
Mat_nVec_Pos = np.mat([[Img_nVec[0, 0], Img_Pos[0, 0]],
[Img_nVec[1, 0], Img_Pos[1, 0]],
[Img_nVec[2, 0], Img_Pos[2, 0]], [0, 1]])
Mat_VecPos_ImgToBase = T0_7 * TransMat_EndToImg * Mat_nVec_Pos
self.sandwitchPos = Mat_VecPos_ImgToBase[0:3, 1]
self.sandwitchVec = Mat_VecPos_ImgToBase[0:3, 0]
print 'T0_7', T0_7
print 'self.sandwitchPos', self.sandwitchPos
print 'self.sandwitchVec', self.sandwitchVec
@property
def finish(self):
return self.pickList == self.pickListAll
def getPlacePos(self):
self.pos = [0, 0.7, -0.8]
self.euler = [-90, 0, 60]
self.phi = -30
self.sucAngle = 0
def move_to_vector_point(
self,
pos,
vector=[1, 0,
0]): # This funthion will move arm and return suction angle
# Only for left arm Euler (0 0 30)
goal_vec = [0, 0, 0]
goal_vec[0], goal_vec[1], goal_vec[2] = -round(vector[0], 4), -round(
vector[1], 4), -round(vector[2], 4)
a = sin(pi / 3)
b = sin(pi / 6)
x, y, z = goal_vec[0], goal_vec[1], goal_vec[2]
roll_angle = 0.0
print 'goal_vec', goal_vec
suc_angle = -round(acos(round(
(b * y - a * z) / (a * a + b * b), 4)), 4)
print 'suc_angle', suc_angle
roll_angle_c = round(acos(round(x / sin(suc_angle), 4)), 4)
print 'asin', round(
-(a * y + b * z) / ((a * a + b * b) * sin(suc_angle)), 4)
roll_angle_s = round(
asin(
round(-(a * y + b * z) / ((a * a + b * b) * sin(suc_angle)),
4)), 4)
print 'roll_angle_c', roll_angle_c
print 'roll_angle_s', roll_angle_s
if roll_angle_s >= 0:
roll_angle = roll_angle_c
else:
roll_angle = -roll_angle_c
pos[0] += vector[0] * 0.065
pos[1] += vector[1] * 0.065
pos[2] += vector[2] * 0.065
euler = self.sandwitchEu
euler[0] = degrees(roll_angle)
euler[1] = 0
euler[2] = 30
self.sandwitchPos = pos
self.sandwitchEu = euler
self.sandwitchSuc = degrees(suc_angle)
@property
def finish(self):
return self.pickList == self.pickListAll
def process(self):
global TIMEOUT
if self.arm.is_stop: # must be include in your strategy
self.finish = True # must be include in your strategy
print "!!! Robot is stop !!!" # must be include in your strategy
self.suction.gripper_vaccum_off(
) # must be include in your strategy
return # must be include in your strategy
if self.state == idle:
if TIMEOUT:
self.finish = True
if self.finish:
return
else:
self.state = move2CamPos1
elif self.state == busy:
if self.arm.is_busy:
return
else:
self.state = self.nextState
self.nowState = self.nextState
return
elif self.state == initPose:
self.state = busy
self.nextState = move2CamPos1
self.suction.gripper_suction_deg(0)
self.arm.set_speed(SPEED)
if TIMEOUT:
self.nextState = idle
self.finish = True
elif self.state == move2CamPos1:
self.state = busy
self.nextState = move2CamPos2
pos = (0, 0.45, -0.4)
euler = (0, 0, -35)
phi = 0
if TIMEOUT:
self.mode = 'line'
self.arm.ikMove(self.mode, pos, euler, phi)
self.suction.gripper_suction_deg(0)
if TIMEOUT:
self.nextState = idle
self.finish = True
elif self.state == move2CamPos2:
self.state = busy
self.nextState = move2CamPos3
pos = (0, 0.86, -0.2)
euler = (0, 0, 90)
phi = 0
self.arm.ikMove('line', pos, euler, phi)
self.suction.gripper_suction_deg(-90)
if TIMEOUT:
self.arm.clear_cmd()
rospy.sleep(.1)
self.state = move2Object0
elif self.state == move2CamPos3:
self.state = busy
self.nextState = moveCam
pos = (0, 0.965, -0.2)
euler = (0, 0, 90)
phi = 0
self.arm.ikMove('p2p', pos, euler, phi)
self.suction.gripper_suction_deg(-90)
if TIMEOUT:
self.arm.clear_cmd()
rospy.sleep(.1)
self.state = move2Object0
elif self.state == moveCam:
if self.checkCnt == 0:
self.ROI_Pos[0] = 99
self.checkCnt = 1
self.mobileStade_pub.publish(13)
self.ROI_regulate()
wheelCmd = Twist()
self.arm.set_speed(10)
if self.ROI_Pos[0] != 99:
if self.camMovePos[0] == 0 and self.camMovePos[1] == 0:
# wheelCmd.linear.x = 0
# wheelCmd.linear.y = 0
self.arm.clear_cmd()
# self.moveWheel_pub.publish(wheelCmd)
self.mobileStade_pub.publish(2)
rospy.sleep(.1)
# print 'state1', wheelCmd
self.state = watch
# self.moveWheel_pub.publish(wheelCmd)
self.mobileStade_pub.publish(2)
self.checkCnt = 0
else:
# wheelCmd.linear.x = 12 * self.camMovePos[0]
# wheelCmd.linear.y = 0
msg = Int32()
msg.data = self.camMovePos[0]
self.mobileMove_pub.publish(msg)
# self.moveWheel_pub.publish(wheelCmd)
# print 'state2', wheelCmd
if not self.arm.is_busy and self.camMovePos[1] != 0:
# pos_y = 0.1 * self.camMovePos[1]
if self.camMovePos[1] > 0:
pos = (0, 1.035, -0.2)
euler = (0, 0, 90)
phi = 0
self.arm.ikMove('line', pos, euler, phi)
else:
pos = (0, 0.965, -0.2)
euler = (0, 0, 90)
phi = 0
self.arm.ikMove('line', pos, euler, phi)
# self.arm.relative_move_pose('line', [0 ,pos_y , 0])
curr_y = self.arm.get_fb().group_pose.position.y
if self.arm.is_busy and self.camMovePos[
1] == 0 or curr_y > Y_MAX or curr_y < Y_MIN:
self.arm.clear_cmd()
rospy.sleep(.1)
else:
self.checkCnt += 1
if self.checkCnt > 30:
self.checkCnt = 1
wheelCmd.linear.x = 12
wheelCmd.linear.y = 0
rate = rospy.Rate(30)
for i in range(60):
msg = Int32()
msg.data = self.camMovePos[0]
self.mobileMove_pub.publish(msg)
# self.moveWheel_pub.publish(wheelCmd)
# print 'state3', wheelCmd
rate.sleep()
self.moveCnt += 1
# wheelCmd.linear.x = 0
# wheelCmd.linear.y = 0
self.mobileStade_pub.publish(2)
# self.moveWheel_pub.publish(wheelCmd)
# print 'state4', wheelCmd
print "self.moveCnt", self.moveCnt
if self.moveCnt >= 1:
TIMEOUT = True
if TIMEOUT:
self.arm.clear_cmd()
rospy.sleep(.1)
self.state = move2Object0
elif self.state == watch:
self.state = move2Object0
rospy.sleep(1)
self.VisiontoArm()
self.move_to_vector_point(self.sandwitchPos, self.sandwitchVec)
if TIMEOUT:
self.arm.clear_cmd()
rospy.sleep(.1)
self.state = move2Object0
elif self.state == move2Object0:
self.state = busy
self.nextState = move2Object1
# self.arm.singleJointMove(index=0, pos=0)
self.arm.set_speed(SPEED)
self.arm.jointMove(
0, (0, radians(-110), 0, radians(84), 0, radians(-50), 0))
if TIMEOUT:
self.nextState = move2CamPos1
elif self.state == move2Object1:
self.state = busy
self.nextState = move2Object2
pos = self.sandwitchPos[:]
pos[2] = -0.3
phi = -10
self.arm.set_speed(SPEED)
self.arm.ikMove('p2p', pos, self.sandwitchEu, phi)
self.suction.gripper_suction_deg(self.sandwitchSuc)
elif self.state == move2Object2:
self.state = busy
self.nextState = pickObject
self.phi = -30
self.arm.set_speed(SPEED)
self.arm.noa_relative_pos('line',
self.sandwitchPos,
self.sandwitchEu,
self.phi,
self.sandwitchSuc,
n=0,
o=0,
a=-0.02)
elif self.state == pickObject:
self.state = grasping
self.suction.gripper_vaccum_on()
self.arm.set_speed(20)
self.arm.noa_move_suction('line',
self.sandwitchSuc,
n=0,
o=0,
a=0.15)
rospy.sleep(.1)
elif self.state == grasping:
if self.suction.is_grip:
self.arm.clear_cmd()
rospy.sleep(.1)
self.state = busy
self.nextState = leaveShelfTop1
self.reGripCnt = 0
elif not self.arm.is_busy:
if self.en_sim:
self.state = busy
self.nextState = leaveShelfTop1
else:
self.suction.gripper_vaccum_off
self.state = move2CamPos3
elif self.state == leaveShelfTop1:
self.state = busy
self.nextState = leaveShelfTop2
self.arm.set_speed(SPEED)
self.arm.relative_move_pose('line', [0, 0, 0.1])
elif self.state == leaveShelfTop2:
self.state = busy
self.nextState = move2QRcode
pos = (0.06, 0.55, -0.3)
euler = (0, 0, 0)
phi = 0
self.arm.ikMove('line', pos, euler, phi)
self.suction.gripper_suction_deg(0)
elif self.state == move2QRcode:
self.state = busy
self.nextState = checkDate
pos = QRCODEPOS
euler = (0, 0, -45)
phi = 0
self.arm.ikMove('p2p', pos, euler, phi)
elif self.state == checkDate:
if self.checkCnt == 0:
self.Qrcode = ''
if self.arm.is_busy is not True:
euler = ((-40 + self.checkCnt * 15), 0, 0)
self.checkCnt += 1
self.arm.set_speed(20)
self.arm.move_euler('p2p', euler)
if self.Qrcode != '' and self.Qrcode in EXPIRED or self.checkCnt >= 8 or TIMEOUT:
self.arm.clear_cmd()
rospy.sleep(.1)
self.state = rearSafetyPos
self.checkCnt = 0
elif self.Qrcode != '':
self.arm.clear_cmd()
self.checkCnt = 0
rospy.sleep(.1)
self.state = move2Shelf1
if self.suction.is_grip is not True:
self.state == idle
self.pickList += 1
elif self.state == frontSafetyPos:
self.state = busy
self.nextState = move2Shelf1
pos = [0, 0.5, -0.5]
euler = [-90, 0, 30]
phi = 0
self.arm.ikMove('line', pos, euler, phi)
elif self.state == rearSafetyPos:
self.state = busy
self.nextState = move2Bin1
pos = [0, 0.4, -0.5]
euler = [90, -20, -30]
phi = 60
self.arm.ikMove('line', pos, euler, phi)
elif self.state == move2Shelf1:
self.state = busy
self.nextState = move2Shelf2
self.getPlacePos()
self.pos[0] -= 0.05
self.arm.set_speed(SPEED)
self.arm.noa_relative_pos('p2p',
self.pos,
self.euler,
self.phi,
self.sucAngle,
n=0,
o=0,
a=-0.05)
elif self.state == move2Shelf2:
self.state = busy
self.nextState = PlaceObject
self.getPlacePos()
self.arm.ikMove('line', self.pos, self.euler, self.phi)
elif self.state == leaveShelfMiddle1:
self.state = busy
self.nextState = idle
self.getPlacePos()
self.pos[0] -= 0.05
self.arm.set_speed(SPEED)
self.arm.noa_relative_pos('line',
self.pos,
self.euler,
self.phi,
self.sucAngle,
n=0,
o=0,
a=-0.1)
self.pickList += 1
if TIMEOUT:
self.nextState = idle
self.finish = True
elif self.state == move2Bin1:
self.state = busy
self.nextState = PlaceObject1
pos = (-0.3, 0.2, -0.45)
euler = (90, -30, -30)
phi = 60
self.arm.ikMove('line', pos, euler, phi)
elif self.state == leaveBin1:
self.state = busy
self.nextState = idle
pos = [0, 0.5, -0.5]
euler = [90, -20, -30]
phi = 60
self.arm.ikMove('line', pos, euler, phi)
elif self.state == PlaceObject:
self.state = leaveShelfMiddle1
self.suction.gripper_vaccum_off()
elif self.state == PlaceObject1:
self.state = idle
self.pickList += 1
self.suction.gripper_vaccum_off()
if TIMEOUT:
self.nextState = idle
self.finish = True
class StockingTask:
def __init__(self, _name='/robotis'):
"""Initial object."""
self.en_sim = False
if len(sys.argv) >= 2:
print(type(sys.argv[1]))
if sys.argv[1] == 'True':
rospy.set_param('self.en_sim', sys.argv[1])
self.en_sim = rospy.get_param('self.en_sim')
self.name = _name
self.status = Status.initPose
self.nowStatus = Status.initPose
self.nextStatus = Status.idle
self.reGripCnt = 0
self.arm = ArmTask(self.name + '_arm', self.en_sim)
self.pickListAll = len(lunchboxPos) + len(riceballPos) + len(drinkPos)
self.pickList = PICKORDER
self.pos = [0, 0, 0]
self.euler = [0, 0, 0]
self.phi = 0
self.sucAngle = 0
self.all_finish = False
if self.name == 'right':
self.is_right = 1
self.speed = SPEED_R
if self.name == 'left':
self.is_right = -1
self.speed = SPEED_L
if self.en_sim:
self.suction = SuctionTask(self.name + '_gazebo')
else:
self.suction = SuctionTask(self.name)
rospy.on_shutdown(self.suction.gripper_vacuum_off)
@property
def finish(self):
return self.pickList == self.pickListAll
@property
def status(self):
return self.status.name
@property
def all_finish(self):
return self.all_finish
def setQuantity(self):
for index in range(lunchQuan):
objectName[index] = 'lunchboxXX'
lunchboxPos[index][1] *= -1
lunchboxPos[lunchQuan - index - 1][2] += LUNCHBOX_H * index
for index in range(4 - lunchQuan):
lunchboxPos[4 - index - 1][2] += LUNCHBOX_H * index
for index in range(drinkQuan):
objectName[index + 4] = 'drinkXX'
for index in range(riceQuan):
objectName[index + 8] = 'riceballXX'
def _getRearSafetyPos(self):
self.pos = [0, -0.5 * self.is_right, -0.5]
self.euler = [-90 * self.is_right, -20, 30 * self.is_right]
self.phi = -60 * self.is_right
def _getObjectPos(self):
if self.finish:
return
while objectPos[self.pickList / 4][self.pickList %
4][1] * self.is_right > 0:
self.pickList += 1
if self.finish:
return
self.pos = objectPos[self.pickList / 4][self.pickList % 4][:]
self.euler = objectEu[self.pickList / 4][:]
if objectName[self.pickList] == 'riceballXX':
self.euler = riceballXXEu[:]
self.euler[0] *= self.is_right
self.euler[2] *= self.is_right
self.phi = -45 * self.is_right
if self.reGripCnt != 0:
if self.reGripCnt == 1:
if self.pickList == 4 or self.pickList == 6 or self.pickList == 8 or self.pickList == 10:
self.pos[0] += 0.005
else:
self.pos[0] += 0.02
self.pos[1] += 0.01
if self.reGripCnt == 2:
if self.pickList == 4 or self.pickList == 6 or self.pickList == 8 or self.pickList == 10:
self.pos[0] += 0.005
else:
self.pos[0] += 0.02
self.pos[1] -= 0.01
if self.reGripCnt == 3:
self.pos[0] -= 0.01
self.pos[1] -= 0.01
def _getPlacePos(self):
if objectName[self.pickList] == 'lunchboxXX':
self.pos = bottomRight[:]
self.euler = bottomRightEu[:]
self.phi = bottomRightPhi * self.is_right
self.sucAngle = -90
self.pos[2] += ((self.pickList % 4)) * 0.05
elif objectName[self.pickList] == 'lunchbox':
self.pos = bottomLeft[:]
self.euler = bottomLeftEu[:]
self.phi = bottomLeftPhi * self.is_right
self.sucAngle = -90
self.pos[2] += ((self.pickList % 4) - lunchQuan) * 0.05
elif objectName[self.pickList] == 'drinkXX':
self.pos = middleRight[:]
self.euler = middleRightEu[:]
self.phi = middleRightPhi * self.is_right
self.sucAngle = -90
self.pos[0] += (drinkQuan - (self.pickList % 4) - 1) * 0.1
elif objectName[self.pickList] == 'drink':
self.pos = middleLeft[:]
self.euler = middleLeftEu[:]
self.phi = middleLeftPhi * self.is_right
self.sucAngle = -90
self.pos[0] += (4 - (self.pickList % 4) - 1) * 0.1
elif objectName[self.pickList] == 'riceballXX':
self.pos = topLeft[:]
self.euler = topLeftEu[:]
self.phi = topLeftPhi * self.is_right
self.sucAngle = topLeftSuc
self.pos[0] += (riceQuan - (self.pickList % 4) - 1) * 0.045
elif objectName[self.pickList] == 'riceball':
self.pos = topRight[:]
self.euler = topRightEu[:]
self.phi = topRightPhi * self.is_right
self.sucAngle = topRightSuc
self.pos[0] += (4 - (self.pickList % 4) - 1) * 0.045
def process(self):
if self.arm.is_stop: # must be include in your strategy
self.finish = True # must be include in your strategy
print("!!! Robot is stop !!!") # must be include in your strategy
self.suction.gripper_vacuum_off(
) # must be include in your strategy
return # must be include in your strategy
if self.status == Status.idle:
self._getObjectPos()
if self.finish:
self.status = Status.backhome
return
else:
if 'lunchbox' in objectName[
self.
pickList] and self.pickList != 8: # or self.pickList==7:
self.status = Status.safePose3
# self.status = Status.rearSafetyPos
else:
self.status = Status.rearSafetyPos
# self.status = Status.rearSafetyPos
print("self.pickList = " + str(self.pickList))
print("state 1")
elif self.status == Status.busy:
if self.arm.is_busy:
if (self.nowStatus == Status.leaveBin
or self.nowStatus == Status.frontSafetyPos
or self.nowStatus == Status.move2Shelf
) and not self.suction.is_grip and not self.en_sim:
self.status = Status.missObj
return
else:
self.status = self.nextStatus
self.nowStatus = self.nextStatus
return
elif self.status == Status.safePose1:
self.status = Status.busy
self.nextStatus = Status.idle
self.pickList += 1
self.euler[2] = 90
self.euler[0] = -10
self.arm.relative_move('line', [0, -0.1, -0.3], self.euler,
self.phi)
elif self.status == Status.leavePlacePos:
self.status = Status.busy
self.nextStatus = Status.leaveShelf
if 'riceball' in objectName[self.pickList]:
self.arm.relative_move('line', [-0.02, 0, 0.02], self.euler,
self.phi)
else:
self.arm.noa_move_suction('line',
suction_angle=self.sucAngle,
n=0,
o=0,
a=-0.01)
elif self.status == Status.safePose3:
self.status = Status.busy
self.nextStatus = Status.rearSafetyPos
self.arm.set_speed(self.speed)
self.arm.jointMove(0, (0, -1.2, 0, 2.4, 0, -1.2, 0))
elif self.status == Status.initPose:
self.status = Status.busy
self.nextStatus = Status.idle
self.arm.set_speed(self.speed)
self.arm.jointMove(0, (0, -1, 0, 1.57, 0, -0.57, 0))
self.suction.gripper_suction_deg(0)
elif self.status == Status.frontSafetyPos:
print("state 9")
self.status = Status.busy
self.nextStatus = Status.move2Shelf
self._getRearSafetyPos()
self.euler[0] = -90 * self.is_right
if 'drink' in objectName[self.pickList]:
self.pos[2] = -0.4
self.euler[1] = -25
elif 'lunchbox' in objectName[self.pickList]:
self.pos[2] = -0.45
self.arm.set_speed(self.speed)
self.arm.ikMove('line', self.pos, self.euler, self.phi)
elif self.status == Status.rearSafetyPos:
self.status = Status.busy
self.nextStatus = Status.move2Bin
self._getRearSafetyPos()
# self.arm.set_speed(self.speed)
self.arm.ikMove('line', self.pos, self.euler, self.phi)
print("state 2")
elif self.status == Status.move2Bin:
print("state 3")
self.status = Status.busy
self.nextStatus = Status.move2Object
self._getObjectPos()
self.pos[2] = -0.6
if 'riceball' not in objectName[self.pickList]:
self.euler[1] = -10
else:
self.euler[1] = -10
if 'lunchbox' in objectName[self.pickList]:
self.euler[0] *= 1.1
self.arm.set_speed(self.speed)
self.arm.ikMove('line', self.pos, self.euler, self.phi)
elif self.status == Status.move2Shelf:
print("state 10")
self.status = Status.busy
self._getPlacePos()
if 'riceball' in objectName[self.pickList]:
self.nextStatus = Status.riceballEuler
self.euler[0] = -45
# self.euler = [0, -10, 0]
# self.pos[2] -= 0.2
else:
self.nextStatus = Status.moveIn2Shelf
self.euler[0] = 0
self.pos[0] = 0.5
if 'lunchbox' in objectName[self.pickList]:
self.pos[2] += 0.01
else:
self.pos[2] += 0.01
self.arm.set_speed(self.speed)
self.arm.noa_relative_pos('line',
self.pos,
self.euler,
self.phi,
suction_angle=0,
n=0,
o=0,
a=-0.15)
if 'riceball' not in objectName[self.pickList]:
self.suction.gripper_suction_deg(-60)
rospy.sleep(.1)
self.suction.gripper_calibration()
elif self.status == Status.riceballEuler:
self.status = Status.busy
self.nextStatus = Status.moveIn2Shelf
self._getPlacePos()
self.pos[2] += 0.01
self.arm.set_speed(self.speed)
print('euler = ', self.euler)
self.arm.move_euler('line', self.euler)
self.suction.gripper_suction_deg(self.sucAngle)
elif self.status == Status.moveIn2Shelf:
self.status = Status.busy
self.nextStatus = Status.move2PlacedPos
self._getPlacePos()
if 'lunchbox' in objectName[self.pickList]:
self.pos[2] += 0.01
else:
self.pos[2] += 0.01
if self.pickList == 5:
self.arm.set_speed(10)
else:
self.arm.set_speed(self.speed)
self.arm.ikMove('line', self.pos, self.euler, self.phi)
if 'riceball' not in objectName[self.pickList]:
self.suction.gripper_suction_deg(-90)
elif self.status == Status.leaveBin:
print("state 8")
self.status = Status.busy
self.nextStatus = Status.frontSafetyPos
self.arm.set_speed(self.speed)
self._getObjectPos()
self.pos[2] = -0.55
if 'drink' in objectName[self.pickList]:
self.pos[0] -= 0.02
self.pos[2] = -0.42
self.euler[1] = -30
self.euler[2] = 40 * self.is_right
if self.pickList == 10:
self.euler[1] = -6
self.euler[2] = 10 * self.is_right
if 'lunchbox' in objectName[self.pickList]:
self.euler[0] *= 1.1
self.arm.ikMove('line', self.pos, self.euler, self.phi)
elif self.status == Status.leaveShelf:
self.status = Status.busy
self.nextStatus = Status.idle
self.arm.set_speed(self.speed)
# if objectName[self.pickList] == 'riceballXX':
# self.arm.noa_move_suction('line', suction_angle=0, n=0.08, o=0, a=-0.22)
# else:
# self.arm.noa_move_suction('line', suction_angle=0, n=0.08, o=0, a=-0.12)
# self.arm.relative_move_pose('line', [-0.3, 0, 0.1])
self._getPlacePos()
if 'riceball' in objectName[self.pickList]:
self.euler[0] = -45
self.nextStatus = Status.safePose1
self.pickList -= 1
else:
self.euler[0] = 0
self.pos[0] = 0.36
self.pos[2] += 0.01
self.arm.set_speed(self.speed)
self.arm.noa_relative_pos('line',
self.pos,
self.euler,
self.phi,
suction_angle=0,
n=0,
o=0,
a=-0.15)
self.pickList += 1
self.suction.gripper_suction_deg(0)
elif self.status == Status.move2Object:
print("state 4")
self.status = Status.busy
self.nextStatus = Status.pickObject
self._getObjectPos()
self.arm.set_speed(self.speed)
self.arm.ikMove('line', self.pos, self.euler, self.phi)
elif self.status == Status.move2PlacedPos:
self.status = Status.busy
self.nextStatus = Status.placeObject
self._getPlacePos()
if self.pickList == 10 or self.pickList == 8:
self.pos[2] -= 0.006
if self.pickList == 8:
self.pos[2] += 0.003
if 'lunchbox' in objectName[self.pickList]:
self.arm.set_speed(60)
else:
self.arm.set_speed(self.speed)
self.arm.ikMove('line', self.pos, self.euler, self.phi)
elif self.status == Status.pickObject:
print("state 5")
self.status = Status.grasping
self.suction.gripper_vacuum_on()
print("state 5-1")
# rospy.sleep(1)
if 'lunchbox' in objectName[self.pickList]:
self.arm.set_speed(20)
else:
self.arm.set_speed(3)
print("state 5-2")
self.arm.noa_move_suction('line',
suction_angle=0,
n=0,
o=0,
a=0.03)
print("state 5-3")
rospy.sleep(.1)
elif self.status == Status.placeObject:
self.status = Status.busy
self.nextStatus = Status.leavePlacePos
if 'lunchbox' in objectName[self.pickList]:
self.nextStatus = Status.leaveShelf
# if 'riceball' in objectName[self.pickList]:
# self.arm.set_speed(80)
# self.arm.relative_move_pose('line', [-0.005, 0, 0])
rospy.sleep(.3)
self.suction.gripper_vacuum_off()
elif self.status == Status.grasping:
print("state 6")
if self.suction.is_grip or self.en_sim:
self.arm.clear_cmd()
# rospy.sleep(.1)
self.status = Status.busy
self.nextStatus = Status.leaveBin
self.reGripCnt = 0
elif not self.arm.is_busy:
self.status = Status.missObj
elif self.status == Status.missObj:
print("state 7")
if self.nowStatus == Status.pickObject or self.nowStatus == Status.leaveBin:
self.status = Status.busy
self.nextStatus = Status.move2Bin
self.nowStatus = Status.idle
self.arm.clear_cmd()
self.reGripCnt += 1
if self.reGripCnt > 3:
self.reGripCnt = 0
self.pickList += 1
if self.finish:
self.nextStatus = Status.idle
elif self.nowStatus == Status.frontSafetyPos or self.nowStatus == Status.move2Shelf:
self.status = Status.busy
self.nextStatus = Status.idle
self.nowStatus = Status.idle
self.arm.clear_cmd()
self.pickList += 1
self.arm.set_speed(20)
elif self.status == Status.backhome:
rospy.loginfo('back home')
self.status = Status.busy
self.nextStatus = Status.finish
self.arm.jointMove(0, (0, -1, 0, 2, 0, -0.7, 0))
elif self.status == Status.finish:
if self.all_finish:
return
self.status = Status.busy
self.nextStatus = Status.finish
self.arm.jointMove(0, (0, 0, 0, 0, 0, 0, 0))
self.all_finish = True
class DualArmTask:
""""""
def __init__(self, _name, en_sim):
self.name = _name
self.en_sim = en_sim
self.right_arm = ArmTask('right', self.en_sim)
self.left_arm = ArmTask('left', self.en_sim)
self.right_value = 0
self.left_value = 0
self.right_limit = False
self.left_limit = False
self.right_event = threading.Event()
self.left_event = threading.Event()
self.stop_event = threading.Event()
self.right_event.clear()
self.left_event.clear()
self.stop_event.clear()
self.right_thread = threading.Thread(
target=self.__right_arm_process_thread)
self.left_thread = threading.Thread(
target=self.__left_arm_process_thread)
self.right_thread.start()
self.left_thread.start()
rospy.on_shutdown(self.shutdown)
def shutdown(self):
self.right_arm.clear_cmd()
self.left_arm.clear_cmd()
self.stop_event.set()
self.right_event.set()
self.left_event.set()
# del self.right_thread
# del self.left_thread
def __right_arm_process_thread(self):
rate = rospy.Rate(20)
while not rospy.is_shutdown():
if self.stop_event.is_set():
return
self.right_arm.process()
if self.right_arm.status == Status.grasping and self.right_arm.suction.is_grip:
self.right_arm.clear_cmd()
rospy.sleep(0.1)
if self.right_arm.cmd_queue_empty:
if self.right_arm.cmd_queue_2nd_empty and self.right_arm.status == Status.idle:
self.right_event.clear()
pass
elif not self.right_arm.is_busy and not self.right_arm.status == Status.occupied:
self.right_arm.cmd_2to1()
rate.sleep()
self.right_event.wait()
def __left_arm_process_thread(self):
rate = rospy.Rate(20)
while not rospy.is_shutdown():
if self.stop_event.is_set():
return
self.left_arm.process()
if self.left_arm.status == Status.grasping and self.left_arm.suction.is_grip:
self.left_arm.clear_cmd()
rospy.sleep(0.1)
if self.left_arm.cmd_queue_empty:
if self.left_arm.cmd_queue_2nd_empty and self.left_arm.status == Status.idle:
self.left_event.clear()
pass
elif not self.left_arm.is_busy and not self.left_arm.status == Status.occupied:
self.left_arm.cmd_2to1()
rate.sleep()
self.left_event.wait()
def __choose_and_check_side(self, side, command_queue):
self.right_value = 0.0
self.left_value = 0.0
self.right_limit = False
self.left_limit = False
cmd_q = queue.Queue()
if 'right' in side:
while not command_queue.empty():
cmd = command_queue.get()
if cmd['cmd'] == 'ikMove':
print(cmd['pos'], cmd['euler'], cmd['phi'])
self.right_value, self.right_limit = self.right_arm.check_range_limit(
cmd['pos'], cmd['euler'], cmd['phi'])
if self.right_limit:
return 'fail', cmd_q
cmd_q.put(cmd)
return 'right', cmd_q
elif 'left' in side:
while not command_queue.empty():
cmd = command_queue.get()
if cmd['cmd'] == 'ikMove':
print(cmd['pos'], cmd['euler'], cmd['phi'])
self.left_value, self.left_limit = self.left_arm.check_range_limit(
cmd['pos'], cmd['euler'], cmd['phi'])
if self.left_limit:
return 'fail', cmd_q
cmd_q.put(cmd)
return 'left', cmd_q
else:
right_sum = 0
left_sum = 0
right_close_limit = False
left_close_limit = False
while not command_queue.empty():
cmd = command_queue.get()
cmd_q.put(cmd)
if cmd['cmd'] == 'ikMove':
if not self.left_limit:
self.left_value, self.left_limit = self.left_arm.check_range_limit(
cmd['pos'], cmd['euler'], cmd['phi'])
if self.left_value > 0.2:
left_close_limit = True
left_sum += self.left_value
if not self.right_limit:
self.right_value, self.right_limit = self.right_arm.check_range_limit(
cmd['pos'], cmd['euler'], cmd['phi'])
if self.right_value > 0.2:
right_close_limit = True
right_sum += self.right_value
if not (self.left_limit or self.right_limit):
if right_sum <= left_sum and self.right_arm.status == Status.idle:
side = 'right'
elif left_sum <= right_sum and self.left_arm.status == Status.idle:
side = 'left'
elif self.right_arm.status == Status.idle and not right_close_limit:
side = 'right'
elif self.left_arm.status == Status.idle and not left_close_limit:
side = 'left'
elif right_sum <= left_sum:
side = 'right'
elif left_sum <= right_sum:
side = 'left'
elif self.right_limit and self.left_limit:
side = 'fail'
elif self.right_limit:
side = 'left'
elif self.left_limit:
side = 'right'
return side, cmd_q
def send_cmd(self, side, priority, command_queue):
side, command_queue = self.__choose_and_check_side(side, command_queue)
if side == 'right':
if priority:
self.right_arm.cmd_queue_put(command_queue)
else:
self.right_arm.cmd_queue_2nd_put(command_queue)
if not self.right_event.is_set():
self.right_event.set()
self.right_arm.status = Status.busy
elif side == 'left':
if priority:
self.left_arm.cmd_queue_put(command_queue)
else:
self.left_arm.cmd_queue_2nd_put(command_queue)
if not self.left_event.is_set():
self.left_event.set()
self.left_arm.status = Status.busy
return side
def get_feedback(self, side):
if side == 'right':
return self.right_arm.get_fb()
elif side == 'left':
return self.left_arm.get_fb()
def suc2vector(self, vec, euler):
rotation = self.right_arm.euler2rotation(euler)
vec = np.array(vec)
rotation = np.array(rotation)
a = rotation[0:3, 2].reshape(-1)
sucangle = -acos(
np.dot(a, -1 * vec) / (np.linalg.norm(a) * np.linalg.norm(vec)))
b = np.dot(a, vec)
c = vec - b * a
n = rotation[0:3, 0]
roll = acos(np.dot(c, n) / (np.linalg.norm(c) * np.linalg.norm(n)))
o = rotation[0:3, 1]
cos_oc = np.dot(o, c) / (np.linalg.norm(o) * np.linalg.norm(c))
if cos_oc < 0:
roll = -1 * roll
return degrees(sucangle), degrees(roll)
class DisposingTask:
def __init__(self, _name = '/robotis'):
"""Initial object."""
self.en_sim = False
if len(sys.argv) >= 2:
print(type(sys.argv[1]))
if sys.argv[1] == 'True':
rospy.set_param('self.en_sim', sys.argv[1])
self.en_sim = rospy.get_param('self.en_sim')
self.name = _name
self.status = Status.initPose
self.nowStatus = Status.initPose
self.nextStatus = Status.idle
self.reGripCnt = 0
self.arm = ArmTask(self.name + '_arm')
self.pickListAll = len(lunchboxPos) + len(riceballPos) + len(drinkPos)
self.pickList = PICKORDER
self.pos = [0, 0, 0]
self.euler = [0, 0, 0]
self.phi = 0
self.sucAngle = 0
self.all_finish = False
if self.name == 'right':
self.is_right = 1
self.speed = SPEED_R
if self.name == 'left':
self.is_right = -1
self.speed = SPEED_L
if self.en_sim:
self.suction = SuctionTask(self.name + '_gazebo')
else:
self.suction = SuctionTask(self.name)
rospy.on_shutdown(self.suction.gripper_vaccum_off)
@property
def finish(self):
return self.pickList == self.pickListAll
@property
def all_finish(self):
return self.all_finish
@property
def status(self):
return self.status.name
def setQuantity(self):
for index in range(lunchQuan):
objectName[index] = 'lunchboxXX'
lunchboxPos[index][1] *= -1
lunchboxPos[lunchQuan - index -1][2] += LUNCHBOX_H * index
for index in range(2 - lunchQuan):
lunchboxPos[2 - index -1][2] += LUNCHBOX_H * index
for index in range(drinkQuan):
objectName[index+2] = 'drinkXX'
for index in range(riceQuan):
objectName[index+4] = 'riceballXX'
def getRearSafetyPos(self):
self.pos = [0, -0.5*self.is_right, -0.5]
self.euler = [-90*self.is_right, -20, 30*self.is_right]
self.phi = -60*self.is_right
def getFrontSafetyPos(self):
self.pos = (0.1, -0.4*self.is_right, -0.45)
self.euler = (0, 0, 0*self.is_right)
self.phi = 45*self.is_right
def getObjectPos(self):
if self.finish:
return
while objectPos[self.pickList/2][self.pickList%2][1]*self.is_right > 0:
self.pickList += 1
if self.finish:
return
self.pos = objectPos[self.pickList/2][self.pickList%2][:]
self.euler = objectEu[self.pickList/2][:]
if objectName[self.pickList] == 'riceballXX':
self.euler = riceballXXEu[:]
self.euler[0] *= self.is_right
self.euler[2] *= self.is_right
self.phi = -45*self.is_right
if self.reGripCnt != 0:
if self.reGripCnt == 1:
if self.pickList == 4 or self.pickList == 6 or self.pickList == 8 or self.pickList == 10:
self.pos[0] += 0.005
else:
self.pos[0] += 0.02
self.pos[1] += 0.01
if self.reGripCnt == 2:
if self.pickList == 4 or self.pickList == 6 or self.pickList == 8 or self.pickList == 10:
self.pos[0] += 0.005
else:
self.pos[0] += 0.02
self.pos[1] -= 0.01
if self.reGripCnt == 3:
self.pos[0] -= 0.01
self.pos[1] -= 0.01
def getPlacePos(self):
if objectName[self.pickList] == 'lunchboxXX':
self.pos = bottomRight[:]
self.euler = bottomRightEu[:]
self.phi = bottomRightPhi*self.is_right
self.sucAngle = -90
# self.pos[2] += ((self.pickList%4))*0.05
elif objectName[self.pickList] == 'lunchbox':
self.pos = bottomLeft[:]
self.euler = bottomLeftEu[:]
self.phi = bottomLeftPhi*self.is_right
self.sucAngle = -90
# self.pos[2] += ((self.pickList%4) - lunchQuan)*0.05
elif objectName[self.pickList] == 'drinkXX':
self.pos = middleRight[:]
self.euler = middleRightEu[:]
self.phi = middleRightPhi*self.is_right
self.sucAngle = -90
# self.pos[0] += (drinkQuan - (self.pickList%4) - 1)*0.1
elif objectName[self.pickList] == 'drink':
self.pos = middleLeft[:]
self.euler = middleLeftEu[:]
self.phi = middleLeftPhi*self.is_right
self.sucAngle = -90
# self.pos[0] += (4 - (self.pickList%4) - 1)*0.1
elif objectName[self.pickList] == 'riceballXX':
self.pos = topLeft[:]
self.euler = topLeftEu[:]
self.phi = topLeftPhi*self.is_right
self.sucAngle = topLeftSuc
# self.pos[0] += (riceQuan - (self.pickList%4) - 1)*0.045
elif objectName[self.pickList] == 'riceball':
self.pos = topRight[:]
self.euler = topRightEu[:]
self.phi = topRightPhi*self.is_right
self.sucAngle = topRightSuc
# self.pos[0] += (4 - (self.pickList%4) - 1)*0.045
def process(self):
if self.arm.is_stop: # must be include in your strategy
self.finish = True # must be include in your strategy
print "!!! Robot is stop !!!" # must be include in your strategy
self.suction.gripper_vaccum_off() # must be include in your strategy
return # must be include in your strategy
if self.status == Status.idle:
print "state 10"
self.getObjectPos()
if self.finish:
self.status = Status.backhome
return
else:
self.status = Status.frontSafetyPos
elif self.status == Status.busy:
if self.arm.is_busy:
if (self.nowStatus == Status.leaveBin or self.nowStatus == Status.frontSafetyPos or self.nowStatus == Status.move2Shelf) and not self.suction.is_grip and not self.en_sim:
# self.status = Status.missObj
pass
return
else:
self.status = self.nextStatus
self.nowStatus = self.nextStatus
return
elif self.status == Status.initPose:
self.status = Status.busy
self.nextStatus = Status.idle
self.arm.set_speed(self.speed)
self.arm.jointMove(0, (0, -1, 0, 1.57, 0, -0.57, 0))
self.suction.gripper_suction_deg(0)
elif self.status == Status.leavePlacePos:
self.status = Status.busy
self.nextStatus = Status.leaveShelf
self.arm.noa_move_suction('line', suction_angle=self.sucAngle, n=0, o=0, a=-0.02)
elif self.status == Status.frontSafetyPos:
self.status = Status.busy
self.nextStatus = Status.move2Shelf
self.getRearSafetyPos()
self.euler[0] = -90*self.is_right
if 'drink' in objectName[self.pickList]:
self.pos[2] = -0.4
self.euler[1] = -25
elif 'lunchbox' in objectName[self.pickList]:
self.pos[2] = -0.45
self.arm.set_speed(self.speed)
self.arm.ikMove('line', self.pos, self.euler, self.phi)
elif self.status == Status.rearSafetyPos:
self.status = Status.busy
self.nextStatus = Status.move2Bin
self.getRearSafetyPos()
self.arm.set_speed(self.speed)
self.arm.ikMove('line', self.pos, self.euler, self.phi)
self.suction.gripper_calibration()
print "state 2"
elif self.status == Status.move2Bin:
print "state Status.move2Bin BBBBBBIIIIIIINNNNNN===================================================================="
self.status = Status.busy
self.nextStatus = Status.move2Object
self.getObjectPos()
self.pos[2] = -0.5
self.euler[1] = -10
if 'lunchbox' in objectName[self.pickList]:
self.euler[0] *= 1.3
self.arm.set_speed(self.speed)
print(objectName[self.pickList])
print(self.pos)
print(self.euler)
print(self.phi)
self.arm.ikMove('line', self.pos, self.euler, self.phi)
self.suction.gripper_suction_deg(0)
rospy.sleep(1.0)
elif self.status == Status.move2Shelf:
self.status = Status.busy
self.getPlacePos()
self.nextStatus = Status.moveIn2Shelf
self.pos[0] = 0.52
self.pos[2] += 0.02
self.arm.set_speed(self.speed)
self.arm.noa_relative_pos('line', self.pos, self.euler, self.phi, suction_angle=0, n=0, o=0, a=-0.15)
elif self.status == Status.moveIn2Shelf:
self.status = Status.busy
self.nextStatus = Status.move2PlacedPos
self.getPlacePos()
self.pos[2] += 0.02
self.arm.set_speed(self.speed-10)
self.arm.ikMove('line', self.pos, self.euler, self.phi)
self.suction.gripper_suction_deg(-90)
elif self.status == Status.leaveBin:
print "state 8"
self.status = Status.busy
self.nextStatus = Status.idle
self.arm.set_speed(self.speed)
self.getObjectPos()
self.pos[2] = -0.48
if 'drink' in objectName[self.pickList]:
self.pos[0] -= 0.02
self.pos[2] = -0.42
self.euler[1] = -30
self.euler[2] = 40*self.is_right
if self.pickList == 10:
self.euler[1] = -6
self.euler[2] = 10*self.is_right
if 'lunchbox' in objectName[self.pickList]:
self.euler[0] *= 1.1
self.arm.ikMove('line', self.pos, self.euler, self.phi)
self.pickList += 1
print "state 9"
elif self.status == Status.leaveShelf:
self.status = Status.busy
self.nextStatus = Status.rearSafetyPos
self.arm.set_speed(self.speed)
self.getPlacePos()
if 'riceball' in objectName[self.pickList]:
self.euler[0] = 0
self.nextStatus = Status.rearSafetyPos
# self.pickList -= 1
else:
self.euler[0] = 0
self.pos[0] = 0.45
self.pos[2] += 0.01
self.arm.set_speed(self.speed)
self.arm.noa_relative_pos('line', self.pos, self.euler, self.phi, suction_angle=0, n=0, o=0, a=-0.08)
elif self.status == Status.move2Object:
print "state Status.move2Object===================================================================="
self.status = Status.busy
self.nextStatus = Status.placeObject
self.getObjectPos()
self.arm.set_speed(self.speed)
self.arm.ikMove('line', self.pos, self.euler, self.phi)
print(objectName[self.pickList])
print(self.pos)
elif self.status == Status.move2PlacedPos:
self.status = Status.busy
self.nextStatus = Status.pickObject
self.getPlacePos()
self.arm.set_speed(self.speed)
self.arm.ikMove('line', self.pos, self.euler, self.phi)
elif self.status == Status.pickObject:
self.status = Status.grasping
self.suction.gripper_vaccum_on()
self.arm.set_speed(5)
self.arm.noa_move_suction('line', suction_angle=0, n=0, o=0, a=0.03)
rospy.sleep(.1)
elif self.status == Status.placeObject:
print "state place"
self.status = Status.busy
self.nextStatus = Status.leaveBin
if 'lunchbox' in objectName[self.pickList]:
self.nextStatus = Status.leaveBin
rospy.sleep(.3)
self.suction.gripper_vaccum_off()
elif self.status == Status.grasping:
if self.suction.is_grip or self.en_sim:
self.arm.clear_cmd()
self.status = Status.busy
self.nextStatus = Status.leavePlacePos
self.reGripCnt = 0
elif not self.arm.is_busy:
self.status = Status.missObj
elif self.status == Status.missObj:
print "state 7"
if self.nowStatus == Status.pickObject or self.nowStatus == Status.leaveBin:
self.status = Status.busy
self.nextStatus = Status.move2Bin
self.nowStatus = Status.idle
self.arm.clear_cmd()
self.reGripCnt += 1
if self.reGripCnt > 3:
self.reGripCnt = 0
self.pickList += 1
if self.finish:
self.nextStatus = Status.idle
elif self.nowStatus == Status.frontSafetyPos or self.nowStatus == Status.move2Shelf:
self.status = Status.busy
self.nextStatus = Status.idle
self.nowStatus = Status.idle
self.arm.clear_cmd()
self.pickList += 1
self.arm.set_speed(self.speed)
elif self.status == Status.backhome:
rospy.loginfo('back home')
self.status = Status.busy
self.nextStatus = Status.finish
self.arm.jointMove(0, (0, -1, 0, 2, 0, -0.7, 0))
elif self.status == Status.finish:
if self.all_finish:
return
self.status = Status.busy
self.nextStatus = Status.finish
self.arm.jointMove(0, (0, 0, 0, 0, 0, 0, 0))
self.all_finish = True
class stockingTask:
def __init__(self, _name = '/robotis'):
"""Initial object."""
self.en_sim = False
if len(sys.argv) >= 2:
print(type(sys.argv[1]))
if sys.argv[1] == 'True':
rospy.set_param('self.en_sim', sys.argv[1])
self.en_sim = rospy.get_param('self.en_sim')
self.img_data = ROI()
self.img_data_list = []
self.name = _name
self.state = initPose
self.nowState = initPose
self.nextState = idle
self.reGripCnt = 0
self.arm = ArmTask(self.name + '_arm')
self.pos = [0, 0, 0]
self.euler = [0, 0, 0]
self.phi = 0
self.sucAngle = 0
self.No_Object_count = 0
self.obj_name = ''
self.standby_safeFlag = True # 第一次開完箱子的狀態切換（只有開箱子用的到）
self.close_box = False # 關箱子的旗標
self.finish_pos = False
self.mode_2D = False
self.finish = False
if self.name == 'left':
self.is_right = -1
self.speed = SPEED_L
self.faster_speed = 40
self.init_pub_sub()
if self.en_sim:
self.suction = SuctionTask(self.name + '_gazebo')
else:
self.suction = SuctionTask(self.name)
def init_pub_sub(self):
rospy.Subscriber('/object/ROI_array', ROI_array, self.get_obj_info_cb)
def process(self):
if self.arm.is_stop:
self.finish = True
print('!!! Robot is stop !!!')
return
if self.state == idle:
if self.finish:
return
if self.state == busy:
if self.arm.is_busy:
return
else:
self.state = self.nextState
self.nowState = self.nextState
return
#(第一個動作)
elif self.state == initPose:
print('self.state == initPose')
self.state = busy
#self.nextState = wait_img_pos
self.arm.set_speed(self.speed)
# self.pos = [0.4, 0.5, -0.3]
# self.euler = [0, 0, 0]
# self.phi = 0
self.nextState = standby_open
self.pos = [0.48, 0.25, -0.4]
self.euler = [0, 0, 0]
self.phi = 90
self.arm.ikMove(mode= 'p2p', pos = self.pos, euler = self.euler, phi = self.phi)
#(第二個動作)準備開抽屜的位置
elif self.state == standby_open:
print('self.state == standby_open')
self.arm.set_speed(self.faster_speed)
self.state = busy
self.nextState = drag_grasp
self.pos = [0.4, 0.42, -0.63]
self.euler = [0, 0, 90]
self.phi = 90
self.arm.ikMove(mode= 'p2p', pos = self.pos, euler = self.euler, phi = self.phi)
# 夾爪變成 drag_mode
elif self.state == drag_grasp:
print('self.state == drag_grasp')
self.state = busy
if self.close_box == False: #判斷是否為要關箱子
self.nextState = move_drawer
else:
self.nextState = standby_safe_point # 現在在stand by, 到safe_point時有機會繞兩次大圈, 但不會有事
self.arm.clear_cmd()
gripper.Send_Gripper_Command('drag_mode')
#(第三個動作)打開抽屜的位置
elif self.state == move_drawer:
print('self.state == move_drawer')
self.arm.set_speed(self.speed)
self.state = busy
self.nextState = catch_drawer
self.pos = [0.4, 0.62, -0.63]
self.euler = [0, 0, 90]
self.phi = 90
self.arm.ikMove(mode= 'line', pos = self.pos, euler = self.euler, phi = self.phi)
# 抓抽屜
elif self.state == catch_drawer:
print('self.state == catch_drawer')
self.arm.clear_cmd()
self.state = busy
gripper.Send_Gripper_Command('Catch_all')
self.nextState = back_pos
# 拉開抽屜
elif self.state == back_pos:
print('self.state == back_pos')
self.arm.set_speed(self.speed)
self.state = busy
self.nextState = loosen_drawer
self.pos = [0.4, 0.42, -0.63]
self.euler = [0, 0, 90]
self.phi = 90
self.arm.ikMove(mode= 'line', pos = self.pos, euler = self.euler, phi = self.phi)
# 放開抽屜
elif self.state == loosen_drawer:
print('self.state == loosen_drawer')
self.arm.clear_cmd()
self.state = busy
gripper.Send_Gripper_Command('Loosen_all')
self.nextState = go_safe_point1
# 開完箱子的第一個安全動作(改姿態+位置) ＃兼關完抽屜後, 回去時的第一個安全動作
elif self.state == go_safe_point1:
print('self.state == go_safe_point1')
self.arm.set_speed(self.speed)
self.state = busy
self.nextState = go_safe_point2
self.pos = [0.48, 0.15, -0.85]
self.euler = [0, 0, 0]
self.phi = 90
self.arm.ikMove(mode= 'p2p', pos = self.pos, euler = self.euler, phi = self.phi)
# 開完箱子的第二個安全動作(上來) ＃兼關完抽屜後, 回去時的第二個安全動作
elif self.state == go_safe_point2:
print('self.state == go_safe_point2')
self.arm.set_speed(self.faster_speed)
self.state = busy
self.nextState = go_safe_point3
self.pos = [0.48, 0.15, -0.4]
self.euler = [0, 0, 0]
self.phi = 90
self.arm.ikMove(mode= 'p2p', pos = self.pos, euler = self.euler, phi = self.phi)
# 開完箱子的第三個安全動作(改fai角度+y往前10公分) ＃兼關完抽屜後, 回去時的第三個安全動作(happy ending)
elif self.state == go_safe_point3:
print('self.state == go_safe_point3')
self.arm.set_speed(self.speed)
self.state = busy
if self.finish_pos == True:
self.nextState = idle
else:
#self.nextState = init_grasp
self.nextState = wait_img_pos
self.pos = [0.48, 0.25, -0.4]
self.euler = [0, 0, 0]
self.phi = 0
self.arm.ikMove(mode= 'p2p', pos = self.pos, euler = self.euler, phi = self.phi)
# 夾爪初始化
elif self.state == init_grasp:
print('self.state == init_grasp')
self.state = busy
#self.nextState = wait_img_pos
self.nextState = move_to_obj
self.arm.clear_cmd()
gripper.Send_Gripper_Command('3D_mode')
elif self.state == wait_img_pos: # wait_img_pos
print('self.state == wait_img_pos')
self.state = wait_img_pos
# print('len(self.img_data_list = ', len(self.img_data_list))
#print('aaaaa')
#rospy.Subscriber('/object/ROI_array', ROI_array, self.get_obj_info_cb)
#if(len(self.img_data_list)!=0):
#print("str(self.img_data.object_name): ", str(self.img_data.object_name))
if(len(self.img_data_list)!=0):
#print('bbbb')
self.No_Object_count = 0
for i in range(len(self.img_data_list)):
# if(i!=0):
# print("\n")
if (self.img_data.min_x > 638) and (self.img_data.min_y > 156) and (self.img_data.Max_x < 1188) and (self.img_data.Max_y < 811):
print("----- stra detect object_" + str(i) + " ----- ")
print("object_name = " + str(self.img_data.object_name))
print("score = " + str(self.img_data.score))
print("min_xy = [ " + str( [self.img_data.min_x, self.img_data.min_y] ) + " ]" )
print("max_xy = [ " + str( [self.img_data.Max_x, self.img_data.Max_y] ) + " ]" )
self.nextState = goto
self.obj_name = self.img_data.object_name
x = (self.img_data.min_x + self.img_data.Max_x)/2
y = (self.img_data.min_y + self.img_data.Max_y)/2
time.sleep(1)
else:
print('object over range!!')
else:
print('no object!!!')
self.nextState = wait_img_pos
self.No_Object_count += 1
self.state = self.nextState
if self.No_Object_count == 500: #判斷桌上是否沒有物件
self.state = move_standby
self.close_box = True
# 防止影像狀態機怪怪的空狀態(必定接在wait_img_pos後面)
elif self.state == goto:
print('self.state == goto')
self.state = busy
if self.standby_safeFlag == True:
self.nextState = standby_safe_point
else:
self.nextState = move_standby
# 開完箱子的第四個安全動作(回到stand by位置前的那個點)
elif self.state == standby_safe_point:
print('self.state == standby_safe_point')
self.arm.set_speed(self.speed)
self.standby_safeFlag = False
self.state = busy
if self.close_box == False:
self.nextState = move_standby
else:
self.nextState = close_box_standy
self.pos = [0.2, 0.4, -0.3]
self.euler = [0, 0, 0]
self.phi = 0
self.arm.ikMove(mode= 'p2p', pos = self.pos, euler = self.euler, phi = self.phi)
# 回到stand by位置 + 判斷是要 1.關抽屜 2.用兩指模式 或 3.三指模式
elif self.state == move_standby:
print('self.state == move_standby')
self.arm.set_speed(self.speed)
self.state = busy
if self.close_box == False:
if self.obj_name == 'red_square' or self.obj_name == 'screw':
self.nextState = mode_2D_catch
self.mode_2D = True
else:
self.mode_2D = False
#self.nextState = move_to_obj
self.nextState = init_grasp
else:
self.nextState = drag_grasp
self.pos = [0, 0.5, -0.4]
self.euler = [0, 0, 0]
self.phi = 0
self.arm.ikMove(mode = 'p2p', pos = self.pos, euler = self.euler, phi = self.phi)
# 到二指模式
elif self.state == mode_2D_catch:
print('self.state == mode_2D_catch')
self.arm.clear_cmd()
self.state = busy
gripper.Send_Gripper_Command('catch_to_2D')
self.nextState = move_to_obj
elif self.state == move_to_obj:
global x
global y
print('self.state == move_to_obj')
self.arm.set_speed(self.speed)
posX , posY = self.Image_transform(x, y)
self.state = busy
self.nextState = down
self.pos = [round(posX, 4), round(posY, 4), -0.45]
print(self.pos)
self.euler = [0, 0, 0]
self.phi = 0
self.arm.ikMove(mode= 'p2p', pos = self.pos, euler = self.euler, phi = self.phi)
elif self.state == down: # down
print('self.state == down')
self.arm.set_speed(self.faster_speed)
self.state = busy
self.nextState = down_sec
self.pos = [0, 0, -0.1]
self.arm.relative_move_pose(mode='p2p', pos=self.pos)
rospy.sleep(.1)
elif self.state == down_sec:
print('self.state == down_sec')
self.arm.set_speed(self.speed)
self.state = busy
self.nextState = grasping
if self.mode_2D == True:
self.pos = [0, 0, -0.09]
else:
self.pos = [0, 0, -0.1]
self.arm.relative_move_pose(mode='p2p', pos=self.pos)
elif self.state == grasping:
print('self.state == ready to grasping')
self.arm.clear_cmd()
self.state = busy
if self.mode_2D == True:
gripper.Send_Gripper_Command('2D_catch')
else:
gripper.Send_Gripper_Command('Catch_all')
self.nextState = up
elif self.state == up: # up
print('self.state == up')
self.arm.set_speed(self.faster_speed)
self.state = busy
self.nextState = box_standby
if self.mode_2D == True:
self.pos = [0, 0, 0.20]
else:
self.pos = [0, 0, 0.21]
self.arm.relative_move_pose(mode='p2p', pos=self.pos)
# 就是stand_by, 跟stand by狀態完全一樣
elif self.state == box_standby:
print('self.state == box_standby')
self.arm.set_speed(self.speed)
self.state = busy
self.nextState = move_to_box
self.pos = [0, 0.5, -0.4]
self.euler = [0, 0, 0]
self.phi = 0
self.arm.ikMove(mode = 'p2p', pos = self.pos, euler = self.euler, phi = self.phi)
elif self.state == move_to_box:
print('self.state == move_to_box')
self.arm.set_speed(self.speed)
self.state = busy
#self.nextState = box_down
self.nextState = box_grasp
self.pos = [0.4, 0.6, -0.3]
self.euler = [0, 0, 10]
self.phi = 0
self.arm.ikMove(mode= 'line', pos = self.pos, euler = self.euler, phi = self.phi)
# 放開那個物件(丟到箱子裡面)
elif self.state == box_grasp:
print('self.state == box_grasp')
self.state = busy
if self.mode_2D == True:
gripper.Send_Gripper_Command('loosen_to_3D')
else:
gripper.Send_Gripper_Command('Loosen_all')
#self.nextState = box_up
self.nextState = wait_img_pos
# 要去關抽屜之前的第一個緩衝點
elif self.state == close_box_standy:
print('self.state == close_box_standy')
self.arm.set_speed(self.speed)
self.state = busy
self.nextState = move_to_init
self.pos = [0.48, 0.25, -0.4]
self.euler = [0, 0, 0]
self.phi = 0
self.arm.ikMove(mode= 'p2p', pos = self.pos, euler = self.euler, phi = self.phi)
# 回到init點 (要去關抽屜之前的第二個緩衝點)
elif self.state == move_to_init:
print('self.state == move_to_init')
self.arm.set_speed(self.speed)
self.state = busy
self.nextState = close_box_down
self.pos = [0.48, 0.25, -0.4]
self.euler = [0, 0, 0]
self.phi = 90
self.arm.ikMove(mode= 'p2p', pos = self.pos, euler = self.euler, phi = self.phi)
# 要去關抽屜之前的第三個緩衝點
elif self.state == close_box_down:
print('self.state == close_box_down')
self.arm.set_speed(self.faster_speed)
self.state = busy
self.nextState = ready_close_box
self.pos = [0.48, 0.15, -0.85]
self.euler = [0, 0, 0]
self.phi = 90
self.arm.ikMove(mode= 'p2p', pos = self.pos, euler = self.euler, phi = self.phi)
# 要去關抽屜之前的第四個緩衝點(切換到要抓抽屜手把的姿態)
elif self.state == ready_close_box:
print('self.state == ready_close_box')
self.arm.set_speed(self.speed)
self.state = busy
self.nextState = box_catch
self.pos = [0.4, 0.4, -0.63]
self.euler = [0, 0, 90]
self.phi = 90
self.arm.ikMove(mode= 'p2p', pos = self.pos, euler = self.euler, phi = self.phi)
# 要關抽屜時的抓手把狀態
elif self.state == box_catch:
print('self.state == box_catch')
self.state = busy
self.nextState = close_box_finish
self.arm.clear_cmd()
gripper.Send_Gripper_Command('Catch_all')
# 關抽屜
elif self.state == close_box_finish:
print('self.state == close_box_finish')
self.arm.set_speed(self.speed)
self.state = busy
self.nextState = box_loosen
self.pos = [0.4, 0.63, -0.63]
self.euler = [0, 0, 90]
self.phi = 90
self.arm.ikMove(mode= 'line', pos = self.pos, euler = self.euler, phi = self.phi)
# 關完抽屜後手放開
elif self.state == box_loosen:
print('self.state == box_loosen')
self.state = busy
self.nextState = back_safe_pose
self.arm.clear_cmd()
gripper.Send_Gripper_Command('Loosen_all')
# 關完抽屜後回去
elif self.state == back_safe_pose:
print('self.state == back_safe_pose')
self.arm.set_speed(self.speed)
self.state = busy
self.finish_pos = True
self.nextState = go_safe_point1
self.pos = [0.4, 0.42, -0.63]
self.euler = [0, 0, 90]
self.phi = 90
self.arm.ikMove(mode= 'line', pos = self.pos, euler = self.euler, phi = self.phi)
#-------------------------座標轉換------------------------------------------------------
def Image_transform(self, Camera_Image_X, Camera_Image_Y):
Arm_posX = (866 - Camera_Image_Y)*0.000889 - 0.4795
Arm_posY = (974 - Camera_Image_X)*0.000889 + 0.3960
return Arm_posX, Arm_posY
#-------------------------------------------------------------------------------
def get_obj_info_cb(self, data):
self.img_data = ROI()
self.img_data_list = data.ROI_list
#print("Detected object number = " + str(len(data.ROI_list)))
for i in range(len(data.ROI_list)):
#if (data.ROI_list[i].min_x > 400) and (data.ROI_list[i].min_y) > 20 and (data.ROI_list[i].Max_x < 1440) and (data.ROI_list[i].Max_y < 990):
self.img_data.object_name = data.ROI_list[i].object_name
self.img_data.score = data.ROI_list[i].score
self.img_data.min_x = data.ROI_list[i].min_x
self.img_data.min_y = data.ROI_list[i].min_y
self.img_data.Max_x = data.ROI_list[i].Max_x
self.img_data.Max_y = data.ROI_list[i].Max_y
return self.img_data
class CDualArmTask:
def __init__(self, _name = '/robotis'):
"""Initial object."""
en_sim = False
if len(sys.argv) >= 2:
rospy.set_param('en_sim', sys.argv[1])
en_sim = rospy.get_param('en_sim')
self.name = _name
self.arm = ArmTask(self.name + '_arm')
self.pick_list = 2
self.pos = (0, 0, 0)
self.euler = (0, 0, 0)
self.phi = 0
if en_sim:
self.suction = SuctionTask(self.name + '_gazebo')
print "aa"
else:
self.suction = SuctionTask(self.name)
print "bb"
def SetSpeed(self, spd):
self.arm.set_speed(spd)
def IdelPos(self):
self.arm.set_speed(50)
self.arm.jointMove(0, (0, -0.5, 0, 1, 0, -0.5, 0))
self.suction.gripper_suction_deg(0)
def InitialPos(self):
self.arm.set_speed(50)
self.arm.jointMove(0, (0, 0, 0, 0, 0, 0, 0))
self.suction.gripper_suction_deg(0)
def MoveAbs(self, Line_PtP, Pos, Euler, Redun):
if(Line_PtP == 'line'):
Line_PtP = 'line'
else:
Line_PtP = 'p2p'
self.arm.ikMove(Line_PtP, Pos, Euler, Redun)
def MoveRelPos(self, Line_PtP, Pos):
if(Line_PtP == 'line'):
Line_PtP = 'line'
else:
Line_PtP = 'p2p'
print('RelMove1')
self.arm.relative_move_pose(Line_PtP, Pos)
print('RelMove1')
def SuctionEnable(self, On_Off):
if(On_Off == True):
self.suction.gripper_vaccum_on()
elif(On_Off == False):
self.suction.gripper_vaccum_off()
def SetSuctionDeg(self, Deg):
self.suction.gripper_suction_deg(Deg)
def SuckSuccess(self):
print('suckflag')
return self.suction.is_grip
def StopRobot_and_ClearCmd(self):
# Force stop robot
print('StopRobot')
self.arm.clear_cmd()
def PauseRobot(self, PauseFlag):
# True : Pause
# False: Continue
self.arm.freeze(PauseFlag)
def GetArmPos(self):
print('Pos1')
GetArmInfo = self.arm.get_fb()
print('Pos2')
ArmPos = GetArmInfo.group_pose.position
print('Pos3')
return ArmPos # .x .y .z
def ChangeModeToGetSuckState(self, Flag):
# True : Get suck success state
# False: Arduino for RFID
self.suction.switch_mode(Flag)