robot_2d_3_joint.py

import numpy as np


class Robot2d3joint:
    """
    Плоский шарнирный манипулятор с 3 степенями свободы
    Жестко заданный угол инструмента
    Все углы theta положительны в направлении против часовой стрелки...
    ... и отрицательный по часовой.
    Все углы в радианах.
    Все длины в миллиметрах.
    Конечное звено согласно условию всегда направленно параллельно оси OX.
    Для обобщения используется угол alpha, образованный осью OX и звеном L_3    
    """
    def __init__(self, L_1, L_2, L_3, theta_1=0, theta_2=0, alpha=0):
        """
        Инициализация робота с заданными длинами звеньев и углом alpha.

        Args:
            L_1 (float): Длина 1-ого звена OA, мм
            L_2 (float): Длина 2-ого звена AB, мм
            L_3 (float): Длина 3-его звена BC, мм
            theta_1 (float): Угол 1-ого звена, радиан
            theta_2 (float): Угол 2-ого звена, радиан
            alpha (float): Угол ориентации рабочего органа (в радиан).
        """
        self.set_lengths(L_1, L_2, L_3)
        
        self.alpha = alpha
        self.set_joint_position(theta_1, theta_2)
        self.set_alpha(alpha)
        # точки A B C
        self.update()
        

    def set_lengths(self, L_1, L_2, L_3):
        """
        Задать длинну звеньев робота, мм
        """
        self.L_1 = L_1
        self.L_2 = L_2
        self.L_3 = L_3


    def set_tcp(self, x, y):
        """
        Устанавливает x, y TCP в мм.
        """
        self.tcp_x, self.tcp_y = x, y



    def set_alpha(self, alpha):
        """
        Задать специфический угол для инструмента относительно OX, радиан
        """
        self.alpha = alpha
        self.theta_3 = self.alpha - self.theta_1 - self.theta_2


    def set_joint_position(self, theta_1, theta_2):
        """
        Задать углы шарниров, радиан
        """
        self.theta_1 = theta_1
        self.theta_2 = theta_2
        self.theta_3 = self.alpha - self.theta_1 - self.theta_2



    def forward_kinematics(self, theta_1=None, theta_2=None, theta_3=None):
        """
        По умолчанию использует последние заданные углы
        Args:
            theta_1 (float): Угол 1-ого сустава (в радианах).
            theta_2 (float): Угол 2-ого сустава (в радианах).
            theta_3 (float): Угол 3-его сустава (в радианах).

        Returns:
            tuple: (x, y) - координаты TCP.
        """
        L_1, L_2, L_3 = self.L_1, self.L_2, self.L_3

        # Использовать значения по умолчанию если нет аргументов
        theta_1 = self.theta_1 if theta_1 is None else theta_1
        theta_2 = self.theta_2 if theta_2 is None else theta_2
        theta_3 = self.theta_3 if theta_3 is None else theta_3

        x = float(L_1 * np.cos(theta_1) + L_2 * np.cos(theta_1 + theta_2) + L_3 * np.cos(theta_1 + theta_2 + theta_3))
        y = float(L_1 * np.sin(theta_1) + L_2 * np.sin(theta_1 + theta_2) + L_3 * np.sin(theta_1 + theta_2 + theta_3))
        return (x, y)


    def inverse_kinematics(self, x=None, y=None):
        """
        Вычисляет углы суставов для достижения заданного положения рабочего органа.
        Использует аналитическое решение для плоского манипулятора с 3 степенями свободы.
        Возвращает несколько решений
        Args:
            x (float): TCP_x.
            y (float): TCP_y.

        Returns:
            list: Список решений - кортежей (theta_1, theta_2, theta_3)
                  Возвращает пустой список, если решение не найдено.
        """
        L_1, L_2, L_3 = self.L_1, self.L_2, self.L_3

        # Использовать значения по умолчанию если нет аргументов
        x = self.tcp_x if x is None else x
        y = self.tcp_y if x is None else y

        alpha = self.alpha

        # Переход от рассчета точки C на точку B (задача на 2 шарнира))
        # Рассчет координат точки B
        b_x = x - L_3 * np.cos(alpha)
        b_y = y - L_3 * np.sin(alpha)
        # Прямая OB (r) по теореме Пифагора:
        r = np.hypot(b_x, b_y)
    
        # Проверка максимальной и минимальной достижимости
        if (r > L_1 + L_2) or (r < abs(L_1 - L_2)):
            raise ValueError("Target position is not reachable")

        # угол q1 = BOX. Из определения косинуса:
        q1 = np.arccos(b_x / r)
        # угол q2 = BOA. Из теоремы косинусов в треугольнике OBA:
        q2 = np.arccos((r**2 + L_1**2 - L_2**2) / (2 * r * L_1))
                
        # Две конфигурации
        theta_1_1 = q1 - q2
        theta_1_2 = q1 + q2
        # По теореме косинусов в треугольнике OBA (2-ая конфигурация зеркально отражает углы плеч по оси симметрии OB):
        theta_2_1 = np.pi - np.arccos((L_1**2 + L_2**2 - r**2) / (2 * L_1 * L_2))
        theta_2_2 = -theta_2_1                                              
        # Вычисление theta_3 с фиксированным углом alpha относительно горизонта OX
        theta_3_1 = alpha - theta_1_1 - theta_2_1
        theta_3_2 = alpha - theta_1_2 - theta_2_2

        solutions = [(theta_1_1, theta_2_1, theta_3_1), (theta_1_2, theta_2_2, theta_3_2)]
        return solutions  # Возвращаем оба решения


    def update(self):
        L_1, L_2, L_3 = self.L_1, self.L_2, self.L_3
        theta_1 = self.theta_1
        theta_2 = self.theta_2
        theta_3 = self.theta_3

        Ax = L_1 * np.cos(theta_1)
        Bx = Ax + L_2 * np.cos(theta_1 + theta_2)
        Cx = Bx + L_3 * np.cos(theta_1 + theta_2 + theta_3)
        
        Ay = L_1 * np.sin(theta_1)
        By = Ay + L_2 * np.sin(theta_1 + theta_2)
        Cy = By + L_3 * np.sin(theta_1 + theta_2 + theta_3)

        self.a_coord = (Ax, Ay)
        self.b_coord = (Bx, By)
        self.c_coord = (Cx, Cy)
        
        self.tcp_x, self.tcp_y = self.forward_kinematics()

    def get_abc_position(self):
        return self.a_coord, self.b_coord, self.c_coord
        
  
    def print_angles(self):
        Q1 = np.round(np.degrees(self.theta_1), decimals=2)
        Q2 = np.round(np.degrees(self.theta_2), decimals=2)
        Q3 = np.round(np.degrees(self.theta_3), decimals=2)
        print(f"- Current angles: Q1: {Q1} deg, Q2: {Q2} deg, Q3: {Q3} deg")


    def print_tcp(self):
        x = round(self.tcp_x, 3)
        y = round(self.tcp_y, 3)
        print(f"- Current TCP x: {x} mm; y: {y} mm")

    
    def print_ik_solutions(self, solutions):
        """
        Выводит на экран углы theta для двух возможных решений обратной кинематики.

        Args:
            first_config (list): Список углов для первого решения (в радианах).
            second_config (list): Список углов для второго решения (в радианах).
        """
        first_config, second_config = solutions
        print("Первая конфигурация (+):")
        rounded_solution_1 = (tuple(map(lambda x: round(float(np.rad2deg(x)), 3), first_config)))
        Q1_1 = rounded_solution_1[0]
        Q2_1 = rounded_solution_1[1]
        Q3_1 = rounded_solution_1[2]
        print(f"- First solution: Q1: {Q1_1} deg, Q2: {Q2_1} deg, Q3: {Q3_1} deg")
        print("Вторая конфигурация (-):")
        rounded_solution_1 = (tuple(map(lambda x: round(float(np.rad2deg(x)), 3), second_config)))
        Q1_2 = rounded_solution_1[0]
        Q2_2 = rounded_solution_1[1]
        Q3_2 = rounded_solution_1[2]
        print(f"- Second solution: Q1: {Q1_2} deg, Q2: {Q2_2} deg, Q3: {Q3_2} deg")




if __name__ == "__main__":
    print("=" * 10, "start", "=" * 10)
    robot = Robot2d3joint(1, 1, 1, np.deg2rad(0), np.deg2rad(0))
    print("Начальное положение:")
    robot.print_angles()
    robot.print_tcp()


    print()
    print("Решение обратной кинематики:")
    target = (0, 0)
    print("Целевая точка", target)
    try:
        robot.print_ik_solutions(robot.inverse_kinematics(*target))
        robot.print_tcp()
    except ValueError:
        print("Позиция недостижима")
    print()


    print()
    print("Решение прямой кинематики:")
    joints_deg = (-135, 0, 0)
    print("Заданные углы, в градусах", joints_deg)
    joints_rad = tuple(map(np.deg2rad, joints_deg))
    print("Заданные углы, в радианах", joints_rad)
    robot.set_joint_position(joints_rad[0], joints_rad[1])
    robot.set_alpha(joints_rad[2])
    robot.forward_kinematics(*joints_rad)
    robot.update()    
    robot.print_tcp()
    print()