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)