pick obj part

gui/robot.py

@@ -49,7 +49,6 @@ class ChangeRobot(QWidget):
 
     def updateConnectButtonText(self):
         radioState = True
-        logger.info(self.status())
         if self.status() == "not_connected":
             self.connectButton.setText("Соединить")
         elif self.status() == "connected":

robot/client_socket.py

@@ -41,6 +41,7 @@ class SocketRobotArm:
     command_data = None
 
     motion_type = "reset"
+    motion_type = "normal"
 
     def __init__(self, application_path, *args, **kwargs):
         self.socket = None
@@ -57,7 +58,9 @@ class SocketRobotArm:
         self.prepare_data = PrepareRobotData()
         self.socket_manager = SocketManager()
         self.urdf_manager = UrdfManager(
-            robot_start_position=robot_start_position, application_path=application_path
+            robot_start_position,
+            application_path,
+            self.palletizing,
         )
 
     def __exit__(self, exc_type, exc_value, traceback):
@@ -69,10 +72,8 @@ class SocketRobotArm:
 
     def start_loop(self, urdf):
         self.urdf_manager.start_loop(urdf)
-
-    def load_models(self):
-        self.urdf_manager.load_models()
         self.body_id = self.urdf_manager.body_id
+        self.num_joints = p.getNumJoints(self.body_id)
 
     def load_palletizing(self):
         self.urdf_manager.load_palletizing()
@@ -91,13 +92,17 @@ class SocketRobotArm:
     def get_status(self):
         return self.socket_manager.status
 
-    def motionFund(self, index, radians):
+    def motionFund(self, radians):
         if self.motion_type == "reset":
+            for index in radians:
                 p.resetJointState(self.urdf_manager.body_id, index, radians)
                 time.sleep(0.3)
         else:
