first

Author: KaiTang98

.gitignore

@@ -0,0 +1,169 @@
+# image and txt file ignore
+.jpg
+.bmp
+.png
+.jpeg
+.txt
+.csv
+.npy
+
+# Byte-compiled / optimized / DLL files
+__pycache__/
+*.py[cod]
+*$py.class
+
+# C extensions
+*.so
+
+# Distribution / packaging
+.Python
+build/
+develop-eggs/
+dist/
+downloads/
+eggs/
+.eggs/
+lib/
+lib64/
+parts/
+sdist/
+var/
+wheels/
+share/python-wheels/
+*.egg-info/
+.installed.cfg
+*.egg
+MANIFEST
+
+# PyInstaller
+#  Usually these files are written by a python script from a template
+#  before PyInstaller builds the exe, so as to inject date/other infos into it.
+*.manifest
+*.spec
+
+# Installer logs
+pip-log.txt
+pip-delete-this-directory.txt
+
+# Unit test / coverage reports
+htmlcov/
+.tox/
+.nox/
+.coverage
+.coverage.*
+.cache
+nosetests.xml
+coverage.xml
+*.cover
+*.py,cover
+.hypothesis/
+.pytest_cache/
+cover/
+
+# Translations
+*.mo
+*.pot
+
+# Django stuff:
+*.log
+local_settings.py
+db.sqlite3
+db.sqlite3-journal
+
+# Flask stuff:
+instance/
+.webassets-cache
+
+# Scrapy stuff:
+.scrapy
+
+# Sphinx documentation
+docs/_build/
+
+# PyBuilder
+.pybuilder/
+target/
+
+# Jupyter Notebook
+.ipynb_checkpoints
+
+# IPython
+profile_default/
+ipython_config.py
+
+# pyenv
+#   For a library or package, you might want to ignore these files since the code is
+#   intended to run in multiple environments; otherwise, check them in:
+# .python-version
+
+# pipenv
+#   According to pypa/pipenv#598, it is recommended to include Pipfile.lock in version control.
+#   However, in case of collaboration, if having platform-specific dependencies or dependencies
+#   having no cross-platform support, pipenv may install dependencies that don't work, or not
+#   install all needed dependencies.
+#Pipfile.lock
+
+# poetry
+#   Similar to Pipfile.lock, it is generally recommended to include poetry.lock in version control.
+#   This is especially recommended for binary packages to ensure reproducibility, and is more
+#   commonly ignored for libraries.
+#   https://python-poetry.org/docs/basic-usage/#commit-your-poetrylock-file-to-version-control
+#poetry.lock
+
+# pdm
+#   Similar to Pipfile.lock, it is generally recommended to include pdm.lock in version control.
+#pdm.lock
+#   pdm stores project-wide configurations in .pdm.toml, but it is recommended to not include it
+#   in version control.
+#   https://pdm.fming.dev/#use-with-ide
+.pdm.toml
+
+# PEP 582; used by e.g. github.com/David-OConnor/pyflow and github.com/pdm-project/pdm
+__pypackages__/
+
+# Celery stuff
+celerybeat-schedule
+celerybeat.pid
+
+# SageMath parsed files
+*.sage.py
+
+# Environments
+.env
+.venv
+env/
+venv/
+ENV/
+env.bak/
+venv.bak/
+
+# Spyder project settings
+.spyderproject
+.spyproject
+
+# Rope project settings
+.ropeproject
+
+# mkdocs documentation
+/site
+
+# mypy
+.mypy_cache/
+.dmypy.json
+dmypy.json
+
+# Pyre type checker
+.pyre/
+
+# pytype static type analyzer
+.pytype/
+
+# Cython debug symbols
+cython_debug/
+
+# PyCharm
+#  JetBrains specific template is maintained in a separate JetBrains.gitignore that can
+#  be found at https://github.com/github/gitignore/blob/main/Global/JetBrains.gitignore
+#  and can be added to the global gitignore or merged into this file.  For a more nuclear
+#  option (not recommended) you can uncomment the following to ignore the entire idea folder.
+#.idea/

BatchMake.py

