remove world coord

robot/client_socket.py

@@ -40,6 +40,8 @@ class SocketRobotArm:
     command_type = "base"
     command_data = None
 
+    motion_type = "reset"
+
     def __init__(self, application_path, *args, **kwargs):
         self.socket = None
         self.host = None
@@ -89,12 +91,20 @@ class SocketRobotArm:
     def get_status(self):
         return self.socket_manager.status
 
+    def motionFund(self, index, radians):
+        if self.motion_type == "reset":
+            p.resetJointState(self.urdf_manager.body_id, index, radians)
+            time.sleep(0.3)
+        else:
+            p.setJointMotorControl2(
+                self.urdf_manager.body_id, index, p.POSITION_CONTROL, radians
+            )
+            time.sleep(1)
+
     def upd_model(self):
         threading.Thread(target=self.upd_model_func, daemon=True).start()
 
     def upd_model_func(self):
-        # logger.info("UPDATE MODEL FUNC")\
-
         self.get_coordinates("axis")
         self.set_text(text=f"Координаты осей {self.axis_coordinates}")
         time.sleep(0.5)
@@ -106,55 +116,38 @@ class SocketRobotArm:
         self._fetch_command_count()
         time.sleep(0.5)
 
-        def motionFund(i, axis):
-            motion_type = "reset"
-            try:
-                v = axis[i]
-                if v is None:
-                    v = 0
-            except IndexError:
-                v = 0
-            v = np.radians(v)
-            if motion_type == "reset":
-                p.resetJointState(self.urdf_manager.body_id, i, v)
-            else:
-                p.setJointMotorControl2(
-                    self.urdf_manager.body_id, i, p.POSITION_CONTROL, v
-                )
-
         num_joints = p.getNumJoints(self.urdf_manager.body_id)
         axisJointPoses = self.axis_coordinates
         for i in range(num_joints - 1):
-            motionFund(i, axisJointPoses)
+            v = axisJointPoses[i] if i < len(axisJointPoses) else 0
+            v = np.radians(v)
+            self.motionFund(i, v)
 
         time.sleep(0.5)
         
-        coordinates = self.world_coordinates[:3]
+        if False:
-        angles = [
+            coordinates = [float(p) * 0.001 for p in self.world_coordinates[:3]]
-            self.world_coordinates[3],
+            angles = [math.radians(p) for p in self.world_coordinates[3:6]]
-            self.world_coordinates[4],
+            worldJointPoses = self.convert_to_joint_base(coordinates, angles)
-            self.world_coordinates[5],
+            for i in range(num_joints):
-        ]
+                v = worldJointPoses[i] if i < len(worldJointPoses) else 0
-        worldJointPoses = self.convert_to_joint_base(
+                self.motionFund(i, v)
-            [float(p) * 0.001 for p in coordinates], [math.radians(p) for p in angles]
-        )
-        for i in range(num_joints):
-            motionFund(i, np.degrees(list(worldJointPoses)))
-        time.sleep(2)
 
-        calcWorldPosition = p.getLinkState(
+            time.sleep(0.5)
-            self.urdf_manager.body_id, num_joints - 1, computeForwardKinematics=True
-        )
-        calcWorldPosition = list(
-            [p * 1000 for p in calcWorldPosition[0]]
-            + [np.degrees(p) for p in p.getEulerFromQuaternion(calcWorldPosition[1])]
-        )
-        time.sleep(0.5)
 
-        position_diff = np.linalg.norm(
+            calcWorldPosition = p.getLinkState(
-            self.world_coordinates[:3] - np.array(calcWorldPosition[:3])
+                self.urdf_manager.body_id, num_joints - 1, computeForwardKinematics=True
-        )
+            )
-        print("Position Difference:", position_diff)
+            calcWorldPosition = list(
+                [p * 1000 for p in calcWorldPosition[0]]
+                + [np.degrees(p) for p in p.getEulerFromQuaternion(calcWorldPosition[1])]
+            )
+            time.sleep(0.5)
+
+            position_diff = np.linalg.norm(
+                self.world_coordinates[:3] - np.array(calcWorldPosition[:3])
+            )
+            print("Разница между расчетом по модели и мировыми координатами:", position_diff)
 
         self.set_text(text=f"Команд в очереди {self.command_count}")
         time.sleep(0.5)
@@ -275,17 +268,10 @@ class SocketRobotArm:
         for point in points:
             jointPoses = [np.radians(float(point[f"m{i}"])) for i in range(6)]
             num_joints = p.getNumJoints(self.urdf_manager.body_id)
+
             for i in range(num_joints):
-                try:
+                v = jointPoses[i] if i < len(jointPoses) else 0
-                    val = jointPoses[i]
+                self.motionFund(i, v)
-                except Exception as e:
-                    val = 0
-                # p.setJointMotorControl2(
-                #     self.urdf_manager.body_id, i, p.POSITION_CONTROL, val
-                # )
-                p.resetJointState(self.urdf_manager.body_id, i, val)
-            time.sleep(0.3)
-            # p.setJointMotorControlArray(self.urdf_manager.body_id, p.POSITION_CONTROL, jointPoses)
 
     def convert_point_to_free(self):
         points = self.steps_from_file()
@@ -304,7 +290,8 @@ class SocketRobotArm:
                     point["m5"],
                 ]
                 jointPoses = self.convert_to_joint_base(
-                    [float(p) * 0.001 for p in points], np.radians([float(x) for x in angles])
+                    [float(p) * 0.001 for p in points],
+                    np.radians([float(x) for x in angles]),
                 )
                 res.append(
                     self.prepare_data.make_step(

robot/urdf_manager.py

@@ -24,13 +24,13 @@ class UrdfManager:
 
         p.resetSimulation()
         self.load_models()
+        p.setGravity(0, 0, -9.81, physicsClientId=self.physics_client)
 
     def run_pybullet(self, type="DIRECT"):
         self.physics_client = p.connect(getattr(p, type))
         logger.info(f"Connect to {self.physics_client} by {type}")
 
     def load_models(self):
-        p.setGravity(0, 0, -9.81, physicsClientId=self.physics_client)
         p.loadURDF(
             os.path.join(self.application_path, "urdf", "plane.urdf"),
             physicsClientId=self.physics_client,