狗腿反解

机器狗移动原理

控制舵机转动实现机器狗足端的移动

重点:根据足端坐标获取两个舵机的转动角度

狗腿反解

在当前的狗腿子中,我们其实是知道每个关节的长度的,现在我们其实要做的事情就是已知足端左表(x,y) ,
求解机械狗上面的舵机需要转动的角度. 这个该如何来进行求解呢?
为了便于大家学习, 我们先将机械狗模型简化为平面几何,如下图所示:

我们需要求出上图左表的夹角alpha, 右边的夹角为beta

反解代码

def __init__(self):
    super().__init__()
    # 设置标题
    self.setWindowTitle('机器狗绘制')
    # 设置窗口大小
    self.setFixedSize(640, 480)
    # -------------- 中心点坐标 -------------- #
    self.center_x = 320
    self.center_y = 100
    # 大小的缩放系数
    self.ratio = 4
    # -------------- 已知狗腿数据 -------------- #
    self.AF = 12.5 * self.ratio
    self.AB = 21.5 * self.ratio
    self.EF = 21.5 * self.ratio
    self.CE = 21.5 * self.ratio
    self.BC = 34.19 * self.ratio
    self.CD = 34.95 * self.ratio
    # BC和CD的夹角
    self.bcd_angle = math.radians(120)
    self.half_AF = self.AF / 2
    # -------------- 各个端点坐标 -------------- #
    self.AX = -self.half_AF
    self.AY = 0
    self.FX = self.half_AF
    self.FY = 0
    self.BX = 0
    self.BY = 0
    self.CX = 0
    self.CY = 0
    self.EX = 0
    self.EY = 0
    # -------------- 足端坐标 -------------- #
    self.DX = 0
    self.DY = 60 * self.ratio
    # -------------- 按下的坐标 -------------- #
    self.press_x = self.DX
    self.press_y = self.DY
    # -------------- alpha和beta -------------- #
    self.alpha = 0
    self.beta = 0
    # -------------- 开始逆解 -------------- #
    self.results = self.inverse(self.DX, self.DY)
def inverse(self, fx, fy):
    # AD长度
    AD2 = (self.half_AF + fx) ** 2 + fy ** 2
    AD = math.sqrt(AD2)
    # DAF夹角
    angle_DAF = math.asin(fy / AD)
    # BD长度
    BD2 = self.BC ** 2 + self.CD ** 2 - 2 * self.BC * self.CD * math.cos(self.bcd_angle)
    BD = math.sqrt(BD2)
    # BAD夹角
    angle_BAD = math.acos((self.AB ** 2 + AD2 - BD2) / (2 * self.AB * AD))
    # AB对应的alpha夹角
    alpha = math.pi - angle_BAD - angle_DAF
    angle_DBA = math.acos((self.AB ** 2 + BD2 - AD2) / (2 * self.AB * BD))
    angle_DBC = math.acos((self.BC ** 2 + BD2 - self.CD ** 2) / (2 * self.BC * BD))
    angle_ABC = angle_DBA - angle_DBC
    # AC长度
    AC2 = self.AB ** 2 + self.BC ** 2 - 2 * self.AB * self.BC * math.cos(angle_ABC)
    AC = math.sqrt(AC2)
    angle_BAC = math.acos((self.AB ** 2 + AC2 - self.BC ** 2) / (2 * self.AB * AC))
    angle_CAF = math.pi - alpha - angle_BAC
    CF2 = self.AF ** 2 + AC2 - 2 * self.AF * AC * math.cos(angle_CAF)
    CF = math.sqrt(CF2)
    angle_AFC = math.acos((self.AF ** 2 + CF2 - AC2) / (2 * self.AF * CF))
    angle_CFE = math.acos((CF2 + self.EF ** 2 - self.CE ** 2) / (2 * CF * self.EF))
    beta = math.pi - angle_AFC - angle_CFE
    # 计算各个点的坐标
    self.BX = -(self.AB * math.cos(alpha) + self.half_AF)
    self.BY = self.AB * math.sin(alpha)
    self.CX = math.cos(angle_CAF) * AC - self.half_AF
    self.CY = AC * math.sin(angle_CAF)
    self.DX = fx
    self.DY = fy
    self.EX = self.half_AF + self.EF * math.cos(beta)
    self.EY = self.EF * math.sin(beta)
    self.alpha = alpha
    self.beta = beta
    return alpha, beta

