Example 10: Visualization and practice of inverse solution of kinematics of quadruped robot

Example 10: Visualization of single leg of inverse kinematics solution of quadruped robot

Purpose

Understand whether there are solutions and whether there are multiple solutions in inverse kinematics.
Understand the solution of inverse kinematic solutions.
Familiar with geometric and algebraic methods of solving inverse kinematics.
Be familiar with the simple calibration of single-leg servos.
Master methods for visualizing inverse kinematics calculation results.

Experimental requirements

Assemble the legs of a mini pupper.
Run the program to visually observe the multiple solutions and solution methods of the inverse kinematics solution.
Perform a simple calibration of the single-leg servo.
Observe the hardware operation of the inverse kinematics solution.

experimental knowledge

1. What is Inverse Kinematics?

Forward kinematics explores the known joint angle $\theta_i$ , solving tool coordinate system ${H\}$ or $^{World}P$ question.
Inverse kinematics explores the known tool coordinate system ${H\}$ The position and attitude of ${$ $H$ $}$ $^{World}P$ $\theta_i$ that meets the requirements $i_{i}$ The problem.
The presence or absence of a solution to the kinematic equation defines the work space . If there is a solution, it means that the robot arm can reach the target point. If there is no solution, it means that the robot arm cannot reach this point. The target point is outside the work space.
Basic inverse kinematics can be seen as the problem of calculating the position and attitude of the end effector of the manipulator, and calculating all joint angles that can reach the given position and attitude. It can be considered as the transformation of the robot's posture from Cartesian space to joint space. "Position" mapping.

2. What are closed solutions and numerical solutions?

Inverse kinematics is not as easy as forward kinematics. Inverse kinematics is nonlinear, and it is difficult to find a closed solution. Sometimes there is no solution, and sometimes there are problems with multiple solutions. This nonlinear system of transcendental equations has no rules. A unified and universal solution method. The solution method is divided into closed solution method and numerical solution method.
The closed solution is derived from the mathematical formula. For any independent variable, the corresponding dependent variable can be found. The calculation amount may be relatively large and the accuracy is high.
The numerical solution can be simulated by methods such as discrete table lookup or interpolation to simulate the final situation. The amount of calculation is relatively small and the accuracy is relatively poor.
Therefore, we need to consider the solution of inverse kinematics according to the actual situation.

2. Does the solution exist?

In inverse kinematics (IK), we can calculate the angle θ \theta that the robot's arm joint should rotate by giving the coordinates of a point relative to the world coordinate system (Frame World $θ$ .
If a given position is far away and completely out of reach of the robot's arms, then the solution is meaningless, so we will use the workspace to describe the area that the robot can reach.

Workspace

The working space is the range of positions that can be reached by the end of the arm. The specified target point must be within the workspace for the inverse kinematics solution to be meaningful.
In order to further describe the workspace, the following two common representations of the workspace can be used:
Reachable workspace Reachable workspace
is the range of positions that the arm can reach using one or more postures. The reachable target coordinate system can describe the position of this Frame relative to the world coordinate system, and a series of reachable target coordinate systems constitute the reachable workspace.
Dexterous workspace
is a position that can be reached at the end of the arm in any posture. The conditions are quite harsh, such as the center of the circle of the swing arm L1=L2 in the RR manipulator model of planar 2DOFs. In this model, this point is the only one that can be used for dexterous work. space. Smart workspaces are a subset of reachable workspaces.

3. Are there multiple solutions?

When solving kinematic equations, you often encounter situations where there is more than one solution. For example, a robot arm with three rotating joints in the plane, for the same point $P$ , these three rotating joints can have different configurations. Under different configurations, the reachable position and attitude of the actuator at the end of the arm can be the same.
Picture: Multiple diagrams of the three-link operating arm

Solution selection

There are some basic principles for selecting solutions:

fastest
Lowest energy consumption
avoid obstacles
Within the limits of the range of motion allowed by the joints

4. How to solve it?

