Robot Motion and Control

Introduction
Inverse Kinematic
Initial Poseture Calculation
Experiment
Result
1. Simulation Result
2. Real Results
Code
1. Main Flow
2. Read Gcode
Conclusion

Introduction

When calculating the poseture of the manipulator based on the initial coordinates, it is necessary to first calculate the DH parameters of the whole manipulator, and then calculate the value of ${ }^{i-1} T_{i}$ according to the calculated DH parameters. The coordinate system as shown in Fig. 1 is established according to the principle that the rotation axis of the joint axis of the manipulator is the Z-axis, and the X-axis of the $ i_{th}$ joint axis is perpendicular to the Z-axis of the $ {(i-1)}{th}$ and $ i{th}$ joint axes, as well as the right-handed coordinate system.

Inverse Kinematic

The inverse kinematics of the manipulator is to solve the angle of each joint of the manipulator according to the position and attitude of the end. Different from the forward kinematics, a set of joint angles corresponds to one end pose, and the inverse kinematics has the probability that a pose at the end of the robot corresponds to multiple joint angles, so constraint conditions need to be added in the process of solving.

Method 1

Method 2

The Classical DH parameters for the transformation between frames are defined as:

\[{ }^{i-1} T_{i}=\left[\begin{array}{cccc} \cos \left(\theta_{i}\right) & -\cos \left(\alpha_{i}\right) \sin \left(\theta_{i}\right) & \sin \left(\alpha_{i}\right) \sin \left(\theta_{i}\right) & a_{i} \cos \left(\theta_{i}\right) \\ \sin \left(\theta_{i}\right) & \cos \left(\alpha_{i}\right) \cos \left(\theta_{i}\right) & -\sin \left(\alpha_{i}\right) \cos \left(\theta_{i}\right) & a_{i} \sin \left(\theta_{i}\right) \\ 0 & \sin \left(\alpha_{i}\right) & \cos \left(\alpha_{i}\right) & d_{i} \\ 0 & 0 & 0 & 1 \end{array}\right]\]

The following table gives the Classical DH parameters for the robot.

\[\begin{array}{|c|c|c|c|c|} \hline i & \alpha_{i} & a_{i} & d_{i} & \theta_{i} \\ \hline 1 & \pi / 2 & 0.0 & (128.3+115.0) & q_{1} \\ \hline 2 & \pi & 280.0 & 30.0 & q_{2}+\pi/2 \\ \hline 3 & \pi/2 & 0.0 & 20.0 & q_{3}+\pi/2 \\ \hline 4 & \pi/2 & 0.0 & (140.0+105.0) & q_{4}+\pi/2 \\ \hline 5 & \pi/2 & 0.0 & (28.5+28.5) & q_{5} +\pi \\ \hline 6 & 0 & 0.0 & (105.0+130.0) & q_{6}+ \pi/2 \\ \hline \end{array}\]

The gripper should be parallel to the table so:

\[\theta_2 - \theta_3 + \theta_5 = \pi/2\] \[{}^{0}_{1} T {}^{-1} {}^{0}_{6}T = {}^{1}_{6}T\] \[\left[\begin{array}{cccc} c_1 & s_1 & 0 & 0\\ 0 & 0 & 1 & -0.2433\\ s_1 & -c_1 & 0 & 0\\ 0 & 0 & 0 & 1 \end{array}\right] \left[\begin{array}{cccc} r_{11} & r_{12} & r_{13} & p_x\\ r_{21} & r_{22} & r_{23} & p_y\\ r_{31} & r_{23} & r_{33} & p_z\\ 0 & 0 & 0 & 1 \end{array}\right] = \left[\begin{array}{cccc} { }^{1} r_{11} & { }^{1} r_{12} & { }^{1} r_{13} & { }^{1} p_{x} \\ { }^{1} r_{21} & { }^{1} r_{22} & { }^{1} r_{23} & { }^{1} p_{y} \\ { }^{1} r_{31} & { }^{1} r_{32} & { }^{1} r_{33} & { }^{1} p_{z} \\ 0 & 0 & 0 & 1 \end{array}\right]\]

where

