def testUnthread():
OriginPosition = [418.4, 0, 629.89, 0, -90, -180]
# GoPosition = [424.686, -169.017, 297.854, 152.65, -4.135, 174.468]
# while True:
# radius = random.uniform(-30, 30)
# GoPosition[0] += radius
# GoPosition[1] += radius
# GoPosition[2] += radius
GoPosition = [560.501, -33.202, 390.669, 152.65, -4.135, 174.468]
while True:
# radius = random.uniform(-30, 10)
# GoPosition[0] += radius
# GoPosition[1] += radius
# GoPosition[2] += radius
# print GoPosition
TargetOk = Com.get_status(ShareMem, "TARGET_OK", GoPosition)
if TargetOk[0] != 0:
print "************Error!!!can not go to the position!!***************"
continue
else:
print "GoPosition1 = ", GoPosition
Com.command_to_robot(ShareMem, "GO", GoPosition, 0)
# time.sleep(2)
ActualPosition = Com.get_curr_pos(ShareMem)[0:6]
print "ActualPosition1 = ", ActualPosition
# GoPosition[1] = copy.deepcopy(-GoPosition[1])
print "GoPosition2 = ", OriginPosition
Com.command_to_robot(ShareMem, "GO", OriginPosition, 0)
# time.sleep(2)
ActualPosition2 = Com.get_curr_pos(ShareMem)[0:6]
print "ActualPosition2 = ", ActualPosition2
def TestMove(self, ThreadOrNot, testNumber):
"""
command_to_robot()
shareMem: shmmemory name
"GO": target cmd
ToPosition: target position
ThreadOrNot=1: open thread
ThreadOrNot=0: unthread
"""
Com.command_to_robot(ShareMem, "GO", OriginPosition, ThreadOrNot=0)
for i in range(0, testNumber):
MovePosition = [264, 120, 123, 0]
randomList = [10, 10, 0, 0, 0, 0]
MovePosition = randomPosition(MovePosition,
randomRangeList=randomList)
if not testTarget_OK(MovePosition):
start = time.time()
Com.command_to_robot(ShareMem, "MOVE", MovePosition,
ThreadOrNot)
print "MOVE time = ", time.time() - start
time.sleep(2)
CurrentPosition = Com.get_curr_pos(ShareMem)
print CurrentPosition[0:6]
compareList(MovePosition, CurrentPosition, error=1)
else:
continue
def initSpeed(SpeedValue):
Speed = [SpeedValue, SpeedValue, SpeedValue]
Accel = [
SpeedValue, SpeedValue, SpeedValue, SpeedValue, SpeedValue, SpeedValue
]
Com.command_to_robot(ShareMem, "SPEED", Speed)
Com.command_to_robot(ShareMem, "ACCEL", Accel)
def TestJumpWithHighSpeed(self, ThreadOrNot, testNumber):
"""
command_to_robot()
shareMem: shmmemory name
"GO": target cmd
ToPosition: target position
ThreadOrNot=1: open thread
ThreadOrNot=0: unthread
"""
Com.command_to_robot(ShareMem, "GO", OriginPosition, ThreadOrNot=0)
for i in range(0, testNumber):
JumpHighPosition = [310, 197, 70, 0, 115, 0]
randomList = [25, 25, 30, 30, 0, 0]
JumpHighPosition = randomPosition(JumpHighPosition,
randomRangeList=randomList)
if not testTarget_OK(JumpHighPosition):
start = time.time()
Com.command_to_robot(ShareMem, "JUMP_WITH_HIGH_SPEED",
JumpHighPosition, ThreadOrNot)
print "JUMP_HIGH time = ", time.time() - start
time.sleep(10)
CurrentPosition = Com.get_curr_pos(ShareMem)
CurrentPosition[4] = 115
print CurrentPosition[0:6]
compareList(JumpHighPosition, CurrentPosition, error=1)
else:
continue
def runConveyor(self, dis_mm=None):
self._my_logger.info('run Conveyor')
Com.command_to_robot(self.__ShmBuf, "CONV_ON", [1])
if dis_mm is not None:
origin_dis_mm = self.getConveyorDis_mm()
while True:
time.sleep(0.1)
now_dis = self.getConveyorDis_mm()
if now_dis - origin_dis_mm > dis_mm:
self.stopConveyor()
break
def testMove():
OriginPosition = [418.4, 0, 629.89, 0, -90, -180]
GoPosition = [464.686, -169.017, 297.854, 152.65, -4.135, 174.468]
while True:
radius = random.uniform(-30, 30)
GoPosition[0] += radius
GoPosition[1] += radius
GoPosition[2] += radius
print GoPosition
TargetOk = Com.get_status(ShareMem, "TARGET_OK", GoPosition)
if TargetOk[0] != 0:
print "************Error!!!can not go to the position!!***************"
else:
Com.command_to_robot(ShareMem, "MOVE", GoPosition)
ActualPosition = Com.get_curr_pos(ShareMem)[0:6]
print "GoPosition = ", GoPosition
print "ActualPosition = ", ActualPosition
GoPosition[1] = -GoPosition[1]
Com.command_to_robot(ShareMem, "MOVE", GoPosition)
ActualPosition = Com.get_curr_pos(ShareMem)[0:6]
print "GoPosition = ", GoPosition
print "ActualPosition = ", ActualPosition
GoPosition = [464.686, -169.017, 297.854, 152.65, -4.135, 174.468]
def TestGO(self, ThreadOrNot, testNumber, GoPosition):
"""
command_to_robot()
shareMem: shmmemory name
"GO": target cmd
ToPosition: target position
ThreadOrNot=1: open thread
ThreadOrNot=0: unthread
"""
for i in range(0, testNumber):
# GoPosition = [300,120,80,20]
# randomList = [10,10,10,10,10,10]
GoPosition = self.YamlLoadPoints['Go']
GoPosition = randomPosition(GoPosition, randomRangeList=randomList)
if not testTarget_OK(GoPosition):
start = time.time()
Com.command_to_robot(ShareMem, "GO", GoPosition, ThreadOrNot)
print "Go time = ", time.time() - start
time.sleep(2)
CurrentPosition = Com.get_curr_pos(ShareMem)
print CurrentPosition[0:6]
compareList(GoPosition, CurrentPosition, error=1)
else:
continue
def testThread():
OriginPosition = [418.4, 0, 629.89, 0, -90, -180]
GoPosition = [424.686, -169.017, 297.854, 152.65, -4.135, 174.468]
while True:
radius = random.uniform(-30, 30)
GoPosition[0] += radius
GoPosition[1] += radius
GoPosition[2] += radius
print GoPosition
TargetOk = Com.get_status(ShareMem, "TARGET_OK", GoPosition)
if TargetOk[0] != 0:
print "************Error!!!can not go to the position!!***************"
else:
Com.command_to_robot(ShareMem, "GO", GoPosition)
ActualPosition = Com.get_curr_pos(ShareMem)[0:6]
print "GoPosition = ", GoPosition
print "ActualPosition = ", ActualPosition
GoPosition[1] = -GoPosition[1]
Com.command_to_robot(ShareMem, "MOVE", GoPosition)
ActualPosition = Com.get_curr_pos(ShareMem)[0:6]
print "GoPosition = ", GoPosition
print "ActualPosition = ", ActualPosition
GoPosition = [464.686, -169.017, 297.854, 152.65, -4.135, 174.468]
JumpP1 = [
474.39855489, 205.01568053, 115.40855304, 152.63334917,
-4.30779311, 174.62465658
]
JumpP2 = [
479.14825634, 207.09563971, 395.88019415, 152.40037482,
-2.2939525, 172.08569708
]
JumpP3 = [
479.3525761, -209.63551643, 395.92318385, 152.40334516,
-2.29067504, 172.0860196
]
JumpP4 = [
479.39361121, -209.6325469, 146.89025608, 152.39699665,
-2.27772204, 172.07260997
]
Com.command_to_robot(ShareMem, "GO", JumpP1)
JUMP3Position = [JumpP2, JumpP3, JumpP4, 0, 0, 0]
Com.command_to_robot(ShareMem, "JUMP3", JUMP3Position)
randomNum = random.uniform(-40, 40)
JumpP1[0] = JumpP1[0] + randomNum
JumpP1[1] = JumpP1[1] + randomNum
JumpP1[2] = JumpP1[2] + randomNum
randomNum = random.uniform(-40, 40)
JumpP2[0] = JumpP2[0] + randomNum
JumpP2[1] = JumpP2[1] + randomNum
JumpP2[2] = JumpP2[2] + randomNum
randomNum = random.uniform(-40, 40)
JumpP3[0] = JumpP3[0] + randomNum
JumpP3[1] = JumpP3[1] + randomNum
JumpP3[2] = JumpP3[2] + randomNum
randomNum = random.uniform(-40, 40)
JumpP4[0] = JumpP4[0] + randomNum
JumpP4[1] = JumpP4[1] + randomNum
JumpP4[2] = JumpP4[2] - randomNum
Com.command_to_robot(ShareMem, "GO", JumpP1)
JUMP3Position = [JumpP2, JumpP3, JumpP4, 0, 0, 0]
Com.command_to_robot(ShareMem, "JUMP3", JUMP3Position)
# JumpP2[2] = JumpP2[2]+randomNum
# randomNum = random.uniform(-40, 40)
# JumpP3[0] = JumpP3[0]+randomNum
# JumpP3[1] = JumpP3[1]+randomNum
# JumpP3[2] = JumpP3[2]+randomNum
# randomNum = random.uniform(-40, 40)
# JumpP4[0] = JumpP4[0]+randomNum
# JumpP4[1] = JumpP4[1]+randomNum
# JumpP4[2] = JumpP4[2]-randomNum
# Com.command_to_robot(ShareMem, "GO", JumpP1, 0)
# JUMP3Position = [JumpP2,JumpP3,JumpP4,0,0,0]
# Com.command_to_robot(ShareMem, "JUMP3",JUMP3Position, 0)
if __name__ == '__main__':
ShareMem = Com.link_port("../../robot_control_gui/shm_VS.bin")
assert ShareMem != None, "Can not connect share memory! Please check the share memory filename!"
Speed = [40, 40, 40]
Accel = [40, 40, 40, 40, 40, 40]
Com.command_to_robot(ShareMem, "SPEED", Speed)
Com.command_to_robot(ShareMem, "ACCEL", Accel)
# testGo()
# testMove()
# testThread()
testUnthread()
def sendRobotPos(self,
robotPos,
error=None,
type="GO",
wait=True,
timeout=0.1,
resendTimes=3):
assert isinstance(robotPos, (np.ndarray, list, tuple))
# assert error is None or len(error) == 6
T0 = time.time()
if 6 == len(robotPos) and not self.isPoseOK(robotPos):
raise RobotPosError('go pose is error')
while self.__IsSendingCommand:
self._my_logger.info('command ' + type + str(robotPos) +
'is waitting')
time.sleep(0.01)
self.__IsSendingCommand = True
self._my_logger.info(type + " sendRobotPos: " + str(robotPos))
if isinstance(robotPos, np.ndarray):
robotPos.shape = (len(robotPos), )
robotPos = robotPos.tolist()
Com.command_to_robot(shm_obj=self.__ShmBuf,
cmd_str=type,
param_list=robotPos,
ThreadOrNot=1,
error=error,
wait=wait)
self._my_logger.info('Command to robot end')
self.__IsSendingCommand = False
if error is None:
return False
PrePos = None
while True:
CurPos = self.getCurPos()
time.sleep(0.01)
if (abs(np.array(robotPos).reshape(-1) - CurPos.reshape(-1)) <
np.array(error).reshape(-1)).all():
return True
if time.time() - T0 > timeout:
if PrePos is not None:
AbsPos = np.abs(PrePos - CurPos)
self._my_logger.info('AbsPos: \n%s', AbsPos)
if (AbsPos < 0.05
).all(): # robot is stop if the abs dis less than 0.05
if resendTimes <= 0:
raise RobotRunTimeError('Robot run time out')
self._my_logger.warn('command %s resending...', type)
self._my_logger.info('cur pos: %s', CurPos)
self._my_logger.info('error: %s', error)
self.sendRobotPos(robotPos=robotPos,
error=error,
type=type,
wait=wait,
timeout=timeout,
resendTimes=resendTimes - 1)
self._my_logger.warn('end of resend')
PrePos = CurPos.copy()
time.sleep(0.1)
def setHandType(self, flag):
flag_map = {'left': [0], 'right': [1], 'auto': [2]}
assert flag in flag_map
self._my_logger.info('set hand type: %s', flag_map[flag])
Com.command_to_robot(self.__ShmBuf, "HAND", flag_map[flag])
def clearConveyor(self):
self._my_logger.info('clear Conveyor')
Com.command_to_robot(self.__ShmBuf, "CONV_CLEAR", [1])
Com.command_to_robot(self.__ShmBuf, "CONV_CLEAR", [0])
def stopConveyor(self):
self._my_logger.info('stop Conveyor')
Com.command_to_robot(self.__ShmBuf, "CONV_ON", [0])
def setConveyorSpeed(self, speed):
"speed: [0~28]"
self._my_logger.info('set Conveyor Speed: ' + str(speed))
Com.command_to_robot(self.__ShmBuf, "CONV_SPEED", speed)
def setAcce(self, AcceValList):
assert isinstance(AcceValList, (list, tuple))
assert len(AcceValList) == 6, 'SpeedValList has 6 values'
self._my_logger.info('set acce')
AcceValList = np.int32(AcceValList).tolist()
Com.command_to_robot(self.__ShmBuf, "ACCEL", AcceValList)
def setSpeed(self, SpeedValList):
assert isinstance(SpeedValList, (list, tuple))
assert len(SpeedValList) == 3, 'SpeedValList has 3 values'
self._my_logger.info('set speed')
SpeedValList = np.int32(SpeedValList).tolist()
Com.command_to_robot(self.__ShmBuf, "SPEED", SpeedValList)
