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 Task:
"""def __init__(self, _name = '/robotis'):
Initial object.
self.en_sim = False
#en_sim = False
if len(sys.argv) >= 2:
rospy.set_param('en_sim', sys.argv[1])
en_sim = rospy.get_param('en_sim')
# if en_sim:
# print en_sim
# return
self.name = _name
self.state = initPose
self.nextState = idle
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 "enable gazebo"
else:
self.suction = SuctionTask(self.name)
print "0"
"""
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.state = initPose @ property
def finish(self):
return self.pickList == self.pickListAll
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.sandwitchPos = [0, 0, 0]
self.sandwitchEu = [0, 0, 0]
self.sandwitchSuc = 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)
@property
def finish(self):
return self.pick_list == 0
def getObjectPos(self):
lunchboxPos = [[-0.4, -0.15, -0.63], [-0.4, -0.15, -0.68]]
drinkPos = [[-0.4, 0.15, -0.63], [-0.4, 0.15, -0.68]]
if self.name == 'right':
self.pos, self.euler, self.phi = lunchboxPos[2 - self.pick_list], (
90, 0, 0), -30
elif self.name == 'left':
self.pos, self.euler, self.phi = drinkPos[2 -
self.pick_list], (-90, 0,
0), 30
def getPlacePos(self):
lunchboxPos = [[-0.4, -0.15, -0.63], [-0.4, -0.15, -0.68]]
drinkPos = [[-0.4, 0.15, -0.63], [-0.4, 0.15, -0.68]]
#-0.3,0.1,-0.4,0,0,0,0 binpos
if self.name == 'right':
self.pos, self.euler, self.phi = lunchboxPos[2 - self.pick_list], (
90, 0, 0), -30
elif self.name == 'left':
self.pos, self.euler, self.phi = drinkPos[2 -
self.pick_list], (-90, 0,
0), 30
def InitLeftArm(self):
self.arm.set_speed(30)
self.arm.jointMove(0, (0, 0, 0, 0, 0, 0, 0))
self.suction.gripper_suction_deg(0)
def getSafetyPos(self):
if self.name == 'right':
self.pos, self.euler, self.phi = (0, 0.49, -0.36), (0, 0, 0), 0
elif self.name == 'left':
self.pos, self.euler, self.phi = (0, 0.49, -0.36), (0, 0, 0), 0
def getShelfPos(self):
ShelfPos = ([0, 0.88, -0.48])
#ShelfPos = ([0, 0.7, -0.51])
if self.name == 'right':
self.pos, self.euler, self.phi = ShelfPos, (180, 180, 90), 0
elif self.name == 'left':
self.pos, self.euler, self.phi = ShelfPos, (180, 180, 90), 0
#self.pos, self.euler, self.phi = ShelfPos, (0, 0, 90), 0
def getBinfPos(self):
ShelfPos = ([-0.4, 0.2, -0.36])
#ShelfPos = ([0, 0.7, -0.51])
if self.name == 'right':
self.pos, self.euler, self.phi = ShelfPos, (0, 0, 0), 0
elif self.name == 'left':
self.pos, self.euler, self.phi = ShelfPos, (0, 0, 0), 0
def proces(self):
flag = False
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
return # must be include in your strategy
if self.state == idle:
print 'Idle'
if self.finish:
print 'finish'
return
else:
self.state = SafetyPos
print 'next SafetyPos'
#print "self.pick_list = " + str(self.pick_list)
elif self.state == initPose:
self.state = busy
self.InitLeftArm()
self.nextState = idle
elif self.state == SafetyPos:
self.state = busy
self.getSafetyPos()
self.arm.set_speed(90)
#self.arm.jointMove(0, (0, -2.27, 0, 2.44, 0, 0, 0))
self.arm.ikMove('p2p', self.pos, self.euler, self.phi)
if flag == False:
print 'flag=', flag
self.nextState = move2Shelf
else:
print 'Next state = move2Bin'
print 'flag=', flag
self.nextState = move2Bin
elif self.state == move2Shelf:
print 'Now State : Move2Shelf'
self.state = busy
self.nextState = moveIn2Shelf
self.getShelfPos()
self.arm.set_speed(90)
self.arm.ikMove('p2p', self.pos, self.euler, self.phi)
elif self.state == moveIn2Shelf:
print 'Now State : moveIn2Shelf'
self.state = busy
self.nextState = move2Object
self.getShelfPos()
#print("pos[2] type = ",type(self.pos))
self.pos[1] += 0.12
#self.pos[2] -= 0.33
self.arm.ikMove('line', self.pos, self.euler, self.phi)
elif self.state == move2Object:
print 'Now State : move2Object'
self.state = busy
self.arm.relative_move_pose('line', [0, 0, -0.05])
self.nextState = pickObject
#record position
elif self.state == pickObject:
#gripper_deg
self.state = busy
#self.arm.relative_move_pose('line', [0, 0.1, -0.1])
#self.suction.gripper_suction_deg(0)
#self.suction.gripper_vaccum_on()
self.nextState = move2Bin
elif self.state == leaveShelf:
self.state = busy
self.arm.relative_move_pose('line', [0, -0.1, +0.1])
self.nextState = SafetyPos
flag = True
print 'flag =', flag
elif self.state == move2Bin:
print 'Now state = move2Bin'
self.state = busy
self.getBinfPos()
self.arm.ikMove('p2p', self.pos, self.euler, self.phi)
self.nextState = move2PlacedPos
elif self.state == move2PlacedPos:
self.state = busy
self.nextState = placeObject
elif self.state == placeObject:
self.state = busy
self.nextState = leaveBin
self.suction.gripper_vaccum_off()
elif self.state == leaveBin:
self.state = busy
self.arm.set_speed(20)
self.arm.relative_move_pose('line', [0, 0, -0.24])
self.nextState = SafetyPos
flag = True
elif self.state == busy:
if self.arm.is_busy:
return
else:
self.state = self.nextState
class exampleTask:
def __init__(self, _name='/robotis'):
"""Initial object."""
en_sim = True
if len(sys.argv) >= 2:
rospy.set_param('en_sim', sys.argv[1])
en_sim = rospy.get_param('en_sim')
# if en_sim:
# print en_sim
# return
self.name = _name
self.state = initPose
self.nextState = idle
self.sucAngle = 0
self.arm = ArmTask(self.name + '_arm')
self.pick_list = 4
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")
@property
def finish(self):
return self.pick_list == 0
def getRearSafetyPos(self):
if self.name == 'right':
self.pos, self.euler, self.phi = (-0.1, -0.45, -0.45), (90, 0,
0), -30
#self.pos, self.euler, self.phi = (0.3, -0.3006, -0.46), (5.029, 82.029, 4.036), 60
print("a1")
elif self.name == 'left':
self.pos, self.euler, self.phi = (-0.1, 0.45, -0.45), (-90, 0,
0), 30
#self.pos, self.euler, self.phi = (0.3, 0.3506, -0.46), (5.029, 82.029, 4.036), -60
print("b1")
def getFrontSafetyPos(self):
if self.name == 'right':
self.pos, self.euler, self.phi = (0.1, -0.45, -0.45), (0, 20,
0), 45
print("a2")
elif self.name == 'left':
self.pos, self.euler, self.phi = (0.1, 0.45, -0.45), (0, 20,
0), -45
print("b2")
def getObjectPos(self):
lunchboxPos = [[-0.4, -0.15, -0.63], [-0.4, -0.15, -0.68],
[-0.4, -0.15, -0.63], [-0.4, -0.15, -0.63]]
drinkPos = [[-0.4, 0.15, -0.63], [-0.4, 0.15, -0.68],
[-0.4, 0.15, -0.68], [-0.4, 0.15, -0.68]]
if self.name == 'right':
self.pos, self.euler, self.phi = lunchboxPos[self.pick_list -
1], (90, 0, 0), -30
print("a3")
elif self.name == 'left':
self.pos, self.euler, self.phi = drinkPos[self.pick_list -
1], (-90, 0, 0), 30
print("b3")
def getPlacePos(self):
lunchboxPos = [[0.46, -0.16, -0.55], [0.51, -0.20, -0.55],
[0.46, -0.24, -0.55], [0.51, -0.28, -0.55]]
lunchboxRoll = [0, -10, -20, -25]
lunchboxSuc = [-30, -40, -50, -60]
drinkPos = [[0.51, 0.28, -0.55], [0.46, 0.24, -0.55],
[0.51, 0.20, -0.55], [0.46, 0.16, -0.55]]
drinkRoll = [25, 20, 10, 0]
drinkSuc = [-60, -50, -40, -30]
if self.name == 'right':
self.pos, self.euler, self.phi = lunchboxPos[self.pick_list - 1], (
lunchboxRoll[self.pick_list - 1], 90, 0), 45
self.sucAngle = lunchboxSuc[self.pick_list - 1]
print(self.pick_list)
print("a4")
elif self.name == 'left':
self.pos, self.euler, self.phi = drinkPos[self.pick_list - 1], (
drinkRoll[self.pick_list - 1], 90, 0), -45
self.sucAngle = drinkSuc[self.pick_list - 1]
print("b4")
#new test
def getPlace(self):
if self.name == 'right':
self.pos, self.euler, self.phi = (0.3, -0.3006,
-0.56), (5.029, 82.029,
4.036), 60
print("newA")
elif self.name == 'left':
self.pos, self.euler, self.phi = (0.3, 0.3506,
-0.56), (5.029, 82.029,
4.036), -60
print("newB")
def catchobj(self):
if self.name == 'right':
self.pos, self.euler, self.phi = (0.55, -0.3006,
-0.56), (5.029, 82.029,
4.036), 60
print("catchA")
elif self.name == 'left':
self.pos, self.euler, self.phi = (0.55, 0.3506,
-0.56), (5.029, 82.029,
4.036), -60
print("catchB")
def backPlace(self):
if self.name == 'right':
self.pos, self.euler, self.phi = (0.3, -0.3006,
-0.46), (5.029, 82.029,
4.036), 60
print("newA")
elif self.name == 'left':
self.pos, self.euler, self.phi = (0.3, 0.3506,
-0.46), (5.029, 82.029,
4.036), -60
print("newB")
#end
def proces(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
return # must be include in your strategy
if self.state == idle:
if self.finish:
return
else:
self.state = frontSafetyPos
print("self.pick_list = " + str(self.pick_list))
elif self.state == initPose:
self.state = busy
self.nextState = idle
self.arm.set_speed(SPEED)
self.arm.jointMove(0, (0, -0.5, 0, 1, 0, -0.5, 0))
#self.arm.jointMove(0, (0, 0, 0, 0, 0, 0, 0))
self.suction.gripper_suction_deg(0)
print("1")
elif self.state == frontSafetyPos:
self.state = busy
self.nextState = move2Shelf
self.getFrontSafetyPos()
self.arm.set_speed(SPEED)
self.arm.ikMove('line', self.pos, self.euler, self.phi)
self.suction.gripper_suction_deg(-20)
print("2")
elif self.state == rearSafetyPos:
self.state = busy
self.nextState = move2Bin
#self.nextState = move2point
self.getRearSafetyPos()
self.arm.ikMove('line', self.pos, self.euler, self.phi)
print("3")
#new test
elif self.state == move2point:
self.state = busy
self.nextState = catchobj
self.getPlace()
self.arm.ikMove('line', self.pos, self.euler, self.phi)
print("1111111")
elif self.state == catchobj:
self.state = busy
self.nextState = back2point
self.catchobj()
self.arm.ikMove('line', self.pos, self.euler, self.phi)
print("2222222")
elif self.state == back2point:
self.state = busy
self.nextState = initPose
self.backPlace()
self.arm.ikMove('line', self.pos, self.euler, self.phi)
print("3333333")
#end
elif self.state == move2Bin:
self.state = busy
self.nextState = placeObject
self.getObjectPos()
self.pos[2] += 0.2
self.arm.ikMove('line', self.pos, self.euler, self.phi)
print("4")
elif self.state == move2Shelf:
self.state = busy
self.nextState = moveIn2Shelf
self.getPlacePos()
self.pos[0] += -0.3
self.pos[2] += 0.1
self.arm.ikMove('line', self.pos, self.euler, self.phi)
print("5")
elif self.state == moveIn2Shelf:
self.state = busy
self.nextState = move2PlacedPos
self.getPlacePos()
self.pos[2] += 0.1
self.arm.ikMove('line', self.pos, self.euler, self.phi)
print("6")
elif self.state == leaveBin:
self.state = busy
self.nextState = idle
self.arm.set_speed(SPEED)
self.arm.relative_move_pose('line', [0, 0, 0.2])
print("7")
elif self.state == leaveShelf:
self.state = busy
self.nextState = rearSafetyPos
self.arm.relative_move_pose('line', [-0.3, 0, 0.1])
self.suction.gripper_suction_deg(0)
print("8")
elif self.state == move2Object:
self.state = busy
self.nextState = placeObject
self.arm.relative_move_pose('line', [0, 0, -0.2])
print("9")
elif self.state == move2PlacedPos:
self.state = busy
self.nextState = pickObject
self.arm.relative_move_pose('line', [0, 0, -0.1])
self.suction.gripper_suction_deg(self.sucAngle)
print("10")
elif self.state == pickObject:
self.state = busy
self.nextState = leaveShelf
self.suction.gripper_vaccum_on()
rospy.sleep(.5)
print("11")
elif self.state == placeObject:
self.state = busy
self.nextState = idle
self.pick_list -= 1
self.suction.gripper_vaccum_off()
print("12")
elif self.state == busy:
if self.arm.is_busy:
return
else:
self.state = self.nextState
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 exampleTask:
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')
# if en_sim:
# print en_sim
# return
self.name = _name
self.state = initPose
self.nextState = idle
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"
@property
def finish(self):
return self.pick_list == 0
def getRearSafetyPos(self):
if self.name == 'right':
self.pos, self.euler, self.phi = (-0.1, -0.45, -0.45), (90, 0, 0), -30
#self.pos, self.euler, self.phi = (0.3, -0.3006, -0.46), (5.029, 82.029, 4.036), 60
print "a1"
elif self.name == 'left':
self.pos, self.euler, self.phi = (-0.1, 0.45, -0.45), (-90, 0, 0), 30
#self.pos, self.euler, self.phi = (0.3, 0.3506, -0.46), (5.029, 82.029, 4.036), -60
print "b1"
def getFrontSafetyPos(self):
if self.name == 'right':
self.pos, self.euler, self.phi = (0.1, -0.45, -0.45), (0, 20, 0), 45
print "a2"
elif self.name == 'left':
self.pos, self.euler, self.phi = (0.1, 0.45, -0.45), (0, 20, 0), -45
print "b2"
def getObjectPos(self):
lunchboxPos = [[-0.4, -0.15, -0.63],
[-0.4, -0.15, -0.68]]
drinkPos = [[-0.4, 0.15, -0.63],
[-0.4, 0.15, -0.68]]
if self.name == 'right':
self.pos, self.euler, self.phi = lunchboxPos[2-self.pick_list], (90, 0, 0), -30
print "a3"
elif self.name == 'left':
self.pos, self.euler, self.phi = drinkPos[2-self.pick_list], (-90, 0, 0), 30
print "b3"
def getPlacePos(self):
lunchboxPos = [[0.5, -0.25, -0.54],
[0.5, -0.25, -0.49]]
drinkPos = [[0.5, 0.25, -0.5],
[0.42, 0.25, -0.5]]
if self.name == 'right':
self.pos, self.euler, self.phi = lunchboxPos[2-self.pick_list], (0, 90, 0), 45
print "a4"
elif self.name == 'left':
self.pos, self.euler, self.phi = drinkPos[2-self.pick_list], (0, 90, 0), -45
print "b4"
#new test
def getPlace(self):
if self.name == 'right':
self.pos, self.euler, self.phi = (0.3, -0.3006, -0.56), (5.029, 82.029, 4.036), 60
print "newA"
elif self.name == 'left':
self.pos, self.euler, self.phi = (0.3, 0.3506, -0.56), (5.029, 82.029, 4.036), -60
print "newB"
def catchobj(self):
if self.name == 'right':
self.pos, self.euler, self.phi = (0.55, -0.3006, -0.56), (5.029, 82.029, 4.036), 60
print "catchA"
elif self.name == 'left':
self.pos, self.euler, self.phi = (0.55, 0.3506, -0.56), (5.029, 82.029, 4.036), -60
print "catchB"
def backPlace(self):
if self.name == 'right':
self.pos, self.euler, self.phi = (0.3, -0.3006, -0.46), (5.029, 82.029, 4.036), 60
print "newA"
elif self.name == 'left':
self.pos, self.euler, self.phi = (0.3, 0.3506, -0.46), (5.029, 82.029, 4.036), -60
print "newB"
#end
def proces(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
return # must be include in your strategy
if self.state == idle:
if self.finish:
return
else:
self.state = rearSafetyPos
print "self.pick_list = " + str(self.pick_list)
elif self.state == initPose:
self.state = busy
self.nextState = idle
self.arm.set_speed(30)
self.arm.jointMove(0, (0, -0.5, 0, 1, 0, -0.5, 0))
#self.arm.jointMove(0, (0, 0, 0, 0, 0, 0, 0))
self.suction.gripper_suction_deg(0)
print "1"
elif self.state == frontSafetyPos:
self.state = busy
self.nextState = move2Shelf
self.getFrontSafetyPos()
self.arm.set_speed(30)
self.arm.ikMove('line', self.pos, self.euler, self.phi)
self.suction.gripper_suction_deg(-20)
print "2"
elif self.state == rearSafetyPos:
self.state = busy
self.nextState = move2Bin
#self.nextState = move2point
self.getRearSafetyPos()
self.arm.ikMove('line', self.pos, self.euler, self.phi)
print "3"
#new test
elif self.state == move2point:
self.state = busy
self.nextState = catchobj
self.getPlace()
self.arm.ikMove('line', self.pos, self.euler, self.phi)
print "1111111"
elif self.state == catchobj:
self.state = busy
self.nextState = back2point
self.catchobj()
self.arm.ikMove('line', self.pos, self.euler, self.phi)
print "2222222"
elif self.state == back2point:
self.state = busy
self.nextState = initPose
self.backPlace()
self.arm.ikMove('line', self.pos, self.euler, self.phi)
print "3333333"
#end
elif self.state == move2Bin:
self.state = busy
self.nextState = move2Object
print("pos[2] type = ",type(self.pos))
self.getObjectPos()
self.pos[2] += 0.2
print("pos[2] type = ",type(self.pos))
self.arm.ikMove('line', self.pos, self.euler, self.phi)
print "4"
elif self.state == move2Shelf:
self.state = busy
self.nextState = moveIn2Shelf
#print("pos[2] type = ",type(self.pos))
self.getPlacePos()
self.pos[0] += -0.3
self.pos[2] += 0.1
self.arm.ikMove('line', self.pos, self.euler, self.phi)
self.suction.gripper_suction_deg(-90)
print "5"
elif self.state == moveIn2Shelf:
self.state = busy
self.nextState = move2PlacedPos
self.getPlacePos()
#print("pos[2] type = ",type(self.pos[2]))
self.pos[2] += 0.1
self.arm.ikMove('line', self.pos, self.euler, self.phi)
print "6"
elif self.state == leaveBin:
self.state = busy
self.nextState = frontSafetyPos
self.arm.set_speed(20)
self.arm.relative_move_pose('line', [0, 0, 0.2])
print "7"
elif self.state == leaveShelf:
self.state = busy
self.nextState = idle
self.arm.relative_move_pose('line', [-0.3, 0, 0.1])
self.pick_list -= 1
self.suction.gripper_suction_deg(0)
print "8"
elif self.state == move2Object:
self.state = busy
self.nextState = pickObject
self.arm.relative_move_pose('line', [0, 0, -0.2])
print "9"
elif self.state == move2PlacedPos:
self.state = busy
self.nextState = placeObject
self.arm.relative_move_pose('line', [0, 0, -0.1])
print "10"
elif self.state == pickObject:
self.state = busy
self.nextState = leaveBin
#self.suction.gripper_vaccum_on()
print "11"
elif self.state == placeObject:
self.state = busy
self.nextState = leaveShelf
#self.suction.gripper_vaccum_off()
print "12"
elif self.state == busy:
if self.arm.is_busy:
return
else:
self.state = self.nextState
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 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)