upd model

client_socket.py

@@ -50,6 +50,9 @@ class SocketRobotArm:
         self.tkinter_imitate = tkinter.Button(
             self.tkinter_root, text="Imitate", command=self.imitate
         )
+        self.tkinter_upd_model = tkinter.Button(
+            self.tkinter_root, text="Update model state", command=self.upd_model
+        )
         self.physics_client = None
         self.body_id = None
 
@@ -73,7 +76,7 @@ class SocketRobotArm:
             f"urdf/{self.urdf_filename}.urdf",
             [0, 0, 0],
             useFixedBase=True,
-            globalScaling=1,
+            globalScaling=1.5,
         )
 
         while True:
@@ -88,12 +91,7 @@ class SocketRobotArm:
         self.socket = None
         # sys.exit()
 
-    def cycle_base(self):
-        self.tkinter_root.geometry("700x400")
-        self.tkinter_imitate.pack(pady=20)
-        self.tkinter_start_cycle.pack(pady=20)
-        self.tkinter_exit.pack(pady=20)
-
+    def upd_model(self):
         self.get_axis()
         self.set_text(text=f"Координаты осей {self.start_axis_coordinates}")
         time.sleep(0.5)
@@ -102,7 +100,18 @@ 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(self.start_world_coordinates[0:3]))
+
+    def cycle_base(self):
+        self.tkinter_root.geometry("700x400")
+
+        self.tkinter_imitate.pack()
+        self.tkinter_upd_model.pack() 
+
+        self.tkinter_start_cycle.pack(pady=20)
+        self.tkinter_exit.pack(pady=20)
+
+        self.upd_model()
 
         self.get_command_count()
         self.set_text(text=f"Команд в очереди {self.remote_command_count}")
@@ -120,9 +129,12 @@ class SocketRobotArm:
         self.add_rcc_list = (
             [self.set_physical_speed(True), self.set_output_laser(True)]
             + self.steps_from_file()
+            # + self.convert_file_to_join()
             + [self.set_physical_speed(False), self.set_output_laser(False)]
         )
 
+        # print(self.convert_file_to_join())
+
         self.tkinter_info_label.config(text=f"Отправка данных...")
         self.tkinter_info_label.pack()
         self.tkinter_root.update()
@@ -140,11 +152,7 @@ class SocketRobotArm:
             self.tkinter_info_label.config(text=f"Отправка данных {i}...{i+step-1}")
             self.tkinter_root.update()
             # print(empty)
-            self.send_data(
-                make_addrcc_data(
-                    self.add_rcc_list[i : i + step], empty
-                )
-            )
+            self.send_data(make_addrcc_data(self.add_rcc_list[i : i + step], empty))
             empty = 0
             time.sleep(0.05)
 
@@ -253,10 +261,13 @@ class SocketRobotArm:
             "tool": str(self.line_tool),
             "ckStatus": "0x3F",
         }
-        if type == "line":
+        if type == "line" or type == "free":
             pairs = zip(self.start_world_coordinates, point)
             m0, m1, m2, m3, m4, m5 = [round(sum(i), 3) for i in pairs]
+            if type == "line":
                 step.update({"action": "10"})
+            if type == "free":
+                step.update({"action": "4"})
             step.update({"m0": m0, "m1": m1, "m2": m2, "m3": m3, "m4": m4, "m5": m5})
             step.update({"m6": 0, "m7": 0})
         elif type == "curve":
@@ -305,6 +316,32 @@ class SocketRobotArm:
             },
         )
 
+    def convert_file_to_join(self):
+        result = []
+        with open(f"data/{self.filename}.nc.result", "r") as fp:
+            for line in fp:
+                data = line.strip().split(" ")
+                prep = {}
+                for item in data:
+                    prep[item[:1]] = float(item[1:])
+
+                result.append(
+                    self.make_world_step(
+                        "free",
+                        self.convert_to_joint(
+                            (
+                                prep.get("X", 0),
+                                prep.get("Y", 0),
+                                prep.get("Z", 0),
+                                # prep.get("U", 0),
+                                # prep.get("V", 0),
+                                # prep.get("W", 0),
+                            )
+                        ),
+                    )
+                )
+        return result
+
     def steps_from_file(self):
         result = []
         with open(f"data/{self.filename}.nc.result", "r") as fp:

data/test.nc.result

@@ -1,5 +1,3 @@
 X0 Y0
-X0 Y400
-X400 Y400
-X400 Y0
+X0 Y200
 X0 Y0

func.py

@@ -1,9 +1,9 @@
 # Настройки клиента Modbus
-# MODBUS_SERVER_HOST = "192.168.70.55"  # IP-адрес Modbus-сервера
-MODBUS_SERVER_HOST = "192.168.70.65"  # IP-адрес Modbus-сервера
+MODBUS_SERVER_HOST = "192.168.70.55"  # IP-адрес Modbus-сервера
+# MODBUS_SERVER_HOST = "192.168.70.65"  # IP-адрес Modbus-сервера
 MODBUS_SERVER_PORT = 502
-# MODBUS_SLAVE_ID = 11
-MODBUS_SLAVE_ID = 22
+MODBUS_SLAVE_ID = 11
+# MODBUS_SLAVE_ID = 22
 
 indent = 21100
 s = 800

urdf/sample.urdf

@@ -7,7 +7,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.1" />
+      <origin rpy="0 0 0" xyz="0 0 -0.2" />
       <geometry>
         <box size="0.2 0.2 0.2" />
       </geometry>
@@ -108,7 +108,7 @@
   </link>
   <!-- joints: main serial chain -->
   <joint name="joint_1" type="revolute">
-    <origin rpy="0 0 0" xyz="0 0 0.2" />
+    <origin rpy="0 0 0" xyz="0 0 0" />
     <parent link="base_link" />
     <child link="link_1" />
     <axis xyz="0 0 1" />
@@ -122,7 +122,7 @@
     <limit effort="0" lower="-0.5759" upper="2.6529" velocity="3.1415" />
   </joint>
   <joint name="joint_3" type="revolute">
-    <origin rpy="0 0 0" xyz="0 0 0.729921" />
+    <origin rpy="0 1.5708 0" xyz="0 0 0.729921" />
     <parent link="link_2" />
     <child link="link_3" />
     <axis xyz="0 -1 0" />