From 73e784bdb64a3151e083917602df0198583989f6 Mon Sep 17 00:00:00 2001
From: Kseninia Mikhaylova <k.mikhailova@svs-tech.pro>
Date: Tue, 8 Oct 2024 11:46:16 +0300
Subject: [PATCH] heregit add .git add .

---
 client_socket.py | 17 +++++++++++++++--
 urdf/sample.urdf | 24 ++++++++++++------------
 2 files changed, 27 insertions(+), 14 deletions(-)

diff --git a/client_socket.py b/client_socket.py
index e4bba36..1926c61 100644
--- a/client_socket.py
+++ b/client_socket.py
@@ -46,6 +46,9 @@ class SocketRobotArm:
         self.tkinter_start_cycle = tkinter.Button(
             self.tkinter_root, text="Start cycle", command=self.cycle_start
         )
+        self.tkinter_imitate = tkinter.Button(
+            self.tkinter_root, text="Imitate", command=self.imitate
+        )
         self.physics_client = None
         self.body_id = None
 
@@ -84,7 +87,8 @@ class SocketRobotArm:
         # sys.exit()
 
     def cycle_base(self):
-        self.tkinter_root.geometry("500x300")
+        self.tkinter_root.geometry("700x400")
+        self.tkinter_imitate.pack(pady=20)
         self.tkinter_start_cycle.pack(pady=20)
         self.tkinter_exit.pack(pady=20)
 
@@ -138,6 +142,15 @@ class SocketRobotArm:
             )
             time.sleep(0.5)
 
+    def imitate(self):
+        points = self.steps_from_file()
+        for p in points:
+            if p["action"] == "10":
+                angles = self.convert_to_joint(
+                    [float(p) for p in [p["m0"], p["m1"], p["m2"]]]
+                )
+                self.set_joint(angles)
+
     def convert_to_joint(self, coordinates):
         num_joints = p.getNumJoints(self.body_id)
         joint_info = [p.getJointInfo(self.body_id, i) for i in range(num_joints)]
@@ -164,7 +177,7 @@ class SocketRobotArm:
             )
             self.tkinter_info_label.pack()
             self.tkinter_root.update()
-            time.sleep(0.5)
+            time.sleep(0.01)
 
     def set_text(self, text):
         label = tkinter.Label(self.tkinter_root, text=text)
diff --git a/urdf/sample.urdf b/urdf/sample.urdf
index 041f39d..b17f2af 100644
--- a/urdf/sample.urdf
+++ b/urdf/sample.urdf
@@ -5,11 +5,11 @@
   <link name="base_link">
     <inertial>
       <mass value="1.0" />
-      <inertia ixx="0.00443333156" ixy="0.0" ixz="0.0" iyy="0.00443333156" iyz="0.0" izz="0.0072"/>
+      <inertia ixx="0.005" ixy="0.0" ixz="0.0" iyy="0.005" iyz="0.0" izz="0.005"/>
     </inertial>
     <visual>
       <geometry>
-        <box size="0.5 0.5 0.1" />
+        <box size="0.5 0.5 0.4" />
       </geometry>
       <material name="grey">
         <color rgba="0.5 0.5 0.5 1.0" />
@@ -20,7 +20,7 @@
   <link name="link1">
     <inertial>
       <mass value="1.0" />
-      <inertia ixx="0.00443333156" ixy="0.0" ixz="0.0" iyy="0.00443333156" iyz="0.0" izz="0.0072"/>
+      <inertia ixx="0.005" ixy="0.0" ixz="0.0" iyy="0.005" iyz="0.0" izz="0.005"/>
     </inertial>
     <visual>
       <geometry>
@@ -35,7 +35,7 @@
   <link name="link2">
     <inertial>
       <mass value="1.0" />
-      <inertia ixx="0.00443333156" ixy="0.0" ixz="0.0" iyy="0.00443333156" iyz="0.0" izz="0.0072"/>
+      <inertia ixx="0.005" ixy="0.0" ixz="0.0" iyy="0.005" iyz="0.0" izz="0.005"/>
     </inertial>
     <visual>
       <geometry>
@@ -50,7 +50,7 @@
   <link name="link3">
     <inertial>
       <mass value="1.0" />
-      <inertia ixx="0.00443333156" ixy="0.0" ixz="0.0" iyy="0.00443333156" iyz="0.0" izz="0.0072"/>
+      <inertia ixx="0.005" ixy="0.0" ixz="0.0" iyy="0.005" iyz="0.0" izz="0.005"/>
     </inertial>
     <visual>
       <geometry>
@@ -65,7 +65,7 @@
   <link name="link4">
     <inertial>
       <mass value="1.0" />
-      <inertia ixx="0.00443333156" ixy="0.0" ixz="0.0" iyy="0.00443333156" iyz="0.0" izz="0.0072"/>
+      <inertia ixx="0.005" ixy="0.0" ixz="0.0" iyy="0.005" iyz="0.0" izz="0.005"/>
     </inertial>
     <visual>
       <geometry>
@@ -80,7 +80,7 @@
   <link name="end_effector">
     <inertial>
       <mass value="1.0" />
-      <inertia ixx="0.00443333156" ixy="0.0" ixz="0.0" iyy="0.00443333156" iyz="0.0" izz="0.0072"/>
+      <inertia ixx="0.005" ixy="0.0" ixz="0.0" iyy="0.005" iyz="0.0" izz="0.005"/>
     </inertial>
     <visual>
       <geometry>
@@ -96,7 +96,7 @@
   <joint name="joint1" type="revolute">
     <parent link="base_link" />
     <child link="link1" />
-    <origin xyz="0 0 0.15" rpy="0 0 0" />
+    <origin xyz="0 0 0.494600" rpy="0 0 0" />
     <axis xyz="0 0 1" />
     <limit lower="-3.14" upper="3.14" effort="10" velocity="1" />
   </joint>
@@ -104,7 +104,7 @@
   <joint name="joint2" type="revolute">
     <parent link="link1" />
     <child link="link2" />
-    <origin xyz="0 0 0.3" rpy="0 0 0" />
+    <origin xyz="0 0 0.729921" rpy="0 0 0" />
     <axis xyz="0 1 0" />
     <limit lower="-3.14" upper="3.14" effort="10" velocity="1" />
   </joint>
@@ -112,7 +112,7 @@
   <joint name="joint3" type="revolute">
     <parent link="link2" />
     <child link="link3" />
-    <origin xyz="0 0 0.3" rpy="0 0 0" />
+    <origin xyz="0 0 0.098277" rpy="0 0 0" />
     <axis xyz="0 1 0" />
     <limit lower="-3.14" upper="3.14" effort="10" velocity="1" />
   </joint>
@@ -120,7 +120,7 @@
   <joint name="joint4" type="revolute">
     <parent link="link3" />
     <child link="link4" />
-    <origin xyz="0 0 0.3" rpy="0 0 0" />
+    <origin xyz="0 0 1.109039" rpy="0 0 0" />
     <axis xyz="0 1 0" />
     <limit lower="-3.14" upper="3.14" effort="10" velocity="1" />
   </joint>
@@ -128,7 +128,7 @@
   <joint name="end_effector_joint" type="revolute">
     <parent link="link4" />
     <child link="end_effector" />
-    <origin xyz="0 0 0.3" rpy="0 0 0" />
+    <origin xyz="0 0 0.0170450" rpy="0 0 0" />
     <axis xyz="0 1 0" />
     <limit lower="-3.14" upper="3.14" effort="10" velocity="1" />
   </joint>