\[\begin{aligned} { }^{1} p_{x} &= 0.245 cos(\theta_2 - \theta_3) + 0.28 s_2\\ { }^{1} p_{y} &= -0.245 sin(\theta_2 - \theta_3) + 0.28 c_2 + 0.235\\ { }^{1} p_{z} &= 0.067 \end{aligned}\]

Then we can get the equation with $\theta_1$.

\[s_1 p_x - c_1 p_y = 0.067\] \[\theta_1 = atan2(p_y,p_x)+atan2(0.067,\sqrt{p_x^2+p_y^2-0.067^2});\]

Now the $\theta_1$ is known, we can calculate $\theta_3$.

\[\begin{aligned} p_z - 0.2433 &= 0.245 cos(\theta_2 - \theta_3) + 0.28 s_2\\ c_1 p_x + s_1p_y &= -0.245 sin(\theta_2 - \theta_3) + 0.28 c_2 + 0.235\\ \end{aligned}\] \[K = \frac{(cos(\theta_1)p_x+sin(\theta_1)*p_y-0.235)^2+(p_z-0.2433)^2-0.245^2-0.28^2}{2*0.245*0.28}\\\] \[\theta_3 = atan2(K,-\sqrt{1-K^2})\]

With $p_z$ is known, we can get $\theta_2$

\[p_z = 0.245cos(\theta_2 - \theta_3) + 0.28sin(\theta_2) = p_z - 0.2433\] \[\begin{aligned} \theta_2 = &atan2(0.245*cos(\theta_3),-(0.245*sin(\theta_3)+0.28))\\ &-atan2(pz-0.2433,-\sqrt{(-(0.245*sin(\theta_3)+0.28))^2+(0.245*cos(\theta_3))^2-(pz-0.2433)^2}) \end{aligned}\]

Finally, we can calculate $\theta_5$

\[\theta_5 = \pi/2 - \theta_2 + \theta_3\]

Calculate IK with Python

def IK(self,px,py,pz):
    from math import atan2,sqrt,cos,sin
    from numpy import rad2deg, pi

    d3 = 0.067;
    theta1 = atan2(py,px)+atan2(d3,sqrt(px**2+py**2-d3**2));

    K = ((cos(theta1)*px+sin(theta1)*py-0.235)**2+(pz-0.2433)**2-0.245**2-0.28**2)/(2*0.245*0.28);
    theta3 = atan2(K,-sqrt(1-K**2));

    a = -(0.245*sin(theta3)+0.28);
    b = 0.245*cos(theta3);
    d = pz-0.2433;
    theta2 = atan2(b,a)-atan2(d,-sqrt(a**2+b**2-d**2));

    theta5 = pi/2 + theta3 - theta2;

    t1 = rad2deg(theta1) + 360
    t2 = rad2deg(theta2) - 90 +360
    t3 = rad2deg(theta3) - 90 + 360
    t4 = -90 +360
    t5 = rad2deg(theta5) - 180 + 360
    t6 = -90 + 180
    joints = [t1,t2,t3,t4,t5,t6]
    return joints

Initial Poseture Calculation

Before using the arm to write or draw, the arm needs to have the behavior of taking a pen. Therefore, in order to protect the manipulator from external interference, we need to calculate the appropriate posture of the manipulator to take the pen by using the inverse kinematics equation previously solved according to the position coordinates of the pen container.

Experiment

Get Word Coordinates

In the software Snapmaker, write out the word “SUSTech”, then export the corresponding gcode code, and extract the corresponding coordinates to write.

Take a pen

item According to the position and posture of the pen holder calculated above, the “go_to_pose()” function is used to control the movement of the manipulator to the specified coordinates.
The “example_send_gripper_command” function is used to clip the ring pen holder with speed feedback.

Writing

The X, Y and Z coordinates of “SUSTech” obtained previously are read into three arrays respectively, and then the “go_to_pose()” function is used to control the manipulator arm for related operations.
In the process of writing, the mechanical arm moves faster after each lift of the pen.After each stroke, the arm slows down relatively.

Put the pen into holder

