4 DOF Robot Arm Simulation

Introduction

This python project simulates the forward and inverse kinematics of a PRRR robot using standard Denavit-Hartenberg parameters and applies it to a pick and place task. The structure of the robot consists of an initial prismatic joint that can move the robot vertically, followed by two horizontal revolute joints like a SCARA arm. The final joint can rotate around the horizontal to increase its vertical reach. The code is structured so that the Denavit-Hartenberg parameters can be easily modified to restructure the robot and the kinematics will adapt accordingly.

Controls

Run main.py to start script

Key	Action (wrt World Frame)
b	Begin pick and place simulation
q	End simulation
w	Move end effector in +Y
x	Move end effector in -Y
d	Move end effector in +X
a	Move end effector in -X
z	Move end effector in +Z
c	Move end effector in -Z

Methodology

The software architecture follows a modular design where robot.py handles all kinematic computation, visualize.py manages graphical representation, and main.py coordinates the task execution. This separation allows easy modification and reuse of components.

• robot.py
• visualize.py
• main.py

Script robot.py

This script contains a class "Robot" that implements all the logic of the robot. Its init function calculates the transformation matrices for each joint with given Denavit-Hartenberg parameters with the variables defined as symbols (implemented with sympy).

Standard DH Parameters Table and Python Implementation

Joint	θ	d	a	α
J0	0	d1	0	0
J1	θ₁	L2D	L2A	0
J2	θ₂	0	L3	0
JV3	-π/2	0	0	-π/2
J3	θ₃ + π/2	0	L4A	0

self.theta1, self.theta2, self.theta3, self.d1 = sp.symbols('theta1 theta2 theta3 d1')
self.T01 = dh_transform(0, self.d1, 0, 0)
self.T12 = dh_transform(self.theta1, self.L2D, self.L2A, 0)
self.T2V3 = dh_transform(self.theta2, 0, self.L3, 0)
self.TV33 = dh_transform(-sp.pi/2, 0, 0, -sp.pi/2)
self.T34 = dh_transform(self.theta3 + np.pi/2, self.L4A, self.L4D, 0)
self.T04 = self.T01 * self.T12 * self.T2V3 * self.TV33 * self.T34

The above snippet uses a "dh_transform" function to create the sp.matrix. This function accepts standard DH parameters and returns the joints' transformation matrix. Note, the order of input arguments reflects the order used for a standard DH joint (θ, d, a, α) as shown below:

import sympy as sp

def dh_transform(theta, d, a, alpha):
    ct = sp.cos(theta)
    st = sp.sin(theta)
    ca = sp.cos(alpha)
    sa = sp.sin(alpha)

    return sp.Matrix([
        [ct, -st*ca,  st*sa, a*ct],
        [st,  ct*ca, -ct*sa, a*st],
        [0,       sa,     ca,    d],
        [0,        0,      0,    1]
    ])

The "forward_kinematics" simply substitutes joint values in the symbolic transformation matrices and returns the coordinates for each joint.

def forward_kinematics(self, theta1, theta2, theta3, d1):
    subs = {
        self.theta1: theta1,
        self.theta2: theta2,
        self.theta3: theta3,
        self.d1: d1
    }

    T01 = np.array(self.T01.subs(subs), dtype=float)
    T02 = np.array((self.T01 * self.T12).subs(subs), dtype=float)
    T03 = np.array((self.T01 * self.T12 * self.T2V3 * self.TV33).subs(subs), dtype=float)
    T04 = np.array(self.T04.subs(subs), dtype=float)

    P0 = np.array([0, 0, 0, 1])

    P1 = (T01 @ P0)[:3]
    P2 = (T02 @ P0)[:3]
    P3 = (T03 @ P0)[:3]
    P4 = (T04 @ P0)[:3]

    return P1, P2, P3, P4
    

The inverse_kinematics function accepts a coordinate and solves the transformation matrix (T04) for the end effector from the origin (0, 0, 0) and returns the joint values. More specifically, it returns the solution that is closest to the current arm state.

$$\begin{bmatrix} r_{11} & r_{12} & r_{13} & t_x \\\ r_{21} & r_{22} & r_{23} & t_y \\\ r_{31} & r_{32} & r_{33} & t_z \\\ 0 & 0 & 0 & 1 \end{bmatrix} \cdot \begin{bmatrix} 0 \\ 0 \\ 0 \\ 1 \end{bmatrix} = \begin{bmatrix} x \\ y \\ z \\ 1 \end{bmatrix}$$

Normal solve takes a long time to complete and is not feasible to be used when stepping the robot. To solve this, nSolve (numeric solve) was implemented and the robot's current position was used as the initial guess. This ensured fast solve times and directly handled the acceptable tolerance and detecting points out of range.

The angle of J4 was taken as an input argument allowing the last link to reach a given point at an angle. This is done so that the arm can be specifically instructed to have the last link vertical when picking and horizontal when placing.

Script visualize.py

This script includes a Visualizer3D class responsible for rendering the robot and its surrounding environment. Using the link and joint positions provided by the forward_kinematics function, it plots the full kinematic structure of the robot in three-dimensional space. The visualizer also generates objects on the floor at random positions, along with three designated target locations where these objects must be placed. To enhance clarity during the simulation, the appearance of the object, the robot’s end effector, and the target location is dynamically updated, creating the visual impression that the robot is actively grabbing and moving the object.

Script main.py

This script sequentially executes the robot’s go and move functions to complete the assigned task. After the task is finished, the script enters a continuous loop that keeps the robot active until the program is manually terminated. This idle window allows the user to interact with the robot and manually move it, providing a convenient way to visualize and verify the kinematic behavior of the system.

Discussion

To ensure that the robot arm can reach points located close to its base, the horizontal components of links L2 and L3 were designed to be equal. This symmetry allows the arm to achieve configurations where θ₂ approaches π, enabling the end-effector to fold back toward the origin without creating singular or unreachable regions.

For the transition from J2 to J3, a virtual joint (JV3) was used. This was done to effectively achieve a rotation about the "Y" axis to go from J2 to J3. DH parameters allow rotation about the Z and X axes, hence a combination of both (without any linear displacement) was used to achieve a rotation about "Y".

In the pick-and-place process, the robot utilizes link L4 in two orientations: a vertical orientation for picking and a horizontal orientation for placing. By switching between these orientations, the required height adjustment from the prismatic joint is minimized. This reduces unnecessary vertical travel, improves efficiency, and simplifies the motion planning.

An out of reach point is assumed when the numeric solve (in inverse kinematics) is unable to find a solution. During execution the script marks the target as out of reach and proceeds with the remaining objectives.

Further improvements to this simulation may include collision for the links where it detects if the links collide with each other or other obstacles.

Links

• GitHub Repository
• Demonstration Video

References

• ChatGPT was used to obtain iteration syntax for "go" and "move" functions
• Concept for drawing links was obtained from the provided github repository