Example 9: Visualization of the positive solution of kinematics of a quadruped robot in plane RR single leg

Example 9: Single-leg visualization of forward kinematics of a quadruped robot

Purpose

1. Through hands-on practice, build the legs of the mini pupper quadruped robot and master the single-leg structure of the robot.
2. Through theoretical study, you will become familiar with the use of geometric methods and rotation matrix methods in the forward solution of kinematics (FK).
3. Through programming practice, become familiar with the method of visually simplifying a single leg of a robot.

Experimental requirements

Through Python programming, in the simplified mini pupper plane two-link model, the user is allowed to input joint angles. Based on the joint angles and basic information of the joints, the coordinates of each joint relative to the base coordinate system are calculated and output visually.

experimental knowledge

1. What is Forward Kinematics?

In the actual movement operation of the mini pupper, it is necessary to consider the static geometric problem of calculating the position and posture relationship of the lower leg end relative to the body base coordinate system. To put it simply, given a set of joint angle values, calculate the pose of the calf end coordinate system relative to the base coordinate system.
The problem of forward kinematics exploration can be seen as joint space $(i_1,i_2,\cdot \cdot \cdot \cdot ,i_n)$ described to Cartesian Space Cartesian Space${（}^{World}P)$describes the position representation.
Exploring the representation relationship of this position is forward kinematics. For robots, it uses the joint variables of the operating arm as independent variables to describe the functional relationship between the posture of the end effector of the operating arm and the base coordinate system.

2. How to describe a leg of a robot?

Since we need to consider the calculation of the position and attitude relationship of the lower leg end relative to the body base coordinate system during the actual movement of the mini pupper, there should be corresponding qualitative and quantitative descriptions and representations to enable the calculation.
In robotics, the operating arm is composed of links . The links are regarded as rigid bodies. Adjacent links are connected by joints . The joints can relatively rotate (revolute) or move (prismatic) . The axis of rotation is called Axis .
This description of the operating arm corresponds to the human body and can be regarded as a simplified human arm. The "big arm" is a connecting rod, connected to another connecting rod "small arm" through the "elbow joint", and the mini pupper is also Same reason.

The rotation and movement of joints lead to two mathematical quantities. For rotating joints, the relative angle change between adjacent members is called joint angle , while for moving joints, the relative displacement change between adjacent members is called joint deflection . Shift amount .

For the manipulator, the number of degrees of freedom is the number of independent position variables. Common industrial robots are mostly open kinematic chains. Each joint position is defined by a unique variable, so the number of joints is often equal to the number of degrees of freedom. quantity.

The end of the operating arm is called the end effector, such as a fixture, a welding gun, etc. The operating arm is usually described by the relative relationship between the tool coordinate system Frame_Tool set on the end effector and the reference base coordinate system Frame_Base of the fixed base of the operating arm. position and posture.

3. How to describe the relationship between the parts between the legs?

In order to describe the relationship between adjacent connecting rods, four variables are introduced to describe the spatial relationship of the connecting rods.

name	symbol	meaning	Comment
Connecting rod length	$a_{i-1}$	The length of the common perpendicular between two adjacent joint axes	The length of the connecting rod is along ${\hat{X}}_{i-1}$direction, ${\hat{Z}}_{i-1}$和${\hat{Z}}_i$The length of the common perpendicular line between. Because $a_{i-1}$is the distance, so there is always $a_{i-1}\ge 0$。
Connecting rod torsion angle	$a_{i-1}$	Alongside $a_{i-1}$The angle between the two joint axes in the projection plane of the direction	The connecting rod torsion angle is along ${\hat{X}}_{i-1}$Z ^ i − 1 \hat Z_{i-1} in the two-dimensional plane formed by looking in the direction${\hat{Z}}_{i-1}$和${\hat{Z}}_i$The angle between , according to the right-hand method from ${\hat{Z}}_{i-1}$扭向${\hat{Z}}_i$Take plus or minus.
Connecting rod offset	$d_i$	The distance along the common axis of two adjacent links.	Alongside ${\hat{Z}}_i$Looking in the direction, for ${\hat{X}}_{i-1}$与${\hat{Z}}_i$The intersection point of the straight line and ${\hat{X}}_i$与${\hat{Z}}_i$The intersection point of the straight lines and the distance between the two intersection points.
joint angle	$i_i$	The angle of rotation of two adjacent links about a common axis.	Alongside ${\hat{Z}}_i$Looking in the direction, X ^ i − 1 \hat X_{i-1} in the two-dimensional plane${\hat{X}}_{i-1}$和${\hat{X}}_i$The angle between , the direction is ${\hat{X}}_{i-1}$扭向${\hat{X}}_i$。

