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, ")")