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 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