diff --git a/gui/robot.py b/gui/robot.py index 3f8e8be..cb8dc64 100644 --- a/gui/robot.py +++ b/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": diff --git a/robot/client_socket.py b/robot/client_socket.py index c33ea94..484b66a 100644 --- a/robot/client_socket.py +++ b/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": - p.resetJointState(self.urdf_manager.body_id, index, radians) - time.sleep(0.3) + for index in radians: + 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 + p.setJointMotorControlArray( + self.urdf_manager.body_id, + [i for i in range(len(radians))], + p.POSITION_CONTROL, + radians, ) time.sleep(1) @@ -116,38 +121,42 @@ 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): - v = axisJointPoses[i] if i < len(axisJointPoses) else 0 - v = np.radians(v) - self.motionFund(i, v) + self.motionFund(np.radians(axisJointPoses)) time.sleep(0.5) - + if False: 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) - - for i in range(num_joints): - v = jointPoses[i] if i < len(jointPoses) else 0 - self.motionFund(i, v) + self.motionFund(jointPoses) 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): - limit = p.getJointInfo(body_id, i) + for i in range(self.num_joints): + limit = p.getJointInfo(self.body_id, i) lower_limits.append(limit[8]) upper_limits.append(limit[9]) joint_angles = p.calculateInverseKinematics( - body_id, - endEffectorLinkIndex=num_joints - 1, + self.body_id, + endEffectorLinkIndex=self.num_joints - 1, targetPosition=[float(c) for c in coordinates], - targetOrientation=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), - # ], + targetOrientation=p.getQuaternionFromEuler(angles), ) return joint_angles diff --git a/robot/conveyor.py b/robot/conveyor.py index 9260841..33b2955 100644 --- a/robot/conveyor.py +++ b/robot/conveyor.py @@ -5,22 +5,25 @@ 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) self.conveyor_stopped = False - + self.box_size = box_size self.box_spacing = box_spacing self.box_speed = box_speed self.boxes = [] self.create_boxes() - + self.stop_thread = False 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,8 +91,7 @@ class ConveyorBelt: return box_id def move_boxes(self): - conveyor_stopped = False - if not self.conveyor_stopped: + 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.conveyor_stopped = False + self.palletizingFunc(position, box_id) + # time.sleep(5) + # self.conveyor_stopped = False def run(self): while not self.stop_thread: diff --git a/robot/urdf_manager.py b/robot/urdf_manager.py index f7f0d94..de4e8b7 100644 --- a/robot/urdf_manager.py +++ b/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], - [-1.5, -1, 0.5], + self.conv_size, + self.conv_pos, [0.2, 0.2, 0.2], 0.2, 0.1, + self.palletizing ) time.sleep(1)