imitate

README.md

@@ -3,4 +3,7 @@
 ### Окружение
 * Запустить сокет сервер - для теста
 * Запустить pybullet сервер - визуализация в отдельном ПРОЦЕССЕ
-* Запустить gui
+* Запустить gui
+
+### Локальный запуск
+`poetry run python run_local.py --test-socket --bullet-gui`

data/test.nc.result

@@ -1,9 +1,19 @@
-X0 Y0
+X0 Y0 Z0 U0
-X0 Y10
+X0 Y0 Z0 U-33.333
-X0 Y20
+X100 Y0 Z0 U-33.333
-X10 Y20
+X200 Y0 Z0 U-33.333
-X20 Y20
+X200 Y100 Z0 U-33.333
-X20 Y10
+X200 Y200 Z0 U-33.333
-X20 Y0
+X100 Y200 Z0 U-33.333
-X10 Y0
+X0 Y200 Z0 U-33.333
-X0 Z0
+X0 Y100 Z0 U-33.333
+X0 Y0 Z0 U-33.333
+X0 Y0 Z-100 W-33.333
+X100 Y0 Z-100 W-33.333
+X200 Y0 Z-100 W-33.333
+X200 Y100 Z-100 W-33.333
+X200 Y200 Z-100 W-33.333
+X100 Y200 Z-100 W-33.333
+X0 Y200 Z-100 W-33.333
+X0 Y100 Z-100 W-33.333
+X0 Y0 Z-100 W-33.333

robot/client_socket.py

@@ -2,6 +2,7 @@ import socket
 import json
 import os
 import math
+import numpy as np
 
 from typing import Literal
 from pprint import pprint
@@ -75,7 +76,14 @@ class SocketRobotArm:
         urdf_path = os.path.join("urdf", f"{self.urdf_filename}.urdf")
         logger.info(urdf_path)
         self.body_id = p.loadURDF(
-            urdf_path, [0, 0, 0], useFixedBase=1, physicsClientId=self.physics_client
+            urdf_path,
+            [
+                -0.075458,  # новое значение x в метрах
+                0.068477,  # новое значение y в метрах
+                0.082201,  # новое значение z в метрах
+            ],
+            useFixedBase=1,
+            physicsClientId=self.physics_client,
         )
         time.sleep(1)
 
@@ -106,7 +114,7 @@ class SocketRobotArm:
             projectionMatrix=p.computeProjectionMatrixFOV(
                 fov=60.0, aspect=1.0, nearVal=0.1, farVal=10.0
             ),
-            renderer=p.ER_TINY_RENDERER,
+            # renderer=p.ER_TINY_RENDERER,
             physicsClientId=self.physics_client,
         )
 
@@ -142,12 +150,34 @@ class SocketRobotArm:
         self.get_world()
         self.set_text(text=f"Мировые координаты {self.start_world_coordinates}")
         time.sleep(0.5)
-        # self.set_joint(self.convert_to_joint(self.start_world_coordinates[0:3]))
+        self.set_joint(
+            self.convert_to_joint(
+                [s * 0.001 for s in self.start_world_coordinates[0:3]]
+            )
+        )
 
         self._get_command_count()
         self.set_text(text=f"Команд в очереди {self.remote_command_count}")
         time.sleep(0.5)
 
+        num_joints = p.getNumJoints(self.body_id)
+        end_effector_state = p.getLinkState(
+            self.body_id, num_joints - 1, computeForwardKinematics=True
+        )
+
+        # Позиция end-effector
+        end_effector_position = end_effector_state[4]
+        # Ориентация end-effector (кватернион)
+        end_effector_orientation = end_effector_state[5]
+
+        # Преобразуем кватернион в углы Эйлера
+        euler_orientation = p.getEulerFromQuaternion(end_effector_orientation)
+
+        euler_orientation = np.degrees(euler_orientation)
+
+        logger.info(end_effector_position)
+        logger.info(euler_orientation)
+
     def cycle_base(self):
         self.upd_model()
 
@@ -192,14 +222,25 @@ class SocketRobotArm:
         for i, point in enumerate(points):
             if point["action"] == "10":
                 self.imitate_point = i
