def __Act(api, lastIndex):
"""Function to execute command"""
# キューに入っているコマンドを実行
dType.SetQueuedCmdStartExec(api)
# Wait for Executing Last Command
while lastIndex > dType.GetQueuedCmdCurrentIndex(api)[0]:
dType.dSleep(100)
# キューに入っているコマンドを停止
dType.SetQueuedCmdStopExec(api)
def Connect_Disconnect(connection_flag: bool, api, CON_STR: tuple=CON_STR):
"""Dobotを接続状態を制御する関数
Args:
connection_flag(bool): 現在Dobotが接続されているか。
api(dType): Dobot API
CON_STR(tuple): 接続時のエラーテーブル
Returns:
rec(bool): 接続結果
True: 接続した
False: 接続していない
Dobot_Err(int): 処理結果
"""
rec = False
err = 0
# Dobotがすでに接続されていた場合
if connection_flag:
dType.DisconnectDobot(api) # DobotDisconnect
print("Dobotとの接続を解除しました．")
rec, err = False, 1
# Dobotが接続されていない場合
else:
portName = dType.SearchDobot(api, maxLen=128)
try:
if ("COM3" in portName) or ("COM4" in portName):
char_size = 115200
state = dType.ConnectDobot(api, portName[0], char_size)
# 接続時にエラーが発生しなかった場合
if CON_STR[state[0]] == "DobotConnect_NoError":
print("Dobotに通信速度 {} で接続されました．".format(char_size))
initDobot(api) # Dobotの初期設定を行う
rec , err = True, state[0]
elif CON_STR[state[0]] == "DobotConnect_NotFound":
print("Dobot を見つけることができません！")
rec, err = False, state[0]
elif CON_STR[state[0]] == "DobotConnect_Occupied":
print(
"Dobot が占有されています。接続するには、アプリケーションを再起動する、Dobot 背面の Reset ボタンを押す、接続USBケーブルを再接続する、のどれかを行う必要があります。")
rec, err = False, state[0]
else:
dType.DisconnectDobot(api)
raise Exception("接続時に予期せぬエラーが発生しました！！")
else:
raise Exception("Dobotがハードウェア上で接続されていない可能性があります。接続されている場合は、portNameに格納されている変数を確認してください。")
except Exception as e:
print(e)
finally:
return rec, err
def __init__(self):
#dll_path = cfg.DOBOT_DLL_DIR + os.sep + "DobotDll.dll"
#self.api = cdll.LoadLibrary(dll_path)
self.api = dType.load()
self.CON_STR = {
dType.DobotConnect.DobotConnect_NoError: "DobotConnect_NoError",
dType.DobotConnect.DobotConnect_NotFound: "DobotConnect_NotFound",
dType.DobotConnect.DobotConnect_Occupied: "DobotConnect_Occupied",
}
self.pose_key = [
"x",
"y",
"z",
"r",
"joint1Angle",
"joint2Angle",
"joint3Angle",
"joint4Angle",
]
self.current_pose = {} # Dobotの現在の姿勢
self.cam = None
self.cam_num = None
# --- エラーフラグ --- #
self.connection = 1 # Connect: 0, DisConnect: 1, Err: -1
self.act_err = 0 # State: 0, Err: -1
# --- 画像プレビュー画面の初期値 --- #
self.fig_agg = None # 画像のヒストグラムを表示する用の変数
self.Image_height = 240 # 画面上に表示する画像の高さ
self.Image_width = 320 # 画面上に表示する画像の幅
# --- GUIの初期化 ---#
self.layout = self.Layout()
self.Window = self.main()
def GripperAutoCtrl(api) -> None:
"""DobotAPIの出力に基づいてグリッパを自動的に開閉制御する関数
Args:
api (dType): DobotAPIのコンストラクタ
Return:
None
"""
sleep_time = 2 #[second]
[value] = dType.GetEndEffectorGripper(api)
# --------------------------- #
# グリッパが閉じている場合 #
# --------------------------- #
if value:
# モータを起動する
_GripperOpenClose(api, motorCtrl=True, gripperCtrl=True)
# グリッパを開く
_GripperOpenClose(api, motorCtrl=True, gripperCtrl=False)
time.sleep(sleep_time)
# モータを停止する
_GripperOpenClose(api, motorCtrl=False, gripperCtrl=False)
# --------------------------- #
# グリッパが開いている場合 #
# --------------------------- #
else:
# モータを起動する
_GripperOpenClose(api, motorCtrl=True, gripperCtrl=False)
# グリッパを閉じる
_GripperOpenClose(api, motorCtrl=True, gripperCtrl=True)
time.sleep(sleep_time)
# モータを停止する
_GripperOpenClose(api, motorCtrl=False, gripperCtrl=True)
def GetPose_UpdateWindow(self) -> dict:
"""
姿勢を取得し、ウインドウを更新する関数
Return:
CurrentPose (dict): Dobotの現在の姿勢
"""
pose = dType.GetPose(self.api) # 現在のDobotの位置と関節角度を取得
for num, key in enumerate(self.CurrentPose.keys()):
self.CurrentPose[key] = pose[num]
self.Window["-Get_JointPose1-"].update(
str(self.CurrentPose["joint1Angle"]))
self.Window["-Get_JointPose2-"].update(
str(self.CurrentPose["joint2Angle"]))
self.Window["-Get_JointPose3-"].update(
str(self.CurrentPose["joint3Angle"]))
self.Window["-Get_JointPose4-"].update(
str(self.CurrentPose["joint4Angle"]))
self.Window["-Get_CoordinatePose_X-"].update(str(
self.CurrentPose["x"]))
self.Window["-Get_CoordinatePose_Y-"].update(str(
self.CurrentPose["y"]))
self.Window["-Get_CoordinatePose_Z-"].update(str(
self.CurrentPose["z"]))
self.Window["-Get_CoordinatePose_R-"].update(str(
self.CurrentPose["r"]))
return self.CurrentPose
def GetPose_UpdateWindow(self):
"""
姿勢を取得し、ウインドウを更新する関数
Parameters
----------
api : Dobot型
DobotAPIのコンストラクタ
Window
PySimpleGUIのウインドウ画面
"""
pose = dType.GetPose(self.api) # 現在のDobotの位置と関節角度を取得
pose = dict(zip(self.pose_key, pose)) # 辞書形式に変換
self.Window["-JointPose1-"].update(str(pose["joint1Angle"]))
self.Window["-JointPose2-"].update(str(pose["joint2Angle"]))
self.Window["-JointPose3-"].update(str(pose["joint3Angle"]))
self.Window["-JointPose4-"].update(str(pose["joint4Angle"]))
self.Window["-CoordinatePose_X-"].update(str(pose["x"]))
self.Window["-CoordinatePose_Y-"].update(str(pose["y"]))
self.Window["-CoordinatePose_Z-"].update(str(pose["z"]))
self.Window["-CoordinatePose_R-"].update(str(pose["r"]))
return pose
def _OneAction(api, pose, mode=dType.PTPMode.PTPMOVLXYZMode):
"""One step operation"""
# if pose["x"] == None or y is None or z is None or r is None:
current_pose = dType.GetPose(api)
if pose["x"] is None:
pose["x"] = current_pose[0]
if pose["y"] is None:
pose["y"] = current_pose[1]
if pose["z"] is None:
pose["z"] = current_pose[2]
if pose["r"] is None:
pose["r"] = current_pose[3]
try:
lastIndex = dType.SetPTPCmd(
api, mode, pose["x"], pose["y"], pose["z"], pose["r"], isQueued=1
)[0]
__Act(api, lastIndex)
except TimeoutError:
return -1
else:
return 0
def SetCoordinatePose_click(self, pose: dict, queue_index: int = 1):
"""デカルト座標系で指定された位置にアームの先端を移動させる関数
Arg:
pose(dict): デカルト座標系および関節座標系で指定された姿勢データ
Return:
response(int):
0 : 応答あり
1 : 応答なし
"""
dType.SetPTPCmd(self.api, dType.PTPMode.PTPMOVJXYZMode, pose["x"],
pose["y"], pose["z"], pose["r"], queue_index)
time.sleep(5)
return 0
def Operation(api, file_name, axis, volume=1, initPOS=None):
"""
A function that sends a motion command in any direction
Parameters
----------
api : CDLL
axis : str
移動方向
volume : int
移動量
"""
axis_list = ["x", "y", "z", "r"]
if initPOS != None:
pose = initPOS
else:
pose = dType.GetPose(api)
if axis in axis_list:
if axis == "x":
_OneAction(api, pose[0] + volume, pose[1], pose[2], pose[3])
elif axis == "y":
_OneAction(api, pose[0], pose[1] + volume, pose[2], pose[3])
elif axis == "z":
_OneAction(api, pose[0], pose[1], pose[2] + volume, pose[3])
else:
print("rは実装されていません。")
else:
print("移動軸に問題があります！")
# 座標をファイルへ書き込む
csv_write(file_name, dType.GetPose(api))
# 1回動作指令を出す関数
# def _OneAction(api, x=None, y=None, z=None, r=None, mode=dType.PTPMode.PTPMOVLXYZMode):
"""One step operation"""
def __init__(self):
#dll_path = cfg.DOBOT_DLL_DIR + os.sep + "DobotDll.dll"
#self.api = cdll.LoadLibrary(dll_path)
self.api = dType.load(cfg.DOBOT_DLL_DIR)
#self.api = dType.load()
self.connection = False #Dobotの接続状態
self.CurrentPose = {
"x": 0.0,
"y": 0.0,
"z": 0.0,
"r": 0.0,
"joint1Angle": 0.0,
"joint2Angle": 0.0,
"joint3Angle": 0.0,
"joint4Angle": 0.0,
}
self.RecordPose = {} # Dobotがオブジェクトを退避させる位置
self.Alignment_1 = {}
self.Alignment_2 = {}
self.cam = None
self.cam_num = None
self.IMAGE_Org = None # スナップショットのオリジナル画像(RGB)
self.IMAGE_bin = None # 二値画像
# --- エンドエフェクタ --- #
self.MotorON = False # 吸引用モータを制御
# --- エラーフラグ --- #
self.Dobot_err = {
0: "DobotAct_NoError",
1: "DobotConnect_NotFound",
2: "DobotConnect_Occupied",
3: "DobotAct_Timeout",
}
self.act_err = 0 # State: 0, Err: -1
self.WebCam_err = {
0: "WebCam_Connect",
1: "WebCam_Release",
2: "WebCam_NotFound",
3: "WebCam_GetImage",
4: "WebCam_NotGetImage"
}
# --- 画像プレビュー画面の初期値 --- #
self.fig_agg = None # 画像のヒストグラムを表示する用の変数
self.Image_height = 240 # 画面上に表示する画像の高さ
self.Image_width = 320 # 画面上に表示する画像の幅
# --- GUIの初期化 ---#
self.layout = self.Layout()
self.Window = self.main()
def GetPose_UpdateWindow(self) -> dict:
"""
姿勢を取得し、ウインドウを更新する関数
Return:
pose (dict):
Dobotの現在の姿勢
"""
pose = dType.GetPose(self.api) # 現在のDobotの位置と関節角度を取得
pose = dict(zip(self.pose_key, pose)) # 辞書形式に変換
self.Window["-JointPose1-"].update(str(pose["joint1Angle"]))
self.Window["-JointPose2-"].update(str(pose["joint2Angle"]))
self.Window["-JointPose3-"].update(str(pose["joint3Angle"]))
self.Window["-JointPose4-"].update(str(pose["joint4Angle"]))
self.Window["-CoordinatePose_X-"].update(str(pose["x"]))
self.Window["-CoordinatePose_Y-"].update(str(pose["y"]))
self.Window["-CoordinatePose_Z-"].update(str(pose["z"]))
self.Window["-CoordinatePose_R-"].update(str(pose["r"]))
return pose
def _GripperOpenClose(
api,
motorCtrl: bool=False,
gripperCtrl: bool=True,
queue_index: int=0
) -> None:
"""グリッパーを開閉する関数
Args:
api(dType): DobotAPIのコンストラクタ
motorCtrl(bool optional): 吸引モータを起動する。default to True
* True: On
* False: Off
gripperCtrl(bool optional): グリッパを開閉する。default to True
True: Close
False: Open
queue_index(int optional): データをQueueとして送るか。default to 0
* 0: 送らない
* 1: 送る
Return: None
"""
dType.SetEndEffectorGripper(api, motorCtrl, gripperCtrl, queue_index)
def SetPoseAct(api, pose: dict, ptpMoveMode: str, queue_index: int=1):
"""デカルト座標系で指定された位置にアームの先端を移動させる関数
Arg:
api(dtype): DobotAPIのコンストラクタ
pose(dict): デカルト座標系および関節座標系で指定された姿勢データ
ptpMoveMode(str): 各座標系におけるDobotの制御方法
queue_index(int): データをQueueとして送るか。default to 0
* 0: 送らない
* 1: 送る
Return:
response(int):
0 : 応答あり
1 : 応答なし
"""
response = ''
try:
for ptpmode in ptpMoveModeDict:
if ptpMoveMode in ptpmode:
response = ptpMoveModeDict[ptpMoveMode]
break
if (not response) and (response != 0):
raise ValueError("指定された制御方法は存在しません。")
except (ValueError, TypeError) as e:
traceback.print_exc()
else:
dType.SetPTPCmd(api,
ptpMoveModeDict[ptpMoveMode],
pose["x"],
pose["y"],
pose["z"],
pose["r"],
queue_index
)
time.sleep(2)
return 0
def __init__(self):
dll_path = cfg.DOBOT_DLL_DIR + os.sep + "DobotDll.dll"
#self.api = cdll.LoadLibrary(dll_path)
self.api = dType.load(dll_path)
self.pose_key = [
"x",
"y",
"z",
"r",
"joint1Angle",
"joint2Angle",
"joint3Angle",
"joint4Angle",
]
self.connection = False #Dobotの接続状態
self.current_pose = {} # Dobotの現在の姿勢
self.cam = None
self.cam_num = None
self.IMAGE_Org = None # スナップショットのオリジナル画像(RGB)
self.IMAGE_bin = None # 二値画像
# --- エンドエフェクタ --- #
self.MotorON = False # 吸引用モータを制御
# --- エラーフラグ --- #
self.Dobot_err = {
0: "DobotAct_NoError",
1: "DobotConnect_NotFound",
2: "DobotConnect_Occupied",
3: "DobotAct_Timeout",
}
self.act_err = 0 # State: 0, Err: -1
# --- 画像プレビュー画面の初期値 --- #
self.fig_agg = None # 画像のヒストグラムを表示する用の変数
self.Image_height = 240 # 画面上に表示する画像の高さ
self.Image_width = 320 # 画面上に表示する画像の幅
# --- GUIの初期化 ---#
self.layout = self.Layout()
self.Window = self.main()
def _wirte(f, data):
"""write content"""
error = 1 # エラーチェック用変数
witer = csv.writer(f, lineterminator="\n")
error = witer.writerows([data])
return error # エラーが無ければNoneを返す
if __name__ == "__main__":
from DobotDLL import DobotDllType as dType
from src.config.config import cfg
dll_path = cfg.DOBOT_DLL_DIR + os.sep + "DobotDll.dll"
api = dType.load(dll_path)
CON_STR = {
dType.DobotConnect.DobotConnect_NoError: "DobotConnect_NoError",
dType.DobotConnect.DobotConnect_NotFound: "DobotConnect_NotFound",
dType.DobotConnect.DobotConnect_Occupied: "DobotConnect_Occupied",
}
# ---------------------------- #
# Dobot の接続ポートの確認 #
# ---------------------------- #
#portName = dType.SearchDobot(api, maxLen=128)
#print(portName)
# ------------------- #
# Dobot のコネクト #
# ------------------- #
figure_canvas_agg.draw()
figure_canvas_agg.get_tk_widget().pack(side='top', fill='both', expand=1)
return figure_canvas_agg
def delete_figure_agg(figure_agg):
figure_agg.get_tk_widget().forget()
plt.close('all')
if __name__ == "__main__":
debug = True
if debug:
dll_path = cfg.DOBOT_DLL_DIR + os.sep + "DobotDll.dll"
api = dType.load(dll_path)
connection_flag = False
connection_flag, result = Connect_Disconnect(connection_flag, api)
if connection_flag:
pose = {
"x": 193,
"y": -20,
"z": 21,
"r": 46,
"joint1Angle": 0.0,
"joint2Angle": 0.0,
"joint3Angle": 0.0,
"joint4Angle": 0.0,
}
def initDobot(api):
dType.SetCmdTimeout(api, 3000) # TimeOut Setup
dType.SetQueuedCmdClear(api) # Clean Command Queued
dSN = dType.GetDeviceSN(api) # デバイスのシリアルナンバーを取得する
print(dSN)
dName = dType.GetDeviceName(api) # デバイス名を取得する
print(dName)
# majorV, minorV, revision = dType.GetDeviceVersion(api) # デバイスのバージョンを取得する
# print(majorV, minorV, revision)
# Home Params の設定
dType.SetHOMEParams(
api, 150, -200, 100, 0, isQueued=1
) # Async Motion Params Setting
dType.SetHOMECmd(api, temp=0, isQueued=1) # Async Home
time.sleep(20) # 初期位置への移動待ち
# JOGパラメータの設定
dType.SetJOGJointParams(
api, 200, 200, 200, 200, 200, 200, 200, 200, isQueued=1
) # 関節座標系での各モータの速度および加速度の設定
dType.SetJOGCoordinateParams(
api, 200, 200, 200, 200, 200, 200, 200, 200, isQueued=1
) # デカルト座標系での各方向への速度および加速度の設定
# JOG動作の速度、加速度の比率を設定
dType.SetJOGCommonParams(api, 100, 100, isQueued=1)
# ----------------------- #
# PTPパラメータの設定 #
# ----------------------- #
# 関節座標系の各モータの速度および加速度を設定
dType.SetPTPJointParams(
api=api,
j1Velocity=ptpJointParams["j1Velocity"],
j1Acceleration=ptpJointParams["j1Acceleration"],
j2Velocity=ptpJointParams["j2Velocity"],
j2Acceleration=ptpJointParams["j2Acceleration"],
j3Velocity=ptpJointParams["j3Velocity"],
j3Acceleration=ptpJointParams["j3Acceleration"],
j4Velocity=ptpJointParams["j4Velocity"],
j4Acceleration=ptpJointParams["j4Acceleration"],
isQueued=1
)
params = dType.GetPTPJointParams(api)
s = '''\
起動直後のMagicianの関節座標系における各モータの速度および加速度
j1Velocity = {}
j1Acceleration = {}
j2Velocity = {}
j2Acceleration = {}
j3Velocity = {}
j3Acceleration = {}
j4Velocity = {}
j4Acceleration = {}
'''
print(s.format(params[0],
params[1],
params[2],
params[3],
params[4],
params[5],
params[6],
params[7]))
# デカルト座標系での各方向への速度および加速度の設定
dType.SetPTPCoordinateParams(
api=api,
xyzVelocity=ptpCoordinateParams["xyzVelocity"],
xyzAcceleration=ptpCoordinateParams["xyzAcceleration"],
rVelocity=ptpCoordinateParams["rVelocity"],
rAcceleration=ptpCoordinateParams["rAcceleration"],
isQueued=1
)
params = dType.GetPTPCoordinateParams(api)
s = '''\
起動直後のMagicianのデカルト座標系における各モータの速度および加速度
xyzVelocity = {}
xyzAcceleration = {}
rVelocity = {}
rAcceleration = {}
'''
print(s.format(params[0],
params[1],
params[2],
params[3]))
# PTP動作の速度、加速度の比率を設定
dType.SetPTPCommonParams(api, 100, 100, isQueued=1)