def groupSetHomePos(self, mask=None, setPos=None): """ Do Homing Manually. Note: this command may record joint offset value. :param mask: a list/set of joint number (0 - 5) ex. mask = [0,2,4] # will do homing for joint 1, 3, 5 :type mask: List :param setPos: a list/set of joint value that assign to joints according to mask ex. mask = [0,2,4], setPos = [0, 20, -90]. will set 0 deg to joint 1, 20 deg to joint 3, and -90 deg to joint 5. :type mask: List :return: error code :rtype: int """ if setPos is None or mask is None: print("arg mask and setPos need to provide") if not isinstance(setPos, list) or not isinstance(mask, list): print("arg mask and setPos both should be list") if len(setPos) != len(mask): print("arg mask and setPos should have the same number of element.") homePos = Pos_T() mask_set = [GROUP_AXIS_MASK_X, GROUP_AXIS_MASK_Y, GROUP_AXIS_MASK_Z, GROUP_AXIS_MASK_A, GROUP_AXIS_MASK_B, GROUP_AXIS_MASK_C] mask_sum = 0 for m, p in zip(mask, setPos): mask_sum += mask_set[m] homePos.pos[m] = p return self.__GroupSetHomePos__(self.id_, self.index_, c_int32(mask_sum), homePos)
def groupLine(self, desPos, maxVel=None): """ Enable the group line interpolation motion from the current position to the target position in the Cartesian space. Note: max. velocity setting is not implement in python version API. TODO: Check c++ version API and update to accept the macVel. :param desPos: a List of 6 element [x, y, z, roll, pitch, yaw] :param maxVel: set the limit of max. velocity. input 0 to ignore the parameter :return: error code :rtype: int """ mask = 0 mask += GROUP_AXIS_MASK_X mask += GROUP_AXIS_MASK_Y mask += GROUP_AXIS_MASK_Z mask += GROUP_AXIS_MASK_A mask += GROUP_AXIS_MASK_B mask += GROUP_AXIS_MASK_C for i in range(len(desPos)): self.desPos_.pos[i] = desPos[i] if maxVel is None: maxVel = c_void_p() else: print("This Python version API do not accept maxVel argument and set to NULL as defualt.") maxVel = c_void_p() return self.__GroupLine__(self.id_, self.index_, c_int32(mask), self.desPos_, maxVel)
def deviceOpenup(self, type_=None, idx_=None): """ Open up the device (Blocking call). :param type_: The specified type. 0: Simulation, 1: EtherCAT :type type_: int :param idx_: The specified index of device which is set to 0. :type idx_: int :return: error code :rtype: int """ if type_ is not None: self.type_ = c_int32(type_) if idx_ is not None: self.index_ = c_int32(idx_) ret = self.__DeviceOpenUp__(self.type_, self.index_, self.id_) if ret == SUCCESS: print("device id =", self.id_.value) return ret
def deviceShutdown(self, id_=None): """ Shutdown the device (Blocking call). :param id_: Device ID (DevID) :type id_: int (option) :return: error code :rtype: int """ if id_ is not None: self.id_ = c_int32(id_) return self.__DeviceShutdown__(self.id_)
def groupAxesHomeDrive(self, mask=None): """ Do Homing by Driver. Note: this command only work for DEVICE_TYPE_ETHERCAT (control real robot) :param mask: a list/set of joint number (0 - 5) ex. mask = [0,2,4] # will do homing for joint 1, 3, 5 :type mask: List :return: error code :rtype: int """ if mask is None: mask = [] mask_set = [GROUP_AXIS_MASK_X, GROUP_AXIS_MASK_Y, GROUP_AXIS_MASK_Z, GROUP_AXIS_MASK_A, GROUP_AXIS_MASK_B, GROUP_AXIS_MASK_C] mask_sum = 0 for m in mask: mask_sum += mask_set[m] return self.__GroupAxesHomeDrive__(self.id_, self.index_, c_int32(mask_sum))
def groupPtpAcsAll(self, desPos): """ Do PTP motion :param desPos: a List of 6 element [j1 - j6] :type desPos: List :return: error code :rtype: int """ for i in range(len(desPos)): self.desPos_.pos[i] = desPos[i] mask = 0 mask += GROUP_AXIS_MASK_X mask += GROUP_AXIS_MASK_Y mask += GROUP_AXIS_MASK_Z mask += GROUP_AXIS_MASK_A mask += GROUP_AXIS_MASK_B mask += GROUP_AXIS_MASK_C return self.__GroupPtpAcsAll__(self.id_, self.index_, c_int32(mask), self.desPos_)
def __init__(self, dll_path=DLL_PATH): self.dll_ = WinDLL(dll_path) self.ini_path_ptr_ = c_char_p(" ".encode('utf-8')) self.id_ = c_int32(0) self.index_ = c_int32(0) self.type_ = c_int32(DEVICE_TYPE_SIMULATOR) self.devState_ = c_int32(0) self.groupState_ = c_int32(0) self.groupVel_ = c_double(0) self.numGroup_ = c_int32(0) self.numGroupAxis_ = c_int32(0) self.actPos_ = Pos_T() self.desPos_ = Pos_T() self.refBasePntArr_ = [Pos_T() for i in range(3)] self.refBaseCoordTrans_ = CoordTrans_T() self.offsetByte_ = c_uint32(0) self.sizeByte_ = c_uint32(2) # total two bytes (16 bits) self.doValue_ = c_uint16(0) self.diValue_ = c_uint16(0) self.pnt_list = [] # a list to store poses as list of 12 joint # Provide ctypes function prototypes self.__GetLibVersion__ = self.dll_.NMC_GetLibVersion self.__GetLibVersion__.argtypes = [POINTER(c_int32), POINTER(c_int32), POINTER(c_int32), POINTER(c_int32)] self.__GetLibVersion__.restype = c_int32 self.__DeviceOpenUp__ = self.dll_.NMC_DeviceOpenUp self.__DeviceOpenUp__.argtypes = [c_int32, c_int32, POINTER(c_int32)] self.__DeviceOpenUp__.restype = c_int32 self.__DeviceShutdown__ = self.dll_.NMC_DeviceShutdown self.__DeviceShutdown__.argtypes = [c_int32] self.__DeviceShutdown__.restype = c_int32 self.__DeviceGetState__ = self.dll_.NMC_DeviceGetState self.__DeviceGetState__.argtypes = [c_int32, POINTER(c_int32)] self.__DeviceGetState__.restype = c_int32 self.__DeviceGetGroupCount__ = self.dll_.NMC_DeviceGetGroupCount self.__DeviceGetGroupCount__.argtypes = [c_int32, POINTER(c_int32)] self.__DeviceGetGroupCount__.restype = c_int32 self.__DeviceGetGroupAxisCount__ = self.dll_.NMC_DeviceGetGroupAxisCount self.__DeviceGetGroupAxisCount__.argtypes = [c_int32, c_int32, POINTER(c_int32)] self.__DeviceGetGroupAxisCount__.restype = c_int32 self.__SetIniPath__ = self.dll_.NMC_SetIniPath self.__SetIniPath__.argtypes = [POINTER(c_char)] self.__SetIniPath__.restype = c_int32 if HAS_ZUGBRUECKE: self.__SetIniPath__.memsync = [ { 'p' : [0], 'n' : True } ] self.__WriteOutputMemory__ = self.dll_.NMC_WriteOutputMemory self.__WriteOutputMemory__.argtypes = [c_int32, c_uint32, c_uint32, POINTER(c_uint16)] self.__WriteOutputMemory__.restype = c_int32 self.__ReadOutputMemory__ = self.dll_.NMC_ReadOutputMemory self.__ReadOutputMemory__.argtypes = [c_int32, c_uint32, c_uint32, POINTER(c_uint16)] self.__ReadOutputMemory__.restype = c_int32 self.__ReadInputMemory__ = self.dll_.NMC_ReadInputMemory self.__ReadInputMemory__.argtypes = [c_int32, c_uint32, c_uint32, POINTER(c_uint16)] self.__ReadInputMemory__.restype = c_int32 self.__GroupResetDriveAlmAll__ = self.dll_.NMC_GroupResetDriveAlmAll self.__GroupResetDriveAlmAll__.argtypes = [c_int32, c_int32] self.__GroupResetDriveAlmAll__.restype = c_int32 self.__GroupGetState__ = self.dll_.NMC_GroupGetState self.__GroupGetState__.argtypes = [c_int32, c_int32, POINTER(c_int32)] self.__GroupGetState__.restype = c_int32 self.__GroupResetState__ = self.dll_.NMC_GroupResetState self.__GroupResetState__.argtypes = [c_int32, c_int32] self.__GroupResetState__.restype = c_int32 self.__GroupEnable__ = self.dll_.NMC_GroupEnable self.__GroupEnable__.argtypes = [c_int32, c_int32] self.__GroupEnable__.restype = c_int32 self.__GroupDisable__ = self.dll_.NMC_GroupDisable self.__GroupDisable__.argtypes = [c_int32, c_int32] self.__GroupDisable__.restype = c_int32 self.__GroupSetVelRatio__ = self.dll_.NMC_GroupSetVelRatio self.__GroupSetVelRatio__.argtypes = [c_int32, c_int32, c_double] self.__GroupSetVelRatio__.restype = c_int32 self.__GroupGetVelRatio__ = self.dll_.NMC_GroupGetVelRatio self.__GroupGetVelRatio__.argtypes = [c_int32, c_int32, POINTER(c_double)] self.__GroupGetVelRatio__.restype = c_int32 self.__GroupGetActualPosAcs__ = self.dll_.NMC_GroupGetActualPosAcs self.__GroupGetActualPosAcs__.argtypes = [c_int32, c_int32, POINTER(Pos_T)] self.__GroupGetActualPosAcs__.restype = c_int32 self.__GroupGetActualPosPcs__ = self.dll_.NMC_GroupGetActualPosPcs self.__GroupGetActualPosPcs__.argtypes = [c_int32, c_int32, POINTER(Pos_T)] self.__GroupGetActualPosPcs__.restype = c_int32 self.__GroupAxesHomeDrive__ = self.dll_.NMC_GroupAxesHomeDrive self.__GroupAxesHomeDrive__.argtypes = [c_int32, c_int32, c_int32] self.__GroupAxesHomeDrive__.restype = c_int32 self.__GroupSetHomePos__ = self.dll_.NMC_GroupSetHomePos self.__GroupSetHomePos__.argtypes = [c_int32, c_int32, c_int32, POINTER(Pos_T)] self.__GroupSetHomePos__.restype = c_int32 self.__GroupPtpAcsAll__ = self.dll_.NMC_GroupPtpAcsAll self.__GroupPtpAcsAll__.argtypes = [c_int32, c_int32, c_int32, POINTER(Pos_T)] self.__GroupPtpAcsAll__.restype = c_int32 self.__GroupLine__ = self.dll_.NMC_GroupLine self.__GroupLine__.argtypes = [c_int32, c_int32, c_int32, POINTER(Pos_T), c_void_p] self.__GroupLine__.restype = c_int32 self.__GroupHalt__ = self.dll_.NMC_GroupHalt self.__GroupHalt__.argtypes = [c_int32, c_int32] self.__GroupHalt__.restype = c_int32 self.__Group3DShow__ = self.dll_.NMC_Group3DShow self.__Group3DShow__.argtypes = [c_int32, c_int32] self.__Group3DShow__.restype = c_int32 self.__Group3DAlwaysTop__ = self.dll_.NMC_Group3DAlwaysTop self.__Group3DAlwaysTop__.argtypes = [c_int32, c_int32, c_bool] self.__Group3DAlwaysTop__.restype = c_int32 self.__Group3DDrawPath__ = self.dll_.NMC_Group3DDrawPath self.__Group3DDrawPath__.argtypes = [c_int32, c_int32, c_bool] self.__Group3DDrawPath__.restype = c_int32 self.__BaseCalib_1p__ = self.dll_.NMC_BaseCalib_1p self.__BaseCalib_1p__.argtypes = [POINTER(Pos_T), POINTER(CoordTrans_T)] self.__BaseCalib_1p__.restype = c_int32 self.__BaseCalib_2p__ = self.dll_.NMC_BaseCalib_2p self.__BaseCalib_2p__.argtypes = [POINTER(Pos_T), POINTER(Pos_T), POINTER(CoordTrans_T)] self.__BaseCalib_2p__.restype = c_int32 self.__BaseCalib_3p__ = self.dll_.NMC_BaseCalib_3p self.__BaseCalib_3p__.argtypes = [POINTER(Pos_T), POINTER(Pos_T), POINTER(Pos_T), POINTER(CoordTrans_T)] self.__BaseCalib_3p__.restype = c_int32 # Get DLL version mj = c_int32(0) mn = c_int32(0) st = c_int32(0) bd = c_int32(0) self.version_ = self.getLibVersion(mj, mn, st, bd) print("Dynamics library version =", self.version_, "(", mj.value, ",", mn.value, ",", st.value, ",", bd.value, ")")