Solving the kinematic equations of the manipulator is a nonlinear problem. There is no universal solution algorithm for the nonlinear equations. The algorithm needs to be formulated based on the model of the robot arm. If an algorithm can solve all joint variables related to the known pose, then the robot arm is solvable.
From ${Frame_{object}}$ 到 ${Frame_{World}}$ The transformation matrix $^W_oT$ can extract 16 numbers containing unknown numbers.
The rotation matrix is limited to only three degrees of freedom by the six conditions of xyz being perpendicular to each other and xyz being a unit vector. The three equations of the position vector component have three degrees of freedom. There are a total of 6 limiting conditions and 6 degrees of freedom. These equations are nonlinear transcendental equations and are difficult to solve.
For a six-rotating joint manipulator, a sufficient condition for the existence of an analytical solution (closed solution) is that the rotation axes of three adjacent joints intersect at one point.

$^0_6T = \left[ \begin{matrix} ^0_6R_{3x3} &^0P_{6{\kern 2pt}ORG{\kern 2pt}3x1}\\ 0{\kern 3pt}0{\kern 3pt}0&1 \end{matrix} \right]_{4x4}= \left[ \begin{matrix} \hat X_6\cdot \hat X_0& \hat Y_6\cdot \hat X_0 & \hat Z_6\cdot \hat X_0 & ^0_6P_{Xorg}\\ \hat X_6\cdot \hat Y_0& \hat Y_6\cdot \hat Y_0&\hat Z_6\cdot \hat Y_0 &^0_6P_{Yorg}\\ \hat X_6\cdot \hat Z_0& \hat Y_6\cdot \hat Z_0&\hat Z_6\cdot \hat Z_0 &^0_6P_{Zorg}\\ 0&0&0&1 \end{matrix} \right]$
For solutions based on analytical forms, common solution methods include geometric methods and algebraic methods. The two methods are similar, but the solution process is different.

geometric method

When solving the inverse kinematics problem of a manipulator using the geometric method, it is often necessary to convert the spatial geometric parameters into a plane geometry problem. at $\alpha_i=0 or ^+_-90°$ $\theta_i$ by applying common formulas in plane geometry and angle conversion. $i_{i}$ value.
Insert image description here

$x^2+y^2=l^2_1+l^2_2-2l_1l_2(\pi- \theta_2) \tag{Cosine theorem}$
$Cos\theta_2={x^2+y^2-l^2_1-l^ 2_2\over 2l_1l_2} \tag{Deformation 1}$
$Cos\psi={(x^2+y^2)+ l^2_1-l^2_2\over 2l_1\sqrt{x^2+y^2}} \tag{Deformation 2}$

$\theta_1= \begin{cases} for2(y,x)+\psi& \theta_2< 0\\ for2(y,x)-\psi& \theta_2>0\\ \end{cases}$
After calculating $\theta_2$ sum $\theta_1$ Finally, according to the geometric angle relationship of the figure, $\theta_3 can be calculated$ $\theta_n$ of successfully inverse solution of kinematics $i_{n}$

math.atan2() method

The math.atan2() method is a two-variable arctangent formula that can calculate the arctangent value of a given y and x value, which is an angle value expressed in radians on the slope line connecting the end point and the starting point of the segment.
atan2() is better than atan() because it can calculate the case x2-x1=0.
Reference link: Python math.atan2(y,x)
calculates the missing degrees of freedom in space

algebraic method

