norm intervals

data/test.nc.result

@@ -1,3 +1,9 @@
 X0 Y0
+X0 Y50
+X0 Y100
+X0 Y150
 X0 Y200
+X0 Y150
+X0 Y100
+X0 Y50
 X0 Y0

gui/imitator.py

@@ -39,5 +39,4 @@ class Imitator(QWidget):
         self.setPalette(p)
         
     def updateState(self):
-        logger.info(f'lalaala {self.imitate_point()}')
         self.stepLabel.setText(f"Отправлено шагов {self.imitate_point()}")

gui/visualize.py

@@ -25,12 +25,13 @@ class Visualize(QWidget):
         # Таймер для обновления изображения
         self.timer = QTimer()
         self.timer.timeout.connect(self.update_image)
-        self.timer.start(int(500))  # Обновление каждые 100 мс
+        self.timer.start(30) 
 
     def update_image(self):
         res = self.get_pybullet_image()
 
-        if not res: return
+        if not res:
+            return
 
         (rgb, width, height) = res
         
@@ -40,8 +41,7 @@ class Visualize(QWidget):
 
         self.label.setPixmap(pixmap)
         self.label.setAlignment(Qt.AlignmentFlag.AlignHCenter)
-        self.label.resize(image.size()) 
+        self.label.update()
-        self.label.repaint()
 
     def paintEvent(self, event):
         p = self.palette()

robot/client_socket.py

@@ -65,11 +65,6 @@ class SocketRobotArm:
     def run_pybullet(self, type="DIRECT"):
         self.physics_client = p.connect(getattr(p, type))
         logger.info(f"Connect to {self.physics_client} by {type}")
-        p.setGravity(0, 0, -9.81, physicsClientId=self.physics_client)
-        p.setAdditionalSearchPath(pybullet_data.getDataPath())
-        p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 0)
-        
-        self.simulation_loop()
 
     def load_models(self):
         p.loadURDF("plane.urdf", physicsClientId=self.physics_client)
@@ -92,36 +87,13 @@ class SocketRobotArm:
             f"Размеры модели (длина x, длина y, длина z): ({size_x}, {size_y}, {size_z})"
         )
 
-    def simulation_loop(self):
-        logger.info(f"Начат цикл симуляции")
-        while True:
-            # bodyUniqueId = self.body_id
-            # position, orientation = p.getBasePositionAndOrientation(bodyUniqueId)
-            # # logger.info(f"Position: {position}, Orientation: {orientation}")
-
-            # # Получение состояния суставов
-            # num_joints = p.getNumJoints(bodyUniqueId)
-            # joint_states = p.getJointStates(bodyUniqueId, range(num_joints))
-
-            # joint_log = []
-            # for i, state in enumerate(joint_states):
-            #     joint_position = state[0]  # Угол
-            #     joint_velocity = state[1]  # Угловая скорость
-            #     joint_log.append(
-            #         f"Joint {i} - Position: {joint_position}, Velocity: {joint_velocity}"
-            #     )
-            # logger.info("\n".join(joint_log))
-
-            p.stepSimulation()
-            time.sleep(1 / 240)
-
     def get_pybullet_image(self):
         if self.physics_client is None:
             return
 
         width, height, rgb, _, _ = p.getCameraImage(
-            width=640,
+            width=300,
-            height=640,
+            height=300,
             viewMatrix=p.computeViewMatrix(
                 cameraEyePosition=[2, 2, 2],
                 cameraTargetPosition=[0, 0, 0],
@@ -154,6 +126,9 @@ class SocketRobotArm:
         return self.status
 
     def upd_model(self):
+        threading.Thread(target=self.upd_model_func, daemon=True).start()
+
+    def upd_model_func(self):
         logger.info("UPDATE")
         self.get_axis()
         self.set_text(text=f"Координаты осей {self.start_axis_coordinates}")
@@ -201,12 +176,15 @@ class SocketRobotArm:
             time.sleep(0.05)
 
     def imitate(self):
+        threading.Thread(target=self.imitate_func, daemon=True).start()
+
+    def imitate_func(self):
         points = self.steps_from_file()
-        for i, p in enumerate(points):
+        for i, point in enumerate(points):
-            if p["action"] == "10":
+            if point["action"] == "10":
                 self.imitate_point = i
                 angles = self.convert_to_joint(
-                    [float(p) for p in [p["m0"], p["m1"], p["m2"]]]
+                    [float(point) for point in [point["m0"], point["m1"], point["m2"]]]
                 )
                 self.set_joint(angles)
                 time.sleep(0.5)

utils/pybullet_server.py

@@ -6,32 +6,31 @@ import pybullet as p
 import pybullet_data
 
 
-def main(type = 'SHARED_MEMORY_SERVER'):
+def main(type="SHARED_MEMORY_SERVER"):
     # Подключаемся к графическому серверу в режиме SHARED_MEMORY_SERVER
     # physicsClient = p.connect(p.SHARED_MEMORY_SERVER)
     physicsClient = p.connect(getattr(p, type))
     print(f"start pybullet ph client {physicsClient}")
     # Настраиваем среду
     p.setAdditionalSearchPath(pybullet_data.getDataPath())
-    p.setGravity(0, 0, -10)
+    p.setGravity(0, 0, -9.81, physicsClientId=physicsClient)
 
     while True:
         # Основной цикл физического движка
         p.stepSimulation()
-        time.sleep(1.0 / 240.0)
+        time.sleep(30 * 0.001)
 
 
 if __name__ == "__main__":
     # Парсер аргументов командной строки
     parser = argparse.ArgumentParser(description="Запуск PyBullet сервера.")
     parser.add_argument(
-        "--test", 
+        "--test", action="store_true", help="Запустить сервер в режиме GUI_SERVER"
-        action="store_true", 
-        help="Запустить сервер в режиме GUI_SERVER"
     )
 
     args = parser.parse_args()
 
     # Если передан аргумент --test, устанавливаем тип подключения SHARED_MEMORY_SERVER
-    connection_type = 'GUI_SERVER' if args.test else 'SHARED_MEMORY_SERVER'
+    connection_type = "GUI_SERVER" if args.test else "SHARED_MEMORY_SERVER"
+    connection_type = "SHARED_MEMORY_SERVER"
     main(connection_type)