The connecting rod torsion angle and connecting rod length define the relationship between the two joint axes of the same member, and the joint angle and connecting rod offset define how the connecting rods are connected. X ^ i \hat X_{i}
mentioned in the comments above${\hat{X}}_i$、${\hat{Y}}_i$、${\hat{Z}}_i$It will be mentioned later in conjunction with the connecting rod coordinate system and can be understood in conjunction.

4.Denavit-Hartenberg expression

Abbreviated as DH expression method, the version is Craig version. DH method is a rule that uses four connecting rod parameters to describe the motion relationship of the mechanism. Two parameters are used to describe the connecting rod itself, and the other two parameters are used to describe the relationship between the connecting rods. connection relationship. These four parameters are the connecting rod torsion angle α i − 1 \alpha_{i-1}
mentioned before$a_{i-1}$, connecting rod length $a_{i-1}$, the angle $i_i$, connecting rod offset $d_i$.
Generally speaking, for a rotating joint, the joint variable is the joint angle $i_i$, for moving joints, the joint variable is the connecting rod offset $d_i$.
For this type of joint, except for the joint variables, the other three link parameters are fixed hardware attribute parameters.

5. Connecting rod coordinate system

In order to describe the relationship between connecting rods, a coordinate system is introduced, so a connecting rod coordinate system needs to be established on each connecting rod. For connecting rod $i-1$ , on which there is a connecting rod coordinate system{ i − 1 } \{i-1\}{ i−1 } , in Craig's improved DH method, the origin of the link coordinate system is usually specified at the joint axis$Axi{s}_{(i-1)}$superior.
According to the provisions of this coordinate system, the orthogonal axes of the fixed link coordinate system are as follows:

axis or point	definition
origin	Public service line $a_i$with joint axis $Axi{s}_i$intersection point
${\hat{Z}}_i$	with joint axis $Axi{s}_i$coincide
${\hat{X}}_i$	along the common perpendicular $a_i$by joint $join{t}_i$Point to joint $join{t}_{i+1}$direction. When the common vertical line $a_i=0$时，${\hat{X}}_i$Perpendicular to ${\hat{Z}}_i$与 ${\hat{Z}}_{i+1}$The formed plane can have two directions, which can be selected according to convenience.
${\hat{Y}}_i$	Determined according to the right-hand rule, the thumb is known to be ${\hat{X}}_i$, the index finger is ${\hat{Y}}_i$, it is known that the middle finger is ${\hat{Z}}_i$

Special existence in the connecting rod coordinate system

First end connecting rod

The first link is called the ground rod, and the coordinate system on the fixed base of the robot is $\left\{0\right\}$ is often used as a reference coordinate system. ${\hat{Z}}_0$Direction and ${\hat{Z}}_1$The same, and when the coordinate system { 1 } \{1\} The joint variableθ i \theta_{i}$of\; \{$$1$$\}$$i_i$或$d_i$When is 0, make the coordinate system $\left\{0\right\}$ and coordinate system$\left\{1\right\}$ overlap to simplify calculations.

$$\text{Revolute joint}{a}_0=0a_0=0d_1=0i_1=?(Framecoincideswhen\; it\; is0)Moving$$
$$\text{Moving jointPrismatic joint}{a}_0=0a_0=0i_1=0d_1=?(Framecoincideswhen\; itis0)$$

end link

The end rod has no next rod, so it needs to be related to the previous rod.
For a rotating joint, ${\hat{X}}_n$Direction and ${\hat{X}}_{n-1}$Same, then choose to set the origin so that $d_n=0For$
moving joints, choose the setting${\hat{X}}_n$The direction is such that $i_n=0$ , when$d_n=When\; 0$ , the origin is located at joint axis$n$ and${\hat{X}}_{n-1}$intersection

After learning the connecting rod coordinate system, the connecting rod parameters can be understood more simply:

