import numpy as np from numpy.linalg import inv # Defines our kinematic tree class KinematicTreeNode: def __init__(self): # Constructor self.translation = None self.rotation = None self.child = None # Define our rotation matrix def RotZ(theta): c = np.cos(theta) s = np.sin(theta) return np.array([[c, -s],[s, c]]) # FORWARD KINEMATICS # Function that returns the End-effector position # Takes the base link and returns the end-effector position def EndEffectorPos(X): if X.child == None: # This is the last node in this branch of the tree return X.translation else: # Return the transformed end effector position return X.translation + np.matmul(RotZ(X.rotation),EndEffectorPos(X.child)) # FORWARD KINEMATICS (Returns the positions of ALL the joints, not just the end-effector) # Takes the base link and returns the position of all joints in a list # Base link is first, then in order out to the end effector # Returns a matrix where the first column is the coordinates of the first joint, second column is second joint, etc. def GetAllJoints(X): if X.child == None: return X.translation else: returned_joints = GetAllJoints(X.child) temp, translate = np.meshgrid(returned_joints[0,:], X.translation) transformed_joints = np.matmul(RotZ(X.rotation),returned_joints)+translate list_of_joints = np.hstack((X.translation, transformed_joints)) return list_of_joints # Build the tree base = KinematicTreeNode() L1 = KinematicTreeNode() L2 = KinematicTreeNode() base.translation = np.array([[0],[0]]) # translation to the joint base.rotation = 0.1 # rotation of the joint L1.translation = np.array([[1],[0]]) L1.rotation = 0.1 L2.translation = np.array([[1],[0]]) L2.rotation = None # Loop for Inverse Kinematics with Newton-Raphson base.child = L1 L1.child = L2 L2.child = None print("This shows that the GetAllJoints command works") print(GetAllJoints(base)) # Build Tree base = KinematicTreeNode() L1 = KinematicTreeNode() L2 = KinematicTreeNode() base.translation = np.array([[0],[0]], dtype=float) # translation to the joint base.rotation = 0.0 L1.translation = np.array([[0.5],[0]], dtype=float) L1.rotation = 0.0 L2.translation = np.array([[0.5],[0]], dtype=float) L2.rotation = 0.0 # Loop for Inverse Kinematics with Newton-Raphson base.child = L1 L1.child = L2 L2.child = None # Inverse Kinematics: # Define a desired position x_des = 0.7 y_des = 0.2 # INVERSE KINEMATICS CODE # Newton-Raphson Search # Define our desired END EFFECTOR POSITION desired_position = np.array([x_des,y_des], dtype=float) # Step 1: Start with an initial guess # Initial guess for our joint angles theta_current = np.array([0.2, 0.2], dtype=float) done = False # Small Epsilon for Finite Difference: epsilon = 0.001 # Iteration Counter: i = 0 while not done: # Step 2: Find f(theta) base.rotation = theta_current[0] L1.rotation = theta_current[1] joint_locations = GetAllJoints(base) end_effector = joint_locations[:, -1] # Step 3: Find the Jacobian of f w.r.t. theta jac = np.array([[0., 0.],[0., 0.]], dtype=float) for j in range(0, 2): for k in range(0, 2): delta_theta = np.array([[0.],[0.]]) delta_theta[k,0] = epsilon base.rotation = theta_current[0] + delta_theta[0,0] L1.rotation = theta_current[1] + delta_theta[1,0] joint_locations = GetAllJoints(base) delta_end_effector = joint_locations[:, -1] partial_der = (delta_end_effector - end_effector) / epsilon jac[j,k] = partial_der[j] # Step 4: Find Step Length delta_theta_step = np.matmul(inv(jac), (desired_position - end_effector)) print("Step Size: ", delta_theta_step) # Theta current: theta_current = theta_current + delta_theta_step print("Search Iteration: ", i) print("The Current Best Joint Angles are: ", theta_current) print("Yielding an end-effector position of: ", end_effector) i = i + 1 # Terminating Condition: if np.linalg.norm(delta_theta_step) < epsilon: done = True