绘制狗腿

def paintEvent(self, event):
    # 创建画家
    painter = QPainter(self)
    # 设置画笔中心
    painter.translate(self.center_x, self.center_y)
    painter.setRenderHint(QPainter.Antialiasing, True)
    # 创建画笔
    pen = QPen()
    # 设置画笔颜色
    pen.setColor(QColor(255, 0, 0))
    pen.setWidth(3)
    # 设置画笔
    painter.setPen(pen)
    # 绘制
    painter.drawLine(self.AX, self.AY, self.FX, self.FY)
    pen.setColor(QColor(0, 255, 0))
    painter.setPen(pen)
    painter.drawLine(self.AX, self.AY, self.BX, self.BY)
    pen.setColor(QColor(0, 0, 255))
    painter.setPen(pen)
    painter.drawLine(self.BX, self.BY, self.CX, self.CY)
    pen.setColor(QColor(0, 0, 255))
    painter.setPen(pen)
    painter.drawLine(self.CX, self.CY, self.DX, self.DY)
    pen.setColor(QColor(255, 0, 255))
    painter.setPen(pen)
    painter.drawLine(self.CX, self.CY, self.EX, self.EY)
    pen.setColor(QColor(255, 255, 0))
    painter.setPen(pen)
    painter.drawLine(self.EX, self.EY, self.FX, self.FY)
    # -------------- 绘制点的label -------------- #
    # 设置刷子
    painter.setBrush(QColor(255, 0, 0))
    painter.drawEllipse(self.press_x - 2, self.press_y - 2, 4, 4)
    radius = 4
    pen.setColor(QColor(255, 0, 0))
    pen.setWidth(5)
    font = QFont()
    font.setBold(True)
    font.setPixelSize(30)
    painter.setFont(font)
    # 设置画笔
    painter.setPen(pen)
    self.draw_point_label(painter, self.AX, self.AY, "A")
    self.draw_point_label(painter, self.BX, self.BY, "B")
    self.draw_point_label(painter, self.CX, self.CY, "C")
    self.draw_point_label(painter, self.DX, self.DY, "D")
    self.draw_point_label(painter, self.EX, self.EY, "E")
    self.draw_point_label(painter, self.FX, self.FY, "F")
    font.setPixelSize(20)
    painter.setFont(font)
    painter.drawText(QPoint(self.DX + 10, self.DY + 10), f"x={self.DX:.2f}.y={self.DY:.2f}")
    # painter.translate(-self.center_x,-self.center_y)
    painter.drawText(QPoint(self.AX - 200, self.AY - 30), f"alpha:{math.degrees(self.alpha):.2f}")
    painter.drawText(QPoint(self.FX + 100, self.FY - 30), f"beta:{math.degrees(self.beta):.2f}")
def draw_point_label(self, painter, x, y, label):
    '''绘制点的标题'''
    x = int(x)
    y = int(y)
    point = QPoint(x, y)
    painter.drawEllipse(point, 5, 5)
    painter.drawText(QPoint(x + 10, y), label)
def mousePressEvent(self, event):
    '''
    鼠标按下
    :param event:
    :return:
    '''
    x = event.x()
    y = event.y()
    self.press_x = x - self.center_x
    self.press_y = y - self.center_y
    # 主动绘制
    # update 自动调用paintEvent
    self.update()
def mouseReleaseEvent(self, event):
    '''
    鼠标松开
    :param event:
    :return:
    '''
    x = event.x()
    y = event.y()
    # 修改腿的坐标
    self.foot_x = x - self.center_x
    self.foot_y = y - self.center_y
    try:
        self.results = self.inverse(self.foot_x, self.foot_y)
    except:
        return
        # 主动绘制
    self.update()

