model fix

2024-11-13 13:26:22 +03:00 · 2024-11-13 13:26:22 +03:00 · ba0e6ea657
parent e50ec6c17d
commit ba0e6ea657
6 changed files with 2614 additions and 51 deletions
--- a/app.log
+++ b/app.log
--- a/robot/client_socket.py
+++ b/robot/client_socket.py
@ -135,7 +135,9 @@ class UrdfManager:
    def load_models(self):
        # p.loadURDF("urdf/plane.urdf", physicsClientId=self.physics_client)

-        urdf_path = os.path.join(self.application_path, "urdf", f"{self.urdf_filename}.urdf")
+        urdf_path = os.path.join(
+            self.application_path, "urdf", f"{self.urdf_filename}.urdf"
+        )
        self.body_id = p.loadURDF(
            urdf_path,
            self.robot_start_position,
@ -196,15 +198,17 @@ class SocketRobotArm:
        self.imitate_point = None

        robot_start_position = [
-            -0.075458,  # новое значение x в метрах
-            0.068477,  # новое значение y в метрах
-            0.082201,  # новое значение z в метрах
+            -0.008700576263427698,
+            0.04579925857591629,
+            0.0036192361602783203,
        ]
-        robot_start_position = [0,0,0]
-        
+        robot_start_position = [0, 0, 0]
+
        self.prepare_data = PrepareRobotData()
        self.socket_manager = SocketManager()
-        self.urdf_manager = UrdfManager(robot_start_position=robot_start_position, application_path=application_path)
+        self.urdf_manager = UrdfManager(
+            robot_start_position=robot_start_position, application_path=application_path
+        )

    def __exit__(self, exc_type, exc_value, traceback):
        logger.info("exiting")
@ -236,39 +240,50 @@ class SocketRobotArm:
        threading.Thread(target=self.upd_model_func, daemon=True).start()

    def upd_model_func(self):
-        logger.info("UPDATE MODEL FUNC")
+        # logger.info("UPDATE MODEL FUNC")

        self.get_axis()
        self.set_text(text=f"Координаты осей {self.start_axis_coordinates}")
-        time.sleep(0.5)
        self.set_joint([math.radians(c) for c in self.start_axis_coordinates])
+        time.sleep(1)

        self.get_world()
        self.set_text(text=f"Мировые координаты {self.start_world_coordinates}")
-        time.sleep(0.5)
+        angles = self.start_world_coordinates[3:]
        self.set_joint(
            self.convert_to_joint(
-                [s * 0.001 for s in self.start_world_coordinates[0:3]]
+                [s * 0.001 for s in self.start_world_coordinates[0:3]],
+                angles,
            )
        )
+        time.sleep(1)

        self._get_command_count()
        self.set_text(text=f"Команд в очереди {self.remote_command_count}")
        time.sleep(0.5)

-        num_joints = p.getNumJoints(self.urdf_manager.body_id)
-        end_effector_state = p.getLinkState(
-            self.urdf_manager.body_id, num_joints - 1, computeForwardKinematics=True
-        )
+        # num_joints = p.getNumJoints(self.urdf_manager.body_id)
+        # end_effector_state = p.getLinkState(
+        #     self.urdf_manager.body_id, num_joints - 1, computeForwardKinematics=True
+        # )

-        end_effector_position = end_effector_state[4]
-        end_effector_orientation = end_effector_state[5]
-        euler_orientation = p.getEulerFromQuaternion(end_effector_orientation)
-
-        logger.info(
-            f"Вычисленное положение {[a * 1000 for a in end_effector_position]}"
-        )
-        logger.info(f"Вычисленная ориентация {np.degrees(euler_orientation)}")
+        # end_effector_position = end_effector_state[4]
+        # end_effector_orientation = end_effector_state[5]
+        # euler_orientation = list(p.getEulerFromQuaternion(end_effector_orientation))
+        # euler_orientation[1] += math.radians(90)
+        
+        # logger.info(
+        #     f"Вычисленное положение {[a * 1000 for a in end_effector_position]}"
+        # )
+        # logger.info(
+        #     f"""{[f'{a*1000} -- {self.start_world_coordinates[i]} -- {self.start_world_coordinates[i] - a*1000}' 
+        #     for i,a in enumerate(end_effector_position)]}"""
+        # )
+        # logger.info(f"Вычисленная ориентация {np.degrees(euler_orientation)}")
+        # logger.info(
+        #     f"""{[f'{a} -- {self.start_world_coordinates[i + 3]} -- {self.start_world_coordinates[i + 3] - a}' 
+        #     for i,a in enumerate(np.degrees(euler_orientation))]}"""
+        # )

    def cycle_base(self):
        self.upd_model()
@ -313,26 +328,28 @@ class SocketRobotArm:

    def convert_to_joint(self, coordinates, angles=[]):
        num_joints = p.getNumJoints(self.urdf_manager.body_id)
+        if len(angles) == 3:
+            angles[1] = float(angles[1]) - 90
+            target_orientation = p.getQuaternionFromEuler(
+                [math.radians(float(a)) for a in angles]
+            )
+        else:
+            target_orientation = None

        joint_angles = p.calculateInverseKinematics(
            self.urdf_manager.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
-            ),
+            targetOrientation=target_orientation,
        )