-            p.setJointMotorControl2(
+            p.setJointMotorControlArray(
-                self.urdf_manager.body_id, index, p.POSITION_CONTROL, radians
+                self.urdf_manager.body_id,
+                [i for i in range(len(radians))],
+                p.POSITION_CONTROL,
+                radians,
             )
             time.sleep(1)
 
@@ -116,12 +121,8 @@ class SocketRobotArm:
         self._fetch_command_count()
         time.sleep(0.5)
 
-        num_joints = p.getNumJoints(self.urdf_manager.body_id)
         axisJointPoses = self.axis_coordinates
-        for i in range(num_joints - 1):
+        self.motionFund(np.radians(axisJointPoses))
-            v = axisJointPoses[i] if i < len(axisJointPoses) else 0
-            v = np.radians(v)
-            self.motionFund(i, v)
 
         time.sleep(0.5)
 
@@ -129,25 +130,33 @@ class SocketRobotArm:
             coordinates = [float(p) * 0.001 for p in self.world_coordinates[:3]]
             angles = [math.radians(p) for p in self.world_coordinates[3:6]]
             worldJointPoses = self.convert_to_joint_base(coordinates, angles)
-            for i in range(num_joints):
+            for i in range(self.num_joints):
                 v = worldJointPoses[i] if i < len(worldJointPoses) else 0
-                self.motionFund(i, v)
+                self.motionFund(worldJointPoses)
 
             time.sleep(0.5)
 
             calcWorldPosition = p.getLinkState(
-                self.urdf_manager.body_id, num_joints - 1, computeForwardKinematics=True
+                self.urdf_manager.body_id,
+                self.num_joints - 1,
+                computeForwardKinematics=True,
             )
             calcWorldPosition = list(
                 [p * 1000 for p in calcWorldPosition[0]]
-                + [np.degrees(p) for p in p.getEulerFromQuaternion(calcWorldPosition[1])]
+                + [
+                    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)
+            print(
+                "Разница между расчетом по модели и мировыми координатами:",
+                position_diff,
+            )
 
         self.set_text(text=f"Команд в очереди {self.command_count}")
         time.sleep(0.5)
@@ -190,6 +199,44 @@ class SocketRobotArm:
     def imitate(self):
         threading.Thread(target=self.imitate_func, daemon=True).start()
 
+    def palletizing(self, position, box_id):
+        conv = position[:]
+        conv[1] += 0.4
+        
+        position[1] += 0.02
+
+        for pos in [conv, position]:
+            jointPoses = self.convert_to_joint_base(
+                pos,
+                np.radians([0, 0, -90]),
+            )
+            self.motionFund(jointPoses)
+            time.sleep(2)
+            
+        end_effector_state = p.getLinkState(self.body_id, self.num_joints - 1)
+        end_effector_pos = end_effector_state[0]  # Позиция наконечника
+        end_effector_orn = end_effector_state[1]  # Ориентация наконечника
+
+        # Позиция стенки (или коробки), к которой мы прикрепляем наконечник
+        object_pos, object_orn = p.getBasePositionAndOrientation(box_id)
+
+        # Определяем смещение, которое будет применено для фиксации позиции (без вращения)
+        parentFramePosition = [position[i] - end_effector_pos[i] for i in range(3)]
+        parentFrameOrientation = end_effector_orn  # Ориентация робота (наконечника)
+
+        # Создаём констрейнт для прикрепления
+        constraint_id = p.createConstraint(
+            parentBodyUniqueId=self.body_id,  # Идентификатор робота
+            parentLinkIndex=self.num_joints - 1,  # Индекс последнего звена (наконечника)
+            childBodyUniqueId=box_id,  # Идентификатор коробки
+            childLinkIndex=-1,  # -1 означает базовую ссылку объекта
+            jointType=p.JOINT_FIXED,  # Фиксированный констрейнт
+            jointAxis=[0, 0, 0],  # Без вращательной оси
+            parentFramePosition=parentFramePosition,  # Позиция относительно робота
+            childFramePosition=[1, 1, 1],  # Позиция объекта (относительно себя)
+            parentFrameOrientation=parentFrameOrientation  # Ориентация относительно робота
+        )
+
     def set_text(self, text):
         logger.info(text)
 
@@ -267,11 +314,7 @@ class SocketRobotArm:
         points = self.convert_point_to_free()
         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)
+            self.motionFund(jointPoses)
-
-            for i in range(num_joints):
-                v = jointPoses[i] if i < len(jointPoses) else 0
-                self.motionFund(i, v)
 
     def convert_point_to_free(self):
         points = self.steps_from_file()
@@ -301,32 +344,18 @@ class SocketRobotArm:
         return res
 
     def convert_to_joint_base(self, coordinates, angles):
-        body_id = 0
-        num_joints = p.getNumJoints(body_id)
-
         lower_limits = []
         upper_limits = []
-        for i in range(num_joints):
+        for i in range(self.num_joints):
-            limit = p.getJointInfo(body_id, i)
+            limit = p.getJointInfo(self.body_id, i)
             lower_limits.append(limit[8])
             upper_limits.append(limit[9])
 
         joint_angles = p.calculateInverseKinematics(
-            body_id,
+            self.body_id,
-            endEffectorLinkIndex=num_joints - 1,
+            endEffectorLinkIndex=self.num_joints - 1,
             targetPosition=[float(c) for c in coordinates],
-            targetOrientation=angles,
+            targetOrientation=p.getQuaternionFromEuler(angles),
-            # lowerLimits=lower_limits,
-            # upperLimits=upper_limits,
-            # jointRanges=[4, 4, 4, 4, 4, 4],
-            # restPoses=[
-            #     math.radians(0.1873),
-            #     math.radians(59.059),
-            #     math.radians(-99.4105),
-            #     math.radians(-0.2214),
-            #     math.radians(41.7877),
-            #     math.radians(-0.0011),
-            # ],
         )
 
         return joint_angles

robot/conveyor.py

@@ -5,7 +5,9 @@ from logger import logger
 
 
 class ConveyorBelt:
-    def __init__(self, size, position, box_size, box_spacing, box_speed):
+    def __init__(
+        self, size, position, box_size, box_spacing, box_speed, palletizinFunc
+    ):
         self.conveyor_size = size
         self.conveyor_position = position
         self.conveyor_id = self.create_conveyor(size, position)
@@ -21,6 +23,7 @@ class ConveyorBelt:
         self.thread = threading.Thread(target=self.run)
         self.thread.start()
 
+        self.palletizingFunc = palletizinFunc
 
     def create_conveyor(self, size, position):
         collision_shape_id = p.createCollisionShape(
@@ -88,7 +91,6 @@ class ConveyorBelt:
         return box_id
 
     def move_boxes(self):
-        conveyor_stopped = False
         if not self.conveyor_stopped:
             for box_id in self.boxes[:]:
                 pos, orn = p.getBasePositionAndOrientation(box_id)
@@ -101,13 +103,13 @@ class ConveyorBelt:
                     # Удаляем коробку с конвейера
                     self.boxes.remove(box_id)
                     self.conveyor_stopped = True
-                    self.call_robot()
+                    right_wall_position = [pos[0], pos[1] + self.box_size[1] * 0.5, pos[2]]
+                    self.call_robot(right_wall_position, box_id)
                 else:
                     # Двигаем коробку вдоль конвейера (по оси X)
                     new_pos = [pos[0] + self.box_speed, pos[1], pos[2]]
                     p.resetBasePositionAndOrientation(box_id, new_pos, orn)
 
-
         # Добавляем новые коробки, если на конвейере меньше, чем максимально возможное количество
         if len(self.boxes) < int(
             self.conveyor_size[0] / (self.box_size[0] + self.box_spacing)
@@ -115,11 +117,12 @@ class ConveyorBelt:
             new_box = self.create_box(self.box_position())
             self.boxes.append(new_box)
 
-    def call_robot(self):
+    def call_robot(self, position, box_id):
         # Здесь можно вызвать робота для выполнения операции, например:
         logger.info("Конвейер остановлен, вызываем робота для работы с коробкой.")
-        time.sleep(5)
+        self.palletizingFunc(position, box_id)
-        self.conveyor_stopped = False
+        # time.sleep(5)
+        # self.conveyor_stopped = False
 
     def run(self):
         while not self.stop_thread:

robot/urdf_manager.py

@@ -15,9 +15,10 @@ class UrdfManager:
     physics_client = None
     body_id = None
 
-    def __init__(self, robot_start_position, application_path):
+    def __init__(self, robot_start_position, application_path, palletizing):
         self.robot_start_position = robot_start_position
         self.application_path = application_path
+        self.palletizing = palletizing
 
     def start_loop(self, urdf):
         self.urdf_filename = urdf
@@ -46,18 +47,22 @@ class UrdfManager:
         )
 
     def load_palletizing(self):
-        pallet_position = [1.5, -1, 0.1]
+        # p.resetSimulation()
+        pallet_position = [1.5, -1.5, 0.1]
         urdf_path_pallet = os.path.join(
             self.application_path, "urdf", "europallet.urdf"
         )
         self.pallet_id = p.loadURDF(urdf_path_pallet, pallet_position)
 
+        self.conv_pos = [-1.5, -1.5, 0.5]
+        self.conv_size = [3.0, 0.5, 0.1]
         ConveyorBelt(
-            [3.0, 0.5, 0.1],
+            self.conv_size,
-            [-1.5, -1, 0.5],
+            self.conv_pos,
             [0.2, 0.2, 0.2],
             0.2,
             0.1,
+            self.palletizing
         )
 
         time.sleep(1)