Apply link parameters ( $\alpha_{i-1}$ , $a_{i-1}$ , $\theta_{i}$ , $d_{i}$ ), the kinematic equation of the robotic arm can be obtained through the forward kinematic solution (FK), which is expressed in the form of a transformation matrix $^0_3T$ _ Therefore, the position of the target point is determined by the arm end coordinate system relative to the base coordinate system. When the research object is a planar robotic arm, you only need to know x, y, ϕ \ $ϕ$ can determine the target point position.
$\phi$ is the attitude angle of the end rod in the plane.
Take the known $^0_3T$ and the newly created $^{world}_{object}T$ , in the case of the solvent,
$\left[ \begin{matrix} c_{123} & -s_{123} & 0.0&l_1c_1+l_2c_{12 } \\s_{123}&c_{123}&0.0&l_1s_1+l_2s_{12}\\0.0&0.0&1.0&0.0\\0&0&0&1\\\end{matrix}\right]=\ left[\begin{matrix} c_{\phi}&-s_{\phi}&0.0&x\\s_{\phi}&c_{\phi}&0.0&y\\0.0&0.0&1.0&0.0\\ 0&0&0&1\\\end{matrix}\right]$
Using trigonometric functions and the angle formula
$Sin_{1 {^+_-}2}=Sin_1Cos_2 {^+_-} Cos_1Sin_2$
$Cos_{1 {^+_-}2}=Cos_1Cos_2 {^-_+}Sin_1Sin_2$
可得
$Cos\theta_2={x^2+y^2-l^2_1-l^2_2\over 2l_1l_2}$
Constant1 $1\geq Cos\theta_2\geq-1$ There is a solution when $1$

Assume that the target point is in the work space, and
$Sin\theta_2= {^+_-}\sqrt {1-c^2}$
Apply math.Atan2() mentioned in the geometric method to solve $\theta_2$ , use $\theta_2$ Then go to other $\theta_n$ To solve the problem, please refer to the textbook for specific methods. This article only introduces the algebraic method.
In layman's terms, it is to confirm $\theta_n$ S $Sin\theta_n$ 和 $Cos\theta_n$ , and then use the double-variable arctangent formula math.Atan2() to find $\theta_n$

Experimental steps

1. Multiple solutions and solutions of inverse kinematics

Run the program, observe the multiple solutions of the inverse kinematics solution, and observe the solution method of the inverse kinematics solution in the program.

sudo python rr_IK.py
# 示例值： 3 7

#!/usr/bin/python
# coding:utf-8
# rr_IK.py
# 逆向运动学IK
# mini pupper的简化单腿，可视作同一平面的RR类机械臂，可视化该机械臂，由给定末端位置计算转轴角度
import matplotlib.pyplot as plt  # 引入matplotlib
import numpy as np  # 引入numpy
from math import degrees, radians, sin, cos


# 几何法：端点坐标转关节角
def position_2_theta(x, y, l1, l2):
    """
    运动学逆解 将输入的端点坐标转化为对应的关节角
    :param x: p点坐标x值
    :param y: p点坐标y值
    :param l1: 大臂长
    :param l2: 小臂长
    :return: 关节角1值1 关节角1值2 关节角2值1 关节角2值1
    """
    cos2 = (x ** 2 + y ** 2 - l1 ** 2 - l2 ** 2) / (2 * l1 * l2)
    # print(cos2)
    sin2_1 = np.sqrt(1 - cos2 ** 2)
    sin2_2 = -sin2_1
    # print(sin2_1)
    # print("sin2有两值，分别为sin2_1=%f, sin2_2=%f" % (sin2_1, sin2_2))  # 若考虑关节情况也可只取一个正值
    theta2_1 = np.arctan2(sin2_1, cos2)
    theta2_2 = np.arctan2(sin2_2, cos2)
    phi_1 = np.arctan2(l2 * sin2_1, l1 + l2 * cos2)
    phi_2 = np.arctan2(l2 * sin2_2, l1 + l2 * cos2)
    theta1_1 = np.arctan2(y, x) - phi_1
    theta1_2 = np.arctan2(y, x) - phi_2
    # print(degrees(theta1_1), degrees(theta1_2), degrees(theta2_1), degrees(theta2_2))
    return theta1_1, theta1_2, theta2_1, theta2_2


