2. Kinematic Analysis

DH Parameters Kinematics Analysis

Joint Reference Frame Origins

The joint reference frame origins are indicated by the blue triangle in orientations as they appear in the URDF file. One thing that we need to understand about the URDF file is that each joint is defined relative to its parent. Meaning to get from joint 2, we have to 'translate' .35 meters in the 'X' direction and .42 meters in the Z direction.

Reference frame assignments in URDF file

Each row of the DH parameter table represents the homogeneous transform between frame {i-1} and frame {i}.Therefore we should incrementally check that the position of our frame origins is consistent with the cumulative displacements defined in the URDF file.

Relative location of joint {i-1} to joint {i}

Since the Robot arm has 6 revolute joints, only the θ terms are time variable. The only complicated one is joint2. θ i is the angle between xi-1 and xi measured about the zi axis in a right-hand sense. The Robot arm is shown where all the joint angles are assumed to be 0.Therefore, x1 is not parallel to x2 when θ2 equals 0.There's a constant offset of -90 degrees

Comparing the total Homogeneous transform between the base link and the Gripper Link

To compare the total homogeneous transform between the base link and the gripper link, one needs to account for the difference in orientation of the gripper link frame. To do this is to apply a sequence of fix body that is intrinsic rotations to the gripper frame in the python code. To do so is to align the two frames, first rotate about the z axis by 180 degrees and then the y-axis by -90 degrees.

Rotate the gripper reference frame about the z-axis by 180 degrees

Rotate the gripper reference frame about the z axis by -90 degrees

DH Parameters base on the URDF file

Twist Angle(α)

Alpha represents the twist angle

Link Length(a)

a represents the link length

Link offsets(d)

d represents the link Offset

Joint Variable(θ)

Theta represent the joint variable

Denavit - Hartenberg (DH) Parameters Table

Transformation matrices using DH Parameters

The transformation matrix is the combination of 2 rotations and 2 translations such as:

1. Rotation along the X axis by α
2. Translation along the X axis by a
3. Rotation along the Z axis by θ
4. Translation along Z axis by d

We can create a dictionary of our known DH parameter quantities, where

• α is the twist angles
• a is the link length
• d is the link offsets
• q is the joint variables, the θs

DH param symbols

            s = {alpha0:        0,  a0:      0, d1: 0.75, 
                 alpha1: -np.pi/2,  a1:   0.35, d2:    0, q2: q2-np.pi/2,
                 alpha2:        0,  a2:   1.25, d3:    0, 
                 alpha3: -np.pi/2,  a3: -0.054, d4:  1.5, 
                 alpha4:  np.pi/2,  a4:      0, d5:    0, 
                 alpha5: -np.pi/2,  a5:      0, d6:    0, 
                 alpha6:        0,  a6:      0, d7:0.303, q7:          0}

We can create the Homogeneous transforms between individueal neighboring links. For example between link 0 and 1, 1 and 2, 2 and 3, all the way between link 6 in the Gripper G. Next, we can begin to incrementally build the total homogenous transform between the base link and the gripper frame , by incrementally post-multiplying individual homogenous transforms.

DH Transformation matrix

# Between link 0 to 1
            T0_1 = Matrix([[             cos(q1),            -sin(q1),            0,              a0],
                           [ sin(q1)*cos(alpha0), cos(q1)*cos(alpha0), -sin(alpha0), -sin(alpha0)*d1],
                           [ sin(q1)*sin(alpha0), cos(q1)*sin(alpha0),  cos(alpha0),  cos(alpha0)*d1],
                           [                   0,                   0,            0,               1]])
            T0_1 = T0_1.subs(s)

# Between link 1 to 2
            T1_2 = Matrix([[             cos(q2),            -sin(q2),            0,              a1],
                           [ sin(q2)*cos(alpha1), cos(q2)*cos(alpha1), -sin(alpha1), -sin(alpha1)*d2],
                           [ sin(q2)*sin(alpha1), cos(q2)*sin(alpha1),  cos(alpha1),  cos(alpha1)*d2],
                           [                   0,                   0,            0,               1]])
            T1_2 = T1_2.subs(s)

# Between link 2 to 3
            T2_3 = Matrix([[             cos(q3),            -sin(q3),            0,              a2],
                           [ sin(q3)*cos(alpha2), cos(q3)*cos(alpha2), -sin(alpha2), -sin(alpha2)*d3],
                           [ sin(q3)*sin(alpha2), cos(q3)*sin(alpha2),  cos(alpha2),  cos(alpha2)*d3],
                           [                   0,                   0,            0,               1]])
            T2_3 = T2_3.subs(s)

#Between link 3 to 4
            T3_4 = Matrix([[             cos(q4),            -sin(q4),            0,              a3],
                           [ sin(q4)*cos(alpha3), cos(q4)*cos(alpha3), -sin(alpha3), -sin(alpha3)*d4],
                           [ sin(q4)*sin(alpha3), cos(q4)*sin(alpha3),  cos(alpha3),  cos(alpha3)*d4],
                           [                   0,                   0,            0,               1]])
            T3_4 = T3_4.subs(s)

#Between link 4 to 5
            T4_5 = Matrix([[             cos(q5),            -sin(q5),            0,              a4],
                           [ sin(q5)*cos(alpha4), cos(q5)*cos(alpha4), -sin(alpha4), -sin(alpha4)*d5],
                           [ sin(q5)*sin(alpha4), cos(q5)*sin(alpha4),  cos(alpha4),  cos(alpha4)*d5],
                           [                   0,                   0,            0,               1]])
            T4_5 = T4_5.subs(s)