-        logger.info(f"convrt to joint {joint_angles}")
-        # logger.info(joint_angles)
+        # logger.info(f"convrt to joint {joint_angles}")
        return joint_angles

    def set_text(self, text):
        logger.info(text)

    def set_joint(self, coordinates):
-        logger.info("set joints")
+        # logger.info("set joints")
        num_joints = p.getNumJoints(self.urdf_manager.body_id)
        if coordinates is None:
            return
@ -345,7 +362,7 @@ class SocketRobotArm:
                controlMode=p.POSITION_CONTROL,
                targetPosition=coordinates[joint_index],
            )
-            time.sleep(0.1)
+            time.sleep(0.01)

    def send_data(self, data):
        return self.socket_manager.send_data(data)
--- a/urdf/coord.py
+++ b/urdf/coord.py
@ -0,0 +1,11 @@
+# Мировые координаты
+world_coordinates = [
+    1131.959351, -588.689941, 1277.805054,
+    6.181231, 54.227802, -100.604988
+]
+
+# Поворот осей
+axis_rotation = [
+    -41.612457, 31.759747, -26.773878,
+    74.869049, -45.417992, -20.86335
+]
--- a/urdf/sample.urdf
+++ b/urdf/sample.urdf
@ -45,7 +45,7 @@
      <inertia ixx="0.005" ixy="0.0" ixz="0.0" iyy="0.005" iyz="0.0" izz="0.005" />
    </inertial>
    <visual>
-      <origin rpy="0 0 0" xyz="0 0 0.0" />
+      <origin rpy="0 0 0" xyz="0 0 0" />
      <geometry>
        <mesh filename="../urdf_support/sample/2.stl"/>
      </geometry>
@ -126,7 +126,7 @@
  <!-- Joints: main serial chain -->
  <joint name="joint_1" type="revolute">
    <limit lower="-2.705" upper="2.705" effort="0" velocity="0"/>
-    <origin rpy="0 0 0" xyz="0.011789 -0.0278 0.26158" />
+    <origin rpy="0 0 0" xyz="0.014359931 0.001723413 0.261326995" />
    <parent link="base_link" />
    <child link="link_1" />
    <axis xyz="0 0 1" />
--- a/utils/pybullet_server.py
+++ b/utils/pybullet_server.py
@ -24,7 +24,7 @@ def main(type="SHARED_MEMORY_SERVER"):
        except Exception as e:
            print(f"Ошибка в pybullet_server{e}")
            time.sleep(1)
-        time.sleep(30 * 0.001)
+        time.sleep(1/240)


 if __name__ == "__main__":
--- a/utils/test_socket_server.py
+++ b/utils/test_socket_server.py
@ -4,6 +4,37 @@ import json
 HOST = "127.0.0.1"  # Standard loopback interface address (localhost)
 PORT = 9760  # Port to listen on (non-privileged ports are > 1023)

+coordinates = [
+    {
+        "world": [
+            1131.959351,
+            -588.689941,
+            1277.805054,
+            6.181231,
+            54.227802,
+            -100.604988,
+        ],
+        "axis": [-41.612457, 31.759747, -26.773878, 74.869049, -45.417992, -20.86335],
+    },
+    {
+        "world": [1282.244, -75.427, 1772.476, 84.629, 34.519, 153.999],
+        "axis": [-8.487, -8.681, 33.058, 88.070, -75.010, -10.566],
+    },
+    {
+        "world": [1630.006, 628.326, 1322.554, 0, 90, 0],
+        "axis": [22.493, -30.696, 35.997, -77.419, -23.007, 76.362],
+    },
+    {
+        "world": [1052.757, -1028.846, 334.417, 162.374, 20.467, 75.065],
+        "axis": [-43.742, -30.698, -12.227, -21.884, -23.078, 76.362],
+    },
+    {
+        "world": [1396.525, 1.687, 1322.697, 0, 90, 0],
+        "axis": [0, -0.002, -0.002, 0.003, -0.001, 0],
+    },
+]
+coordinates_index = 2
+

 def handle_client(conn, addr):
    print(f"Connected by {addr}")
@ -16,24 +47,10 @@ def handle_client(conn, addr):
            res = {"queryData": ["ok"]}

            if "queryAddr" in req.keys() and "axis-0" in req["queryAddr"]:
-                res["queryData"] = [
-                    -41.612457,
-                    31.759747,
-                    -26.773878,
-                    74.869049,
-                    -45.417992,
-                    -20.86335,
-                ]
+                res["queryData"] = coordinates[coordinates_index]["axis"]

            if "queryAddr" in req and "world-0" in req["queryAddr"]:
-                res["queryData"] = [
-                    1131.959351,
-                    -588.689941,
-                    1277.805054,
-                    6.181231,
-                    54.227802,
-                    -100.604988,
-                ]
+                res["queryData"] = coordinates[coordinates_index]["world"]

            if req["reqType"] == "command":
                res["cmdReply"] = ["ok"]