import numpy as np
from numpy.linalg import inv
import matplotlib.pyplot as plt
# Kinematic Tree Class:
class KinematicTreeNode:
def __init__(self): # Constructor
self.translation = None
self.rotation = None
self.child = None
# Rotation Matrix:
def rotate_joint(theta):
c = np.cos(theta)
s = np.sin(theta)
return np.array([[c, -s],[s, c]])
# Function that returns ALL joint positions relative to the input:
def get_joints(kinematic_tree):
if kinematic_tree.child == None:
return kinematic_tree.translation
else:
relative_positions = get_joints(kinematic_tree.child)
rows, columns = relative_positions.shape
transformed_joints = np.zeros((rows, columns))
for i in range(0, columns):
rotated_joint = rotate_joint(kinematic_tree.rotation) @ relative_positions[:, i]
rotated_joint = np.reshape(rotated_joint, (2, 1))
transformed_joints[:, i:i+1] = rotated_joint + kinematic_tree.translation
joint_positions = np.hstack((kinematic_tree.translation, transformed_joints))
return joint_positions
# Build the tree
base = KinematicTreeNode()
L1 = KinematicTreeNode()
L2 = KinematicTreeNode()
L3 = KinematicTreeNode()
base.translation = np.array([[0],[0]], dtype=float) # translation to the joint
base.rotation = 0.1 # rotation of the joint
L1.translation = np.array([[1],[0]], dtype=float)
L1.rotation = 0.2
L2.translation = np.array([[1],[0]], dtype=float)
L2.rotation = 0.3
# Loop for Inverse Kinematics with Newton-Raphson
base.child = L1
L1.child = L2
L2.child = None
print("This shows that the get_joints command works")
joint_pos = get_joints(base)
print(joint_pos)
This shows that the GetAllJoints command works
[[0. 0.99500417 1.95034065]
[0. 0.09983342 0.39535362]]
fig, ax = plt.subplots()
p1, = ax.plot([], [], color='black', linewidth=2)
lb, ub = -5, 5
ax.set_xlim([lb, ub])
ax.set_ylim([lb, ub])
ax.set_xlabel('X') # X Label
ax.set_ylabel('Y') # Y Label
ax.set_title('Forward Kinematics:')
video_title = "simulation"
x_pendulum_arm = joint_pos[0, :]
y_pendulum_arm = joint_pos[1, :]
p1.set_data(x_pendulum_arm, y_pendulum_arm)
fig.canvas.draw()
# 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 = get_joints(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 = get_joints(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
Step Size: [-2.69192456 5.3523653 ]
Search Iteration: 0
The Current Best Joint Angles are: [-2.49192456 5.5523653 ]
Yielding an end-effector position of: [0.95056379 0.29404384]
Step Size: [ 4.65659226 -9.30211622]
Search Iteration: 1
The Current Best Joint Angles are: [ 2.1646677 -3.74975092]
Yielding an end-effector position of: [-0.89649681 -0.26192964]
Step Size: [-1.0471137 2.15354465]
Search Iteration: 2
The Current Best Joint Angles are: [ 1.117554 -1.59620627]
Yielding an end-effector position of: [-0.28693007 -0.08555868]
Step Size: [-0.0838641 0.08187749]
Search Iteration: 3
The Current Best Joint Angles are: [ 1.0336899 -1.51432878]
Yielding an end-effector position of: [0.66274963 0.21922437]
Step Size: [-5.55157464e-06 3.56368940e-03]
Search Iteration: 4
The Current Best Joint Angles are: [ 1.03368435 -1.51076509]
Yielding an end-effector position of: [0.69917586 0.19842351]
Step Size: [-4.55946878e-06 4.82424694e-06]
Search Iteration: 5
The Current Best Joint Angles are: [ 1.03367979 -1.51076027]
Yielding an end-effector position of: [0.69999798 0.20000105]