#Between link 5 to 6
            T5_6 = Matrix([[             cos(q6),            -sin(q6),            0,              a5],
                           [ sin(q6)*cos(alpha5), cos(q6)*cos(alpha5), -sin(alpha5), -sin(alpha5)*d6],
                           [ sin(q6)*sin(alpha5), cos(q6)*sin(alpha5),  cos(alpha5),  cos(alpha5)*d6],
                           [                   0,                   0,            0,               1]])
            T5_6 = T5_6.subs(s)

#Between link 6 to 7
            T6_7 = Matrix([[             cos(q7),            -sin(q7),            0,              a6],
                           [ sin(q7)*cos(alpha6), cos(q7)*cos(alpha6), -sin(alpha6), -sin(alpha6)*d7],
                           [ sin(q7)*sin(alpha6), cos(q7)*sin(alpha6),  cos(alpha6),  cos(alpha6)*d7],
                           [                   0,                   0,            0,               1]])
            T6_7 = T6_7.subs(s)

Orientation Difference between Definition of Gripper link in URDF and DH Convention

To account for the difference in orientation of the gripper link as it is defined in the URDF and the DH convention, we aplpy a body fixed rotation about the z axis and then the y axis. Therefore the R_correction is the composition of rorations of this z axis and y axis rotation

Correction need to account of Orientation Difference

# 90 degrees on the z axis
             R_z = Matrix([[          cos(np.pi/2),         -sin(np.pi/2),            0],
                           [          sin(np.pi/2),          cos(np.pi/2),            0],
                           [                    0,                      0,            1]])
# 90 degrees on the Y axis
             R_y = Matrix([[        cos(-np.pi/2),                   0,   sin(-np.pi/2)],
                           [                   0,                    1,               0],
                           [      -sin(-np.pi/2),                   0,    cos(-np.pi/2)]])

             R_correction = R_z * R_y
            

From this correction, we get the Forward kinematics of the Robot arm and one can check this by evaluating the total transform between the base link and the corrected gripper length by evaluating a matrix at some arbitrary joint values and see if it matches the translation component of the end effector.

Inverse Kinematics

The Inverse Kinematics is the opposite of Forwards kinematics. Meaning that the position and orientation of the end-effector is known and the goal is to calculate the joint angles of the Arm Robot. However for a Robot Arm with 6 joints, the overall transformation between the base and end effector involves 6 multiples of Equation which can result in a complicated non-linear equation.

Numerical Approach VS Closed-Form Solution

There are 2 ways to solve the Inverse Kinematic. The first consist of purely numerical approach(For example Raphson Algorithm) which is to guess and iterate until the error is sufficiently small or the robot is such an idiot that you give up. The second approach is known as the closed-form solution. Closed_form solutions are specific algebraic equation or equations that do not require iteration to solve. It has two main advantages:

• Must faster to solve than numerical approaches
• Easier to develop rules for which of the possible solutions is the appropriate one.

The disadvantage which is not really a problem in our case is that certain types of arm robot are solvable in closed-form.If these two conditions are satisfied, then the arm robot is solvable in Closed-form.

1. 3 neighboring joint axes intersect at a single point
2. 3 neighboring joint axes are parallel

Our 6 Joints Robot Arm satisfy one of the above conditions.The last 3 joints 4, 5, 6 of our Robot Arm satisfy the first condition and this type of design is called Spherical Wrist and the point of intersection is called the Wrist Center.

Spherical Wrist and Wrist Center

The location of the Wrist Center and End Effector relative to the base frame 0 is given by 0_rwc/0 and 0_rEE/0 respectively and the location of the End Effector to the Wrist Center is given by 0_ree/wc.

Inverse Position and Orientation

Next is to symbolically define the homogeneous transform

[ l_x, m_x, n_x, p_x ]

[ l_y, m_y, n_y, p_y ]

[ l_z, m_z, n_z, p_z ]

[ 0, 0, 0 , 1 ]

l, m, n are orthonormal vectors representing the end-effector orientation along X, Y, Z axes of the local coordinate frame.

Because n is the vector along the gripper link z-axis, we have this:

w_x = p_x - (d₆ + l) * n_x

w_y = p_y - (d₆ + l) * n_y

w_z = p_z - (d₆ + l) * n_z

• p_x,y,z is the end-effector positions
• w_x,y,z is the wrist positions
• d₆ from DH table
• l is the end-effector length

Position Calculation

Since O₅ is the Wrist Center(w)Joint 1 angle is:

• &theta,₁ is atan2(w_y,w_x).

By using Joint 1 Angle we can get Joint 2 angle:

• θ₂ = 90 degree + (O_2zW / O₂W) - ( (O₂O₃)² + (O₂W)² - (O₃W)² ) / (2 * O₂O₃ * O₂W)

And with Joint 2 angle we can get O₃,therefore we can say:

• O_3zW / O₃W = sin(θ₃ + θ₂)

Orientation Calculation

By providing the angle value for Joint 1(θ₁), Joint 2(θ₂) and Joint 3(θ₃) into the transformation matrix we can get the orientation of the Robot Arm when Joint 4, joint 5 and joint 6 angles(θ₄,θ₅,θ₆) is 0.