def preprocess_drawing_data(theta1, theta2, l1, l2):
    """
    处理角度数据,转化为matplotlib适应的绘图格式
    :param theta1: 角度数据1
    :param theta2: 角度数据2
    :param l1: 杆件长1
    :param l2: 杆件长2
    :return: 绘图数据x坐标list和对应的y坐标list
    """
    xs = [0]
    ys = [0]
    # 分别算出x1 y1和x2 y2
    x1 = l1 * cos(theta1)
    y1 = l1 * sin(theta1)
    x2 = x1 + l2 * cos(theta1 + theta2)
    y2 = y1 + l2 * sin(theta1 + theta2)
    xs.append(x1)
    xs.append(x2)
    ys.append(y1)
    ys.append(y2)
    return xs, ys


def annotate_angle(x0, y0, rad1, rad2, name, inverse=False):
    """
    为两条直线绘制角度
    :param x0: 圆心x坐标
    :param y0: 圆心x坐标
    :param rad1: 起始角
    :param rad2: 终止角
    :param name: 角名
    :param inverse: 用于解决点1的重叠问题
    :return: 无
    """
    theta = np.linspace(rad1, rad2, 100)  # 0~rad
    r = 0.2  # circle radius
    x1 = r * np.cos(theta) + x0
    y1 = r * np.sin(theta) + y0
    plt.plot(x1, y1, color='red')
    plt.scatter(x0, y0, color='blue')
    degree = degrees((rad2 - rad1))
    if inverse:
        plt.annotate("%s=%.1f°" % (name, degree), [x0, y0], [x0 - r / 1.5, y0 - r / 1.5])
    else:
        plt.annotate("%s=%.1f°" % (name, degree), [x0, y0], [x0 + r / 1.5, y0 + r / 1.5])


# 关节信息
# 大臂长度：5 cm 小臂长度：7.5 cm
link_length = [5, 7.5]  # in cm
# 输入末端位置
position_pre = input("请输入末端的x坐标和y坐标，以空格隔开：")
position = [float(n) for n in position_pre.split()]
print(position)

# 计算并预处理绘图数据
joints_angles = position_2_theta(position[0], position[1], link_length[0], link_length[1])
# print(joints_angles)
figure1 = preprocess_drawing_data(joints_angles[0], joints_angles[2], link_length[0], link_length[1])
figure2 = preprocess_drawing_data(joints_angles[1], joints_angles[3], link_length[0], link_length[1])
# print(figure1)
# print(figure2)

# 绘图
fig, ax = plt.subplots()  # 建立图像
plt.axis("equal")
ax.grid()
plt.plot(figure1[0], figure1[1], color='black', label='method 1')
plt.scatter(figure1[0], figure1[1], color='black')
plt.plot(figure2[0], figure2[1], color='red', label='method 2')
plt.scatter(figure2[0], figure2[1], color='blue')
ax.set(xlabel='X', ylabel='Y', title='mini pupper IK RR model')
plt.legend()
# 标注
annotate_angle(figure1[0][0], figure1[1][0], 0, joints_angles[0], "theta1_1")
annotate_angle(figure1[0][1], figure1[1][1], joints_angles[0], joints_angles[2]+joints_angles[0], "theta2_1")
annotate_angle(figure2[0][0], figure2[1][0], 0, joints_angles[1], "theta1_2", inverse=True)
annotate_angle(figure2[0][1], figure2[1][1], joints_angles[1], joints_angles[3]+joints_angles[1], "theta2_2")
plt.annotate("P(%d, %d)" % (position[0], position[1]), [figure1[0][2], figure1[1][2]],
             [figure1[0][2] + 0.1, figure1[1][2] + 0.1])
plt.tight_layout()
plt.show()

2.Visualization of inverse kinematics

Observation program to observe the movement of the mini pupper's legs through the inverse kinematic solution of the circular trajectory

#!/usr/bin/python
# coding:utf-8
# rr_IK_circle.py
# 逆向运动学IK
# mini pupper的简化单腿，可视作同一平面的RR类机械臂，可视化四足机器人逆运动学画圈
import matplotlib.pyplot as plt  # 引入matplotlib
import numpy as np  # 引入numpy
from math import degrees, radians, sin, cos
import matplotlib.animation as animation