+                points = [
+                    point["m0"],
+                    point["m1"],
+                    point["m2"],
+                ]
+                coor = [(float(point) * 0.001) for point in points]
                 angles = self.convert_to_joint(
-                    [float(point) for point in [point["m0"], point["m1"], point["m2"]]]
+                    coor,
+                    [
+                        point["m3"],
+                        point["m4"],
+                        point["m5"],
+                    ],
                 )
                 # logger.info(f"point {point} {angles}")
-                self.set_joint((*angles, point["m2"], point["m3"], point["m4"]))
+                self.set_joint(angles)
                 time.sleep(1)
 
-    def convert_to_joint(self, coordinates):
+    def convert_to_joint(self, coordinates, angles=[]):
         num_joints = p.getNumJoints(self.body_id)
         joint_info = [p.getJointInfo(self.body_id, i) for i in range(num_joints)]
 
@@ -207,6 +248,9 @@ class SocketRobotArm:
             self.body_id,
             endEffectorLinkIndex=num_joints - 1,
             targetPosition=coordinates,
+            targetOrientation=p.getQuaternionFromEuler(
+                [math.radians(float(a)) for a in angles]
+            ) if len(angles) == 3  else None,
         )
         logger.info(f"convrt to joint {joint_angles}")
         # logger.info(joint_angles)
@@ -302,6 +346,9 @@ class SocketRobotArm:
     def round(v):
         return round(v, 3)
 
+    def relative_axis_coordinates(self):
+        start = self.start_axis_coordinates
+
     def make_world_step(self, type, point):
         step = {
             "oneshot": "1",

run_local.py

@@ -33,7 +33,8 @@ signal.signal(signal.SIGTERM, lambda *_: cleanup())
 
 # Парсер аргументов командной строки
 parser = argparse.ArgumentParser(description="Python launcher script.")
-parser.add_argument("--test", action="store_true", help="Запуск в режиме тестирования")
+parser.add_argument("--test-socket", action="store_true", help="Запуск в режиме тестирования")
+parser.add_argument("--bullet-gui", action="store_true", help="Вывод визуального pybullet")
 args = parser.parse_args()
 
 # Установка команды для pybullet_server
@@ -46,7 +47,7 @@ pybullet_command = [
 ]  # Путь к исполняемому файлу
 
 # Если включен режим тестирования, добавляем флаг --test
-if args.test:
+if args.bullet_gui:
     pybullet_command.append("--test")
 
 # Запуск pybullet_server.exe
@@ -59,7 +60,7 @@ if server_process.poll() is not None:
     sys.exit(1)
 
 # Если включен режим тестирования, запускаем test_socket_server.exe
-if args.test:
+if args.test_socket:
     print("Запуск test_socket_server...")
     test_server_process = subprocess.Popen(
         ["poetry", "run", "python", "utils/test_socket_server.py"]

urdf/sample.urdf

@@ -160,7 +160,7 @@
     <limit lower="-2.007" upper="2.007" effort="0" velocity="0"/>
     <parent link="link_4" />
     <child link="link_5" />
-    <axis xyz="0 1 0" />
+    <axis xyz="0 -1 0" />
     <origin rpy="0 0 0" xyz="0.867755 0.037912 0.000177" />
   </joint>
 

utils/test_socket_server.py

@@ -17,28 +17,22 @@ def handle_client(conn, addr):
 
             if "queryAddr" in req.keys() and "axis-0" in req["queryAddr"]:
                 res["queryData"] = [
-                    -60,
+                    -8.487,
-                    40,
+                    -8.681,
-                    -10,
+                    33.058,
-                    -60,
+                    88.070,
-                    -75,
+                    -75.010,
-                    0,
+                    -10.566,
                 ]
 
             if "queryAddr" in req and "world-0" in req["queryAddr"]:
                 res["queryData"] = [
-                    643.622,
+                    1282.244,
-                    -1289.604,
+                    -75.427,
-                    254.682,
+                    1772.476,
-                    124.70,
+                    84.629,
-                    24.209,
+                    34.519,
-                    -58.492,
+                    153.999,
-                    # 0,
-                    # 0,
-                    # 0,
-                    # 0,
-                    # 0,
-                    # 0,
                 ]
 
             if req["reqType"] == "command":