Kinematics

Forward Kinematics

Finger

Each finger was considered a 3-DOF serial link manipulator consisting of three purely rotational joints. The forward kinematics were computed using the Product of Exponentials method. I highly recommend the free Modern Robotics textbook and lecture series by Kevin Lynch and Frank Park to gain a deeper understanding of this method: http://hades.mech.northwestern.edu/index.php/Modern_Robotics

The first step is to define a zero configuration for the finger. We than chose to define the finger spatial frame at the the base of the first servo and the end effector body frame at the tip of the claw.

Note that both xs and xb are positive in the direction out of the page.

The corresponding pose of the body frame in the zero configuration is represented in matrix form as:

Tsb0 = np.array([[1,0,0,0],
                 [0,1,0,35.7],
                 [0,0,1,202.9],
                 [0,0,0,1]])

The next step is to define screw axes for each of the three rotational joints. Each screw axis is a 6 parameter vector consisting of a rotational component and a linear component. The rotational component (w) makes up the first 3 parameters and is defined as a unit vector in the direction of rotation. The linear component makes up the last 3 parameters and is defined as the linear velocity of the spatial frame undergoing a rotation of 1 rad/s along the rotation axis w. This can also be calculated using the formula: v = -w x q, where q is any point on the axis of rotation.

w1 = np.array([0, -1, 0]).reshape(3,1)
p1 = np.array([0, 0, 43.1]).reshape(3,1)
w2 = np.array([-1, 0, 0]).reshape(3,1)
p2 = np.array([0, 55.15, 43.1]).reshape(3,1)
w3 = np.array([1, 0, 0]).reshape(3,1)
p3 = np.array([0, 55.15, 104.5]).reshape(3,1)

Using these values a twist coordinate can be generated for each joint using the following function.

def twistGen(w, p):
    twist = np.vstack((w, np.cross(-w.reshape(1,3), p.reshape(1,3)).reshape(3,1)))
    return twist

To calculate the transformation resulting from any joint rotation, the exponential of the twist for each joint must be calculated. This can be done using the following function.

def twistExpGen(twist, theta):
    w = twist[0:3]
    v = twist[3:6]
    eS = np.eye(3) + skew(w) * sin(theta) + skew(w) @ skew(w) * (1 - cos(theta))
    G = np.eye(3) * theta + skew(w) * (1 - cos(theta)) + skew(w) @ skew(w) * (theta - sin(theta))
    temp = np.hstack((eS, G @ v))
    eX = np.vstack((temp, [0, 0, 0, 1]))
    return eX

Finally, given rotations of all three joints, the forward kinematics solution of the body frame can be calculated using the following function:

def Tsbgen(theta):
    eX1 = twistExpGen(twist1, theta[0])
    eX2 = twistExpGen(twist2, theta[1])
    eX3 = twistExpGen(twist3, theta[2])
    Tsb = eX1@eX2@eX3@Tsb0
    return Tsb

Finally a function is written to obtain only the position of the body frame origin.

def fkine(theta): Tsb = Tsbgen(theta) pos = Tsb[0:3, 3] return pos

These functions are the base of the Product of Exponentials forward kinematics solution, and can be generalized to any serial link manipulator with proper choice of the zero configuration and screw axis definitions. See the "Finger" class in the "Motion Controller.py" file for the code.

Hand

The forward kinematics discussed previously obtains solutions in the finger spatial frame coordinates. However, oftentimes we care about representing points in the entire hand spatial frame coordinates. For example, trajectories for all five fingers are generated in the hand spatial frame. The forward kinematics developed earlier for an individual finger needs to be modified to account for its orientation relative to the hand spatial frame. The hand spatial frame was defined as shown:

Each finger is placed equally around a circle of radius 69mm. The orientation of each finger spatial frame can be obtained by first translating the hand spatial frame along the y axis followed by a rotation around the z-axis. This can be calculated using the following two functions:

def Rgamma(gamma):
    R = np.array([[cos(gamma), -sin(gamma), 0],
                  [sin(gamma),   cos(gamma), 0],
                  [0,            0,          1]])
    return R

def Tasgen(r, fingernum, fangle):

    # Translate by r in the z direction
    R1 = Rgamma(0)
    p1 = np.array([0, r, 0]).reshape(3, 1)
    T1 = np.hstack((R1, p1))
    T1 = np.vstack((T1, [0, 0, 0, 1]))

    # Rotate by angle of finger around z axis
    R2 = Rgamma(fangle)
    p2 = np.array([0, 0, 0]).reshape(3, 1)
    T2 = np.hstack((R2, p2))
    T2 = np.vstack((T2, [0, 0, 0, 1]))
    
    # Combine
    Tas = T2@T1
    return Tas

Note that the angle in the Tasgen function is measured counter clockwise from the positive y-axis to the angle of the finger spatial frame. Now the forward kinematics for any joint angles can be calculated by pre-multiplying the finger spatial frame forward kinematics solution with the corresponding Tas transformation. For more details see the "Hand" class in the "Motion Controller.py" file.