# 几何法：端点坐标转关节角
def position_2_theta(x, y, l1, l2):
    """
    运动学逆解 将输入的端点坐标转化为对应的关节角
    :param x: p点坐标x值
    :param y: p点坐标y值
    :param l1: 大臂长
    :param l2: 小臂长
    :return: 关节角1值1 关节角1值2 关节角2值1 关节角2值1
    """
    cos2 = (x ** 2 + y ** 2 - l1 ** 2 - l2 ** 2) / (2 * l1 * l2)
    sin2_1 = np.sqrt(1 - cos2 ** 2)
    sin2_2 = -sin2_1
    theta2_1 = np.arctan2(sin2_1, cos2)
    theta2_2 = np.arctan2(sin2_2, cos2)
    phi_1 = np.arctan2(l2 * sin2_1, l1 + l2 * cos2)
    phi_2 = np.arctan2(l2 * sin2_2, l1 + l2 * cos2)
    theta1_1 = np.arctan2(y, x) - phi_1
    theta1_2 = np.arctan2(y, x) - phi_2
    return theta1_1, theta1_2, theta2_1, theta2_2


def preprocess_drawing_data(theta1, theta2, l1, l2):
    """
    处理角度数据,转化为matplotlib适应的绘图格式
    :param theta1: 角度数据1
    :param theta2: 角度数据2
    :param l1: 杆件长1
    :param l2: 杆件长2
    :return: 绘图数据x坐标list和对应的y坐标list
    """
    xs = [0]
    ys = [0]
    # 分别算出x1 y1和x2 y2
    x1 = l1 * cos(theta1)
    y1 = l1 * sin(theta1)
    x2 = x1 + l2 * cos(theta1 + theta2)
    y2 = y1 + l2 * sin(theta1 + theta2)
    xs.append(x1)
    xs.append(x2)
    ys.append(y1)
    ys.append(y2)
    return xs, ys


def animate_plot(n):
    # 生成圆轨迹
    circle_point = [2.696152422706633, -7.330127018922193]  # 圆周运动的圆心
    position = [0, 0]
    history_position_x = [0]
    history_position_y = [0]
    circle_r = 2
    theta = n * np.pi / 100
    position[0] = circle_point[0] + circle_r * np.cos(theta)
    position[1] = circle_point[1] + circle_r * np.sin(theta)

    # 计算并预处理绘图数据
    joints_angles = position_2_theta(position[0], position[1], link_length[0], link_length[1])
    figure1 = preprocess_drawing_data(joints_angles[0], joints_angles[2], link_length[0], link_length[1])

    # 轨迹追踪
    for i in range(0, (n % 200)+1):
        history_theta = ((n % 200) + 1 - i) * np.pi / 100
        history_position_x.append(circle_point[0] + circle_r * np.cos(history_theta))
        history_position_y.append(circle_point[1] + circle_r * np.sin(history_theta))

    # 画图
    p = plt.plot(figure1[0], figure1[1], 'o-', lw=2, color='black')
    p += plt.plot(history_position_x, history_position_y, '--', color='blue', lw=1)
    return p


# 关节信息
# 大臂长度：5 cm 小臂长度：7.5 cm
link_length = [5, 7.5]  # in cm

# matplotlib可视化部分
fig, ax = plt.subplots()  # 建立图像
plt.axis("equal")
plt.grid()
ax.set(xlabel='X', ylabel='Y', title='mini pupper IK RR model Circle Plot')
ani = animation.FuncAnimation(fig, animate_plot, interval=10, blit=True)
plt.show()

3. Calibrate the steering gear

Connect the assembled single-leg servo wires according to the wiring prompts in the program, and perform zero return calibration on servo 2 and servo 3.

#!/usr/bin/python
# coding:utf-8
# servo_calibrate.py
# 默认舵机全部回零，随后等待输入角度

import RPi.GPIO as GPIO
import time


def servo_map(before_value, before_range_min, before_range_max, after_range_min, after_range_max):
    """
    功能:将某个范围的值映射为另一个范围的值
    参数：原范围某值，原范围最小值，原范围最大值，变换后范围最小值，变换后范围最大值
    返回：变换后范围对应某值
    """
    percent = (before_value - before_range_min) / (before_range_max - before_range_min)
    after_value = after_range_min + percent * (after_range_max - after_range_min)
    return after_value