name	symbol	meaning
Connecting rod torsion angle	$a_{i-1}$	The connecting rod torsion angle is around ${\hat{X}}_{i-1}$轴，${\hat{Z}}_{i-1}$Rotate to ${\hat{Z}}_i$the angle of
Connecting rod length	$a_{i-1}$	The length of the connecting rod is along ${\hat{X}}_{i-1}$轴，${\hat{Z}}_{i-1}$Move to ${\hat{Z}}_i$The length of the common perpendicular
joint angle	$i_i$	绕${\hat{Z}}_i$轴，${\hat{X}}_{i-1}$Rotate to ${\hat{X}}_i$the angle of
Connecting rod offset	$d_i$	Alongside ${\hat{Z}}_i$轴，${\hat{X}}_{i-1}$Move to ${\hat{X}}_i$distance

Establish connecting rod coordinate system and DH table

1. Find the joint axes.
   Analyze the motion status and find the axes of joint translation or rotation (joint axes).
2. Z ^ i ${\hat{Z}}_i$
   ${\hat{Z}}_i$The positive direction is the axis direction of rotation or movement
3. Find ${\hat{X}}_i$
   ${\hat{X}}_i$The positive direction of is on the common vertical line between the two joint axes, pointing to the direction of the next joint axis
   ( $a_i=0$ , that is, when the two joint axes intersect,$X$ sum$Z_i$Sum $Z_{i+1}$The direction is vertical, whatever is convenient for the direction at this time)
4. Determine the origin.
   The origin is ${\hat{Z}}_i$与${\hat{X}}_i$intersection point
5. Y ^ i \hat Y_{i }${\hat{Y}}_i$
   According to the right-hand rule, the thumb is ${\hat{X}}_i$, the index finger is ${\hat{Y}}_i$, the middle finger is ${\hat{Z}}_i$
6. Process the first link
   and treat it as $Fram{e}_1$The joint variable $i_i$或$d_i$When 0, make $Fram{e}_0$和$Fram{e}_1$overlap to simplify calculations.
7. When dealing with the end connecting rod,
   try to make the connecting rod parameters equal to 0 to simplify the calculation.

Based on the results of the above steps, the DH parameter table can be obtained

The DH parameter table is the relationship between Frames. Each i represents the relationship between a pair of axes.
1 in the table is the base coordinate system $Fram{e}_0$Coordinate system with the first axis $Fram{e}_1$The relationship
3 in the table is the relationship between the end link and the last axis that can (rotate, translate)

i	$a_{i-1}$	$a_{i-1}$	$i_i$	$d_i$
1	0	0	$i_1$	0
2	0	$L_1$	$i_2$	0
3	0	$L_2$	$i_3$	0

6. Conversion between connecting rod coordinate systems

Frame_{i-1 }$Fram{e}_{i-1}$、 Essentially $Axi{s}_{i-1}$到$Axi{s}_i$.
First, complete the equation from ${\hat{Z}}_{i-1}$到${\hat{Z}}_i$Rotation $a_{i-1}$Peace transition $a_{i-1}$distance.

Then, rotate and translate $Axi{s}_{i-1}$Rotation $i_i$, and along ${\hat{Z}}_i$Direction translation $d_i$distance.
After a total of four operations, we finally get
$${}^{i-1}P{=}_i^{i-1}{T}^{i}P$$
${}_i^{i-1}$Given the equivalence of$T$
$${}_i^{i-1}T=\left[\begin{array}{cccc}Cos{\theta }_i& -Sin{\theta }_i& 0& a_{i-1}\\ Sin{\theta }_{i}Cos{\alpha }_{i-1}& Cos{\theta }_{i}Cos{\alpha }_{i-1}& -Sin{\alpha }_{i-1}& -Sin{\alpha }_{i-1}{d}_i\\ Sin{\theta }_{i}Sin{\alpha }_{i-1}& Cos{\theta }_{i}Sin{\alpha }_{i-1}& Cos{\alpha }_{i-1}& Cos{\alpha }_{i-1}{d}_i\\ 0& 0& 0& 1\end{array}\right]$$
Using this transformation matrix multiple times can obtain the transformation relationship between non-adjacent joints.

7.Simplified kinematic structure of mini pupper

The single leg of the mini pupper can be simplified as a typical three-link non-planar operating arm. The three-degree-of-freedom design allows it to theoretically reach any position within the space limit range.
When we further simplify and do not consider the lateral hip joint motion, the legs of the mini pupper can be simplified to a planar two-link mechanism RR model, and the geometric solution can be used at the known $i_1$Sum $i_2$Find 0 P ^0P in the case of${}^{The\; coordinates\; of\; 0}P$complete the positive solution of kinematics.
For the intersection of the big arm and the small arm$Q$：
$$x_q=l_{1}Cos{\theta }_1$$
$$y_q=l_{1}Sin{\theta }_1$$