完整代码

import sys
from PyQt5.QtWidgets import QApplication, QWidget
import math
from PyQt5.QtCore import QPoint
from PyQt5.QtGui import QPainter, QColor, QPen, QFont
class MainWindow(QWidget):
    def __init__(self):
        super().__init__()
        # 设置标题
        self.setWindowTitle('机器狗绘制')
        # 设置窗口大小
        self.setFixedSize(640, 480)
        # -------------- 中心点坐标 -------------- #
        self.center_x = 320
        self.center_y = 100
        # 大小的缩放系数
        self.ratio = 4
        # -------------- 已知狗腿数据 -------------- #
        self.AF = 12.5 * self.ratio
        self.AB = 21.5 * self.ratio
        self.EF = 21.5 * self.ratio
        self.CE = 21.5 * self.ratio
        self.BC = 34.19 * self.ratio
        self.CD = 34.95 * self.ratio
        # BC和CD的夹角
        self.bcd_angle = math.radians(120)
        self.half_AF = self.AF / 2
        # -------------- 各个端点坐标 -------------- #
        self.AX = -self.half_AF
        self.AY = 0
        self.FX = self.half_AF
        self.FY = 0
        self.BX = 0
        self.BY = 0
        self.CX = 0
        self.CY = 0
        self.EX = 0
        self.EY = 0
        # -------------- 足端坐标 -------------- #
        self.DX = 0
        self.DY = 60 * self.ratio
        # -------------- 按下的坐标 -------------- #
        self.press_x = self.DX
        self.press_y = self.DY
        # -------------- alpha和beta -------------- #
        self.alpha = 0
        self.beta = 0
        # -------------- 开始逆解 -------------- #
        self.results = self.inverse(self.DX, self.DY)
    def inverse(self, fx, fy):
        # AD长度
        AD2 = (self.half_AF + fx) ** 2 + fy ** 2
        AD = math.sqrt(AD2)
        # DAF夹角
        angle_DAF = math.asin(fy / AD)
        # BD长度
        BD2 = self.BC ** 2 + self.CD ** 2 - 2 * self.BC * self.CD * math.cos(self.bcd_angle)
        BD = math.sqrt(BD2)
        # BAD夹角
        angle_BAD = math.acos((self.AB ** 2 + AD2 - BD2) / (2 * self.AB * AD))
        # AB对应的alpha夹角
        alpha = math.pi - angle_BAD - angle_DAF
        angle_DBA = math.acos((self.AB ** 2 + BD2 - AD2) / (2 * self.AB * BD))
        angle_DBC = math.acos((self.BC ** 2 + BD2 - self.CD ** 2) / (2 * self.BC * BD))
        angle_ABC = angle_DBA - angle_DBC
        # AC长度
        AC2 = self.AB ** 2 + self.BC ** 2 - 2 * self.AB * self.BC * math.cos(angle_ABC)
        AC = math.sqrt(AC2)
        angle_BAC = math.acos((self.AB ** 2 + AC2 - self.BC ** 2) / (2 * self.AB * AC))
        angle_CAF = math.pi - alpha - angle_BAC
        CF2 = self.AF ** 2 + AC2 - 2 * self.AF * AC * math.cos(angle_CAF)
        CF = math.sqrt(CF2)
        angle_AFC = math.acos((self.AF ** 2 + CF2 - AC2) / (2 * self.AF * CF))
        angle_CFE = math.acos((CF2 + self.EF ** 2 - self.CE ** 2) / (2 * CF * self.EF))
        beta = math.pi - angle_AFC - angle_CFE
        # 计算各个点的坐标
        self.BX = -(self.AB * math.cos(alpha) + self.half_AF)
        self.BY = self.AB * math.sin(alpha)
        self.CX = math.cos(angle_CAF) * AC - self.half_AF
        self.CY = AC * math.sin(angle_CAF)
        self.DX = fx
        self.DY = fy
        self.EX = self.half_AF + self.EF * math.cos(beta)
        self.EY = self.EF * math.sin(beta)
        self.alpha = alpha
        self.beta = beta
        return alpha, beta
    def paintEvent(self, event):
        # 创建画家
        painter = QPainter(self)
        # 设置画笔中心
        painter.translate(self.center_x, self.center_y)
        painter.setRenderHint(QPainter.Antialiasing, True)
        # 创建画笔
        pen = QPen()
        # 设置画笔颜色
        pen.setColor(QColor(255, 0, 0))
        pen.setWidth(3)
        # 设置画笔
        painter.setPen(pen)
        # 绘制
        painter.drawLine(self.AX, self.AY, self.FX, self.FY)
        pen.setColor(QColor(0, 255, 0))
        painter.setPen(pen)
        painter.drawLine(self.AX, self.AY, self.BX, self.BY)
        pen.setColor(QColor(0, 0, 255))
        painter.setPen(pen)
        painter.drawLine(self.BX, self.BY, self.CX, self.CY)
        pen.setColor(QColor(0, 0, 255))
        painter.setPen(pen)
        painter.drawLine(self.CX, self.CY, self.DX, self.DY)
        pen.setColor(QColor(255, 0, 255))
        painter.setPen(pen)
        painter.drawLine(self.CX, self.CY, self.EX, self.EY)
        pen.setColor(QColor(255, 255, 0))
        painter.setPen(pen)
        painter.drawLine(self.EX, self.EY, self.FX, self.FY)
        # -------------- 绘制点的label -------------- #
        # 设置刷子
        painter.setBrush(QColor(255, 0, 0))
        painter.drawEllipse(self.press_x - 2, self.press_y - 2, 4, 4)
        radius = 4
        pen.setColor(QColor(255, 0, 0))
        pen.setWidth(5)
        font = QFont()
        font.setBold(True)
        font.setPixelSize(30)
        painter.setFont(font)
        # 设置画笔
        painter.setPen(pen)
        self.draw_point_label(painter, self.AX, self.AY, "A")
        self.draw_point_label(painter, self.BX, self.BY, "B")
        self.draw_point_label(painter, self.CX, self.CY, "C")
        self.draw_point_label(painter, self.DX, self.DY, "D")
        self.draw_point_label(painter, self.EX, self.EY, "E")
        self.draw_point_label(painter, self.FX, self.FY, "F")
        font.setPixelSize(20)
        painter.setFont(font)
        painter.drawText(QPoint(self.DX + 10, self.DY + 10), f"x={self.DX:.2f}.y={self.DY:.2f}")
        # painter.translate(-self.center_x,-self.center_y)
        painter.drawText(QPoint(self.AX - 200, self.AY - 30), f"alpha:{math.degrees(self.alpha):.2f}")
        painter.drawText(QPoint(self.FX + 100, self.FY - 30), f"beta:{math.degrees(self.beta):.2f}")
    def draw_point_label(self, painter, x, y, label):
        '''绘制点的标题'''
        x = int(x)
        y = int(y)
        point = QPoint(x, y)
        painter.drawEllipse(point, 5, 5)
        painter.drawText(QPoint(x + 10, y), label)
    def mousePressEvent(self, event):
        '''
        鼠标按下
        :param event:
        :return:
        '''
        x = event.x()
        y = event.y()
        self.press_x = x - self.center_x
        self.press_y = y - self.center_y
        # 主动绘制
        # update 自动调用paintEvent
        self.update()
    def mouseReleaseEvent(self, event):
        '''
        鼠标松开
        :param event:
        :return:
        '''
        x = event.x()
        y = event.y()
        # 修改腿的坐标
        self.foot_x = x - self.center_x
        self.foot_y = y - self.center_y
        try:
            self.results = self.inverse(self.foot_x, self.foot_y)
        except:
            return
            # 主动绘制
        self.update()
if __name__ == '__main__':
    app = QApplication(sys.argv)
    window = MainWindow()
    window.show()
    # window.autoAnimation2()
    sys.exit(app.exec_())