signal_ports = input("键入各舵机的信号端口号，以空格隔开,无输入按回车默认信号口为：32 33 35\n请输入：") or "32 33 35"
signal_ports = [int(n) for n in signal_ports.split()]
for i in range(0, len(signal_ports)):
    print("舵机%d对应的口为%d" % (i+1, signal_ports[i]))

GPIO.setmode(GPIO.BOARD)  # 初始化GPIO引脚编码方式
servo = [0, 0, 0]
servo_SIG = signal_ports  # PWM信号端
servo_VCC = [2, 4, 1]  # VCC端
servo_GND = [30, 34, 39]  # GND端
servo_freq = 50  # PWM频率
servo_width_min = 2.5  # 工作脉宽最小值
servo_width_max = 12.5  # 工作脉宽最大值
GPIO.setmode(GPIO.BOARD)  # 初始化GPIO引脚编码方式
for i in range(0, len(servo_SIG)):
    GPIO.setup(servo_SIG[i], GPIO.OUT)
    servo[i] = GPIO.PWM(servo_SIG[i], servo_freq)
    servo[i].start(0)
    servo[i].ChangeDutyCycle((servo_width_min + servo_width_max) / 2)  # 回归舵机中位
print("初始化回零完成，两秒后等待输入")
time.sleep(2)

# 为舵机指定位置
try:  # try和except为固定搭配，用于捕捉执行过程中，用户是否按下ctrl+C终止程序
    while 1:
        angles = input("如果你需要改变舵机角度，请依次为不同舵机输入0°-180°的角度值：\n")
        angles = [int(n) for n in angles.split()]

        for i in range(0, len(angles)):
            dc_trans = servo_map(angles[i], 0, 180, servo_width_min, servo_width_max)
            servo[i].ChangeDutyCycle(dc_trans)
            print("舵机%d已转动到%d°" % (i+1, angles[i]))
except KeyboardInterrupt:
    pass
for i in range(0, len(servo_SIG)):
    servo[i].stop()  # 停止pwm
GPIO.cleanup()  # 清理GPIO引脚

4. Observe the actual operation of the inverse kinematics solution

The actual operation of the inverse kinematic solution will be interfered by many factors, such as calibration conditions, measurement errors between rods, transmission fluctuations in signal cables, and the computing power of the Raspberry Pi itself.
It is worth mentioning that the servo calibration situation is different for everyone. For single-legged servos, the calibration procedures mentioned in 3 can only help you find simple installation errors if necessary. For actual calibration, you need to run the visual calibration code of the entire machine.

If you want to visualize the operation of the hardware, although the mini pupper does not have foot feedback, you can also restore the comments in the drawing part of the code, which will synchronize the servo movement and computer drawing, which requires a lot of computing power. If it is higher, lag may occur.

#!/usr/bin/python
# coding:utf-8
# rr_IK_circle_synchronous.py
# 运动学逆解画圆，同步控制端图像显示和硬件运动
import matplotlib.pyplot as plt  # 引入matplotlib
import numpy as np  # 引入numpy
from math import degrees, radians, sin, cos
import matplotlib.animation as animation
import time
import RPi.GPIO as GPIO


# 几何法：端点坐标转关节角
def position_2_theta(x, y, l1, l2):
    """
    运动学逆解 将输入的端点坐标转化为对应的关节角
    :param x: p点坐标x值
    :param y: p点坐标y值
    :param l1: 大臂长
    :param l2: 小臂长
    :return: 关节角1值1 关节角1值2 关节角2值1 关节角2值1
    """
    cos2 = (x ** 2 + y ** 2 - l1 ** 2 - l2 ** 2) / (2 * l1 * l2)
    sin2_1 = np.sqrt(1 - cos2 ** 2)
    sin2_2 = -sin2_1
    theta2_1 = np.arctan2(sin2_1, cos2)
    theta2_2 = np.arctan2(sin2_2, cos2)
    phi_1 = np.arctan2(l2 * sin2_1, l1 + l2 * cos2)
    phi_2 = np.arctan2(l2 * sin2_2, l1 + l2 * cos2)
    theta1_1 = np.arctan2(y, x) - phi_1
    theta1_2 = np.arctan2(y, x) - phi_2
    return theta1_1, theta1_2, theta2_1, theta2_2


