RobotKinematicExamples

kinematics.py

import numpy as np
from math_op import *

def generate_transformation_from_xi(xi, theta):
    """
    Given a \\xi, generate the corresponding transformation according
    to exponential coordinates
    """
    rot = rotation_matrix_from_axis_angle(xi[3:], theta)
    trans = (np.eye(3) - rot).dot(np.cross(xi[3:], xi[0:3])) + xi[3:] * xi[3:].T * xi[0:3] * theta
    transformation = np.eye(4)
    transformation[0:3,0:3] = rot
    transformation[0:3,3] = trans
    return transformation

def convert_trans_to_Adj(trans):
    R = trans[0:3,0:3]
    p = trans[0:3,3]
    Adj = np.zeros((6,6))
    Adj[0:3,0:3] = R
    Adj[0:3,3:] = np.matmul(skew_symmetric_matrix(p),R)
    Adj[3:,3:] = R
    return Adj

def forward_kinematics(gst0, xi_array, thetas):

    index = len(thetas) - 1
    trans_mat = gst0
    while index >= 0:
        trans_mat = generate_transformation_from_xi(xi_array[:,index], thetas[index]).dot(trans_mat)
        index -= 1
    return trans_mat

def convert_trans_mat_to_vec(mat):
    """
    Return vec where [x,y,z, q_x, q_y, q_z, q_w]
    """
    vec = np.zeros((7,))
    vec[0:3] = mat[:3,3]
    q = Quaternion(matrix=mat[:3,:3])
    vec[3:] = q.elements
    # vec[3] = q.elements[1]
    # vec[4] = q.elements[2]
    # vec[5] = q.elements[3]
    # vec[6] = q.elements[0]
    return vec

def inverse_kinematics(gst0, xi_list, curr_theta, desire_pose):

    # get current pose
    curr_pose = forward_kinematics(gst0, xi_list, curr_theta)
    curr_pose = convert_trans_mat_to_vec(curr_pose)

    # calculate error
    error = calculate_pose_difference(curr_pose, desire_pose)
    # check if error less than range
    while (np.linalg.norm(error) > 1e-3):
        # get the current jacobian
        jac = calculate_jacobian(xi_list, curr_theta)
        # inverse jac
        inv_jac = np.linalg.pinv(jac)
        # update theta
        delta_theta = inv_jac.dot(error)
        curr_theta += delta_theta * 0.05
        #find new pose
        curr_pose = forward_kinematics(gst0, xi_list, curr_theta)
        curr_pose = convert_trans_mat_to_vec(curr_pose)
        # calculate new error
        error = calculate_pose_difference(curr_pose, desire_pose)
        #print(np.linalg.norm(error))

    return curr_theta


def calculate_jacobian(xi_array, thetas):
    #Calculation of Jacobian from the xi matrix
    Jacobian = np.zeros((6,np.size(thetas,0)))
    Jacobian[:,0] = xi_array[:,0] 

    prev_transformation_matrix = np.eye(4)
    for i in range(0, np.size(thetas,0)):
        
        Adj_matrix = convert_trans_to_Adj(prev_transformation_matrix)
        Jacobian[:,i] = Adj_matrix.dot(xi_array[:,i])
        transformation_matrix = generate_transformation_from_xi(xi_array[:,i],thetas[i])
        prev_transformation_matrix = np.matmul(prev_transformation_matrix, transformation_matrix)

    return Jacobian

math_op.py

import numpy as np
from pyquaternion import Quaternion

#From Alloy

def skew_symmetric_matrix(vec):
    """
    Return a 3x3 skew symmetric matrix from the given vector
    parameters:
    -----------
    vec : numpy array(3,)
        vector array with three elements (x, y, z)
    
    returns:
    --------
    numpy array(3,3)
        the skew symmetric matrix from the given vector
    """
    matrix = [
        [0, -vec[2], vec[1]],
        [vec[2], 0, -vec[0]],
        [-vec[1], vec[0], 0]
    ]
    return np.array(matrix)

def rotation_matrix_from_axis_angle(axis, theta):
    """
    Given an axis and angle(in radian), return the rotation matrix that
    represent the that rotation of theta around given axis.
    Notes
    -----
    This is similar to the exponential rotation(R = exp(\omega_hat,theta)) as it uses the Rodrigues formula,
    where \omega_hat is the skew symmetric matrix of the axis and theta is the rotation.
    """
    if np.shape(axis) != (3,):
        raise AttributeError('Axis is incorrect size, must be (3,)')
    axis_hat = skew_symmetric_matrix(axis)
    #from page 28 of MLS 1994
    return np.eye(3) + axis_hat * np.sin(theta) + axis_hat.dot(axis_hat) * ( 1 - np.cos(theta))

def calculate_pose_difference(p1, p2):
    """Calculate the pose error from p1 to p2. Note the resulting
    error is calculated in the frame of p1 and not the base frame
    do p[0:3] = p[0:3] - np.cross(x[0:3],p[3:])
    """

    error = np.zeros(6,)
    #the position error is just the difference in position
    error[0:3] = p2[0:3] - p1[0:3]
    #orientation error is more tricky
    desire_q = Quaternion(p2[3:])
    error_q = desire_q * Quaternion(p1[3:]).inverse
    error[3:] = error_q.axis * error_q.angle
    
    error[:3] = error[:3] - np.cross(p1[:3], error[3:]) 
    return error