@@ -0,0 +1,74 @@
+# -*- coding: utf-8 -*-
+import codecs
+import csv
+import os
+
+import numpy as np
+
+if __name__ == "__main__":
+
+    # os.chdir("Batch_Make")
+
+    time1 = 10
+    time2 = 2
+
+    file_name = "angle_100"
+    file_name2 = "angle_100"
+
+    # str_des_angle = '-36.88 24.47 50.87 -79.25 54.49 20.4'
+    # str_des_angle = "38.55 25.87 47.36 77.0 53.21 -13.85"
+
+    # 保存してある関節角を読み込む
+    robot_name = "B"
+    cur_angle_deg = np.loadtxt(
+        "./data/" + robot_name + "/current_angle.csv"
+    ).astype(np.float32)
+    cur_angle_deg_str = cur_angle_deg.astype(str)
+    str_des_angle = " ".join(cur_angle_deg_str)
+
+    # -----------------x_trainの取得-----------------
+    angle = np.empty((0, 6), dtype=np.float32)
+    with codecs.open(
+        "data/"  + robot_name + "/"+ file_name + ".csv", "r", "utf-8-sig"
+    ) as f:  # ここが間違ってないか要確認
+        angle_reader = csv.reader(f, delimiter=",")
+        for row in angle_reader:
+            row = np.expand_dims(row, axis=0)
+            angle = np.append(angle, row, axis=0)
+        f.close()
+
+    angle = angle.astype("float32")
+
+    os.chdir("../../../Batch/Kai/Calibration")
+
+    f = open("Calib_" + file_name2 + ".txt", "w")
+
+    f.write(",Get_Calib_Img,LABEL,,.exe\n")
+    f.write(",ServoON,ServoON,0,.exe\n")
+    f.write(",CameraON,StartCamera_Calib,NOWAIT,.exe\n")
+
+    # 初期位置から目標位置に移動する
+    f.write("●,StartPos,FifthPolynomialRT,")
+    f.write(str_des_angle + " ")  # 目標位置を変更したらここを変更すること
+    f.write(str(time1) + ",.exe\n")
+
+    for l in range(0, np.size(angle, axis=0)):
+
+        # 手先位置を微小移動させる
+        f.write(",Move,FifthPolynomialRT,")
+
+        for k in range(0, 6):
+            f.write("{0:f}".format(angle[l][k]) + " ")
+
+        f.write(str(time2) + ",.exe\n")
+        f.write(",SaveImg,SaveImg_Calib,0,.exe\n")
+
+    f.write("●,Back2StartPos,FifthPolynomialRT,")
+    f.write(str_des_angle + " ")  # 目標位置を変更したらここを変更すること
+    f.write(str(time2) + ",.exe\n")
+
+    f.write(",ServoOFF,ServoOFF,,.exe\n")
+    f.write(",CameraOFF,EndCamera_Calib,NOWAIT,.exe")
+    f.close()
+
+    print("OK\n")

CalcBetweenRobot.py

@@ -0,0 +1,77 @@
+# -*- coding: utf-8 -*-
+
+import cv2
+import numpy as np
+import scipy.linalg
+
+
+class CalcBetweenRobot:
+    def __init__(self):
+
+        self.T_cam2baseA = np.load("data/A/T_cam2base.npy")
+        self.T_baseB2cam = np.load("data/B/T_base2cam.npy")
+
+    def get_T_B2A(self):
+
+        T_B2A = np.zeros((4, 4))
+
+        T_B2A = np.dot(self.T_cam2baseA, self.T_baseB2cam)
+
+        # T_B2A[0][2] = 0.0
+        # T_B2A[1][2] = 0.0
+
+        # T_B2A[2][0] = 0.0
+        # T_B2A[2][1] = 0.0
+        # T_B2A[2][2] = 1.0
+        # T_B2A[2][3] = 0.0
+
+        return T_B2A
+
+    def get_T_A2World(self, T_B2A):
+
+        T_World2A = np.zeros((4, 4))
+        R = np.zeros((3, 3))
+
+        R = scipy.linalg.sqrtm(T_B2A[0:3, 0:3])
+
+        T_World2A[0:3, 0:3] = R.real
+
+        # T_World2A[2][2] = 1.0
+
+        T_World2A[0][3] = T_B2A[0][3] / 2
+        T_World2A[1][3] = T_B2A[1][3] / 2
+        T_World2A[2][3] = T_B2A[2][3] / 2
+        T_World2A[3][3] = 1.0
+
+        return np.linalg.inv(T_World2A)
+
+    def get_T_B2World(self, T_B2A, T_A2World):
+
+        T_B2World = np.zeros((4, 4))
+
+        T_B2World = np.dot(T_A2World, T_B2A)
+
+        return T_B2World
+
+    def save_T(self, T_B2World, T_A2World):
+
+        np.save("data/T_baseB2world", T_B2World.real)
+        np.save("data/T_baseA2world", T_A2World.real)
+
+
+def main():
+    calc = CalcBetweenRobot()
+
+    T_B2A = calc.get_T_B2A()
+    T_A2World = calc.get_T_A2World(T_B2A)
+    T_B2World = calc.get_T_B2World(T_B2A, T_A2World)
+    calc.save_T(T_B2World, T_A2World)
+
+    print(T_B2A)
+    print(np.dot(np.linalg.inv(T_A2World), T_B2World))
+
+    print("fin!")
+
+
+if __name__ == "__main__":
+    main()