def servo_map(before_value, before_range_min, before_range_max, after_range_min, after_range_max):
    """
    功能:将某个范围的值映射为另一个范围的值
    参数：原范围某值，原范围最小值，原范围最大值，变换后范围最小值，变换后范围最大值
    返回：变换后范围对应某值
    """
    percent = (before_value - before_range_min) / (before_range_max - before_range_min)
    after_value = after_range_min + percent * (after_range_max - after_range_min)
    return after_value


def theta_to_servo_degree(theta, servo_number, relation_list, config_calibration_value=None):
    """
    将杆件的角度转化为舵机角度
    :param theta: 弧度制 杆件角度
    :param servo_number: 舵机号
    :param relation_list: 舵机关系映射表
    :param config_calibration_value:
    :return:舵机角 in 角度制
    """
    if config_calibration_value is None:
        config_calibration_value = [0, 0, 0]
    theta = degrees(theta)
    servo_degree = 0
    if servo_number == 1:
        #print("servo1")
        servo_degree = 0  # 此处需要根据舵机1修改
    elif servo_number == 2:
        #print("servo2")
        servo_degree = theta + relation_list[1] + config_calibration_value[1]
    elif servo_number == 3:
        #print("servo3")
        servo_degree = theta + relation_list[2] + config_calibration_value[2]
    else:
        #print("ERROR:theta_to_servo_degree")
        servo_degree = 0
    return servo_degree


def servo_control(servo_number, degree):
    """
    通过角度值控制电机输出对应的角度
    :return:
    """
    dc_trans = servo_map(degree, 0, 180, servo_width_min, servo_width_max)
    servo[servo_number - 1].ChangeDutyCycle(dc_trans)
    print("舵机%d已转动到%f°处" % (servo_number, degree))


def circle_point_generate(center_point, radius, frame):
    """
     输入圆心[x0,y0]，半径r，及计数c，返还圆周上单个点的坐标[x,y]
    :param center_point: 圆心[x0,y0]
    :param radius: 半径
    :param frame: 切分的样本点个数
    :return: 圆周上单个点的坐标[x,y]组成的两个array的list
    """
    theta = np.linspace(0, 2 * np.pi, frame)
    xs = center_point[0] + radius * np.cos(theta)
    ys = center_point[1] + radius * np.sin(theta)
    return xs, ys


def preprocess_drawing_data(theta1, theta2, l1, l2):
    """
    处理角度数据,转化为matplotlib适应的绘图格式
    :param theta1: 角度数据1
    :param theta2: 角度数据2
    :param l1: 杆件长1
    :param l2: 杆件长2
    :return: 大小腿点位x坐标数组和y坐标数组
    """
    xs = [0]
    ys = [0]
    # 分别算出x1 y1和x2 y2
    x1 = l1 * cos(theta1)
    y1 = l1 * sin(theta1)
    x2 = x1 + l2 * cos(theta1 + theta2)
    y2 = y1 + l2 * sin(theta1 + theta2)
    xs.append(x1)
    xs.append(x2)
    ys.append(y1)
    ys.append(y2)
    return xs, ys


def animation_update(frame):
    """
    更新动画并同步到硬件电机
    注意：matplotlib在线动画锁帧在30fps，frame不得高于30
    :param frame:
    :return:
    """
    # 硬件舵机运动同步
    servo_control(1, servo_degree[0][frame])
    servo_control(2, servo_degree[1][frame])

    # 更新circle绘画
    circle_artist.set_xdata(xs[0:frame])  # 直接设置x
    circle_artist.set_ydata(ys[0:frame])  # 直接设置y
    # 更新leg绘画
    leg_artist.set_xdata(leg_data_xs[frame])  # 直接设置x
    leg_artist.set_ydata(leg_data_ys[frame])  # 直接设置y
    return circle_artist, leg_artist