Default function:
$$x_p=l_{1}Cos{\theta }_1+l_{2}Cos(i\_{}_1+i_2)$$
$$y_p=l_{1}Sin{\theta }_1+l_{2}Sin(i_1+i_2)$$

Picture: Planar two-link mechanism RR model

Experimental steps

1. Write Python code rr_FK.py

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


# 几何法：关节角转端点坐标
def theta_2_position_rr(l1, l2, theta1, theta2):
    """
    运动学正解 将输入的关节角转化为对应的端点坐标
    :param l1: 大臂长
    :param l2: 小臂长
    :param theta1: 大臂关节角
    :param theta2: 小臂关节角
    :return: 端点1坐标 端点2坐标
    """
    point_1 = [l1*cos(radians(theta1)), l1*sin(radians(theta1))]
    point_2 = [l1*cos(radians(theta1))+l2*cos(radians(theta1+theta2)),
               l1*sin(radians(theta1))+l2*sin(radians(theta1+theta2))]
    print(point_1)
    print(point_2)
    return point_1, point_2


def preprocess_drawing_data(points):
    """
    处理点坐标数据转化为matplotlib适应的绘图格式
    :param points: 点数据
    :return: 绘图数据x坐标list和对应的y坐标list
    """
    xs = [0]
    ys = [0]
    xs.append(points[0][0])
    xs.append(points[1][0])
    ys.append(points[0][1])
    ys.append(points[1][1])
    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.3  # 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 小臂长度：6 cm
link_length = [5, 6]  # in cm

# 关节角初始化
joints_angle_origin = [-150, 90]
joints_angle = [0, 0]
print("关节角初始状态 theta1=%d°， theta2=%d°" % (joints_angle_origin[0], joints_angle_origin[1]))

# 输入连杆参数：各关节角
for i in range(1, 3):
    joints_angle[i-1] = int(input("请输入腿部舵机[%d]要转动的的角度：" % i))
    print("腿部舵机[{0}]将转动{1}°".format(i, joints_angle[i-1]))

# 计算并预处理绘图数据
points_origin = theta_2_position_rr(link_length[0], link_length[1], joints_angle_origin[0], joints_angle_origin[1])
points_after = theta_2_position_rr(link_length[0], link_length[1], joints_angle[0], joints_angle[1])
data_origin = preprocess_drawing_data(points_origin)
data_after = preprocess_drawing_data(points_after)

# 绘图
fig, ax = plt.subplots()  # 建立图像
plt.plot(data_origin[0], data_origin[1], color='black', label='original')
plt.scatter(data_origin[0], data_origin[1], color='black')
plt.plot(data_after[0], data_after[1], color='red', label='after')
plt.scatter(data_after[0], data_after[1], color='blue')
ax.set(xlabel='X', ylabel='Y', title='mini pupper FK RR model')
ax.grid()
plt.axis("equal")
plt.legend(loc=2)

# 标注
annotate_angle(data_origin[0][0], data_origin[1][0],
               0, radians(joints_angle_origin[0]), "theta1_original", inverse=True)
annotate_angle(data_origin[0][1], data_origin[1][1], radians(joints_angle_origin[0]),
               radians(joints_angle_origin[0]+joints_angle_origin[1]), "theta2_original", inverse=True)
annotate_angle(data_after[0][0], data_after[1][0],
               0, radians(joints_angle[0]), "theta1_after")
annotate_angle(data_after[0][1], data_after[1][1], radians(joints_angle[0]),
               radians(joints_angle[0]+joints_angle[1]), "theta2_after")
plt.show()

2. Run the program and observe the effect

Execute the following commands in the directory of rr_FK.py:

sudo python rr_FK.py

At this point the visual coordinates calculated from the joint angles should be observed.

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
hardware	Construction of mini pupper single-leg structure			✔
Kinesiology	Correct kinematics solution of planar RR type manipulator			✔
Kinesiology	Non-planar RRR type robotic arm	✔
Kinesiology	Geometric method completes the correct solution of kinematics		✔
Kinesiology	Basic knowledge of correct solutions of kinematics		✔
Visualization	Visualizing kinematics in Python	✔

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