After drawing “SUSTech”, input the position of the pen holder to function “go_to_pose()” and control the robotic arm to put the pen back into the pen holder.

Result

Simulation Result

The simulation experiment can be carried out normally. See the video for detailed results.

Real Results

In the real experiment, the arm was able to automatically pick up the pen, write, and put the pen back into the container. Compared to previous experiments, the robotic arm has been able to write the corresponding words very smoothly.

Code

Main Flow

from read_gcode import read_gcode
from robot_api import Robot_Api


def main():
    robot = Robot_Api("my_gen3_lite")
    robot.get_pose()
    robot.clear_faults()
    robot.example_subscribe_to_a_robot_notification()
    robot.example_set_cartesian_reference_frame()

    ## 读取gcode生成轨迹
    points = read_gcode("gcode.nc", 0.03, 0.006, 0.2, -0.2, 0.5)
    robot.gripper_open() ## 张开机械爪
    robot.example_retract_the_robot() ## 回到Retract位置
    joints = robot.IK(0.3, -0.4, 0.055)  
    robot.example_send_joint_angles(joints) ## 运动到抓笔姿态
    joints = robot.IK(0.36, -0.48, 0.055) 
    robot.example_send_joint_angles(joints) ## 前往笔的位置
    robot.example_send_gripper_command() ## 机械爪抓取
    robot.go_to_pose(0.3 * 1.2, -0.4 * 1.2, 0.2) ## 机械臂抬起
    robot.example_home_the_robot() ## 前往工作位置
    robot.go_to_pose(0.2, -0.2, 0.2, 90, 0, 100, True, 0.18) 
    robot.go_to_pose(0.2, -0.2, 0.03, 90, 0, 100, True, 0.18) ## 前往写字位置
    for points in points:  ## 写字
        robot.go_to_pose(points[0], points[1], points[2], 90, 0, 100, theta=True, speed=points[3]) 
    robot.go_to_pose(0.2, -0.2, 0.2, 90, 0, 100, True, 0.18) ## 抬笔
    robot.example_home_the_robot() ## 前往工作位置
    robot.go_to_pose(0.36, -0.48, 0.2, 90, 0, 40, True, 0.18) ## 前往笔筒上方
    robot.go_to_pose(0.36, -0.48, 0.06) ## 放下
    robot.gripper_open() ## 机械爪松开
    robot.go_to_pose(0.36, -0.48, 0.2) ## 机械臂抬起
    robot.example_home_the_robot() ## 前往工作位置
    robot.example_retract_the_robot() ## 回到初始位置


if __name__ == "__main__":
    main()

Read Gcode

def read_gcode(file_name, z_free, z_press, x_offset, y_offset, scale=1.0):
    f = open(file_name)
    text = f.readlines()

    ### Skip header lines
    start = 0
    for data in text:
        start += 1
        if 'G-code START' in data:
            break
    points = []
    ### Start Generate Trajectory
    z = z_free
    for i in range(start + 3, len(text)):
        if 'G-code END' in text[i]:
            break
        speed = 0.02
        if 'G' in text[i]:
            if 'G0' in text[i]:
                speed = 0.18 # G0 refer to higher speed
            point = text[i].split(" ")
            x = point[1]
            y = point[2]
            x = float(x[1:]) / 100 * scale + x_offset
            y = float(y[1:]) / 100 * scale + y_offset
            if 'M3' in text[i + 1]: ## M3 means robot arm move down
                points.append((x, y, z_free, speed))
                points.append((x, y, z_press, 0.01))
                z = z_press
            elif 'M5' in text[i + 1]: ## M3 means robot arm move up
                points.append((x, y, z_press, speed))
                points.append((x, y, z_free, 0.18))
                z = z_free
            else:
                points.append((x, y, z, speed))
    f.close()
    return points

Conclusion

In this project, we set up a coordinate system for Kinova Gen3 Lite, write out the corresponding DH parameters, and then solve its forward kinematics and inverse kinematics process. Then, according to the information obtained before, the robot arm is controlled to automatically take the pen, write and put the pen back into the pen container.At the same time, we control the speed change when the manipulator arm is lifting, writing and so on, so that the whole process is very smooth.