# 配置及初始化
center_point = [1.767767, -8.838835]  # 圆周运动的圆心
radius = 2  # 圆半径
frame = 60  # 切分样本数
leg_data_xs=[]#腿部各个点位数据x
leg_data_ys=[]#腿部各个点位数据y
position = [0,0]  # 传给舵机的位置
link_length = [5, 7.5]  # 杆件长度 in cm
config_degree_relation_list = [+0, +225, +0]
servo = [0, 0, 0]
servo_degree = [[],[]] #舵机数据表
servo_SIG = [32, 33]  # PWM信号端
servo_VCC = [2, 4, 1]  # VCC端
servo_GND = [30, 34, 39]  # GND端
servo_freq = 50  # PWM频率
servo_width_min = 2.5  # 工作脉宽最小值
servo_width_max = 12.5  # 工作脉宽最大值
GPIO.setmode(GPIO.BOARD)  # 初始化GPIO引脚编码方式
for i in range(0, len(servo_SIG)):
    GPIO.setup(servo_SIG[i], GPIO.OUT)
    servo[i] = GPIO.PWM(servo_SIG[i], servo_freq)
    servo[i].start(0)
    servo[i].ChangeDutyCycle((servo_width_min + servo_width_max) / 2)  # 回归舵机中位


# 圆轨迹生成
xs, ys = circle_point_generate(center_point, radius, frame)
# 腿部轨迹生成
for i in range(0,frame):
    position[0] = xs[i]
    position[1] = ys[i]
    # 获取运动学逆解值
    joints_angles = position_2_theta(position[0], position[1], link_length[0], link_length[1])
    # 逆解值转绘图数据
    leg_data_pre = preprocess_drawing_data(joints_angles[0], joints_angles[2], link_length[0], link_length[1])
    leg_data_xs.append(leg_data_pre[0])
    leg_data_ys.append(leg_data_pre[1])
    # 杆件角度转舵机角度
    servo_degree[0].append(theta_to_servo_degree(joints_angles[0], 2, config_degree_relation_list))
    servo_degree[1].append(theta_to_servo_degree(joints_angles[2], 3, config_degree_relation_list))
    
print("初始化完成，1秒后等待操作")
time.sleep(1)
# matplotlib可视化部分
fig, ax = plt.subplots(figsize=(6,6))  # 建立图像
plt.axis("equal")
plt.grid()
circle_artist = ax.plot(xs[0], ys[0], '--', lw=2, color='blue')[0]
leg_artist = ax.plot(leg_data_xs[0], leg_data_ys[0], 'o-', lw=2, color='black')[0]
ax.set(xlim=[-6,7],ylim=[-12,1],xlabel='X', ylabel='Y', title='mini pupper IK RR model Circle Plot')
#plt.tick_params(axis="both")
ani = animation.FuncAnimation(fig=fig, func=animation_update, frames=frame, interval=1,blit=True)  # 设置动画，interval单位为ms

plt.show()
plt.clf("all")
for i in range(0, len(servo_SIG)):
    servo[i].stop()  # 停止pwm
GPIO.cleanup()  # 清理GPIO引脚

Experiment summary

After studying and experimenting with this knowledge point, you should be able to reach the following levels:

Knowledge points	content	learn	familiar	master
inverse kinematics	Whether the inverse solution of kinematics has any solution and whether there are multiple solutions	✔
inverse kinematics	Solving the inverse solution of kinematics	✔
inverse kinematics	Geometry and Algebra		✔
hardware	Simple calibration of single leg servo		✔
Visualization	Dynamic visualization of kinematics calculation results			✔

Copyright information: The teaching materials are not yet complete. Copyright information processing methods are reserved here.
Mini pupper related content can be accessed: https://github.com/mangdangroboticsclub