mirror of
https://github.com/AtsushiSakai/PythonRobotics.git
synced 2026-04-22 03:00:22 -04:00
102 lines
3.3 KiB
Python
102 lines
3.3 KiB
Python
"""
|
|
Inverse kinematics for an n-link arm using the Jacobian inverse method
|
|
|
|
Author: Daniel Ingram (daniel-s-ingram)
|
|
"""
|
|
import matplotlib.pyplot as plt
|
|
import numpy as np
|
|
|
|
from NLinkArm import NLinkArm
|
|
|
|
#Simulation parameters
|
|
Kp = 2
|
|
dt = 0.1
|
|
N_LINKS = 10
|
|
N_ITERATIONS = 10000
|
|
|
|
#States
|
|
WAIT_FOR_NEW_GOAL = 1
|
|
MOVING_TO_GOAL = 2
|
|
|
|
def n_link_arm_ik():
|
|
"""
|
|
Creates an arm using the NLinkArm class and uses its inverse kinematics
|
|
to move it to the desired position.
|
|
"""
|
|
link_lengths = [1]*N_LINKS
|
|
joint_angles = np.array([0]*N_LINKS)
|
|
goal_pos = [N_LINKS, 0]
|
|
arm = NLinkArm(link_lengths, joint_angles, goal_pos)
|
|
state = WAIT_FOR_NEW_GOAL
|
|
solution_found = False
|
|
while True:
|
|
old_goal = goal_pos
|
|
goal_pos = arm.goal
|
|
end_effector = arm.end_effector
|
|
errors, distance = distance_to_goal(end_effector, goal_pos)
|
|
|
|
#State machine to allow changing of goal before current goal has been reached
|
|
if state is WAIT_FOR_NEW_GOAL:
|
|
if distance > 0.1 and not solution_found:
|
|
joint_goal_angles, solution_found = inverse_kinematics(link_lengths, joint_angles, goal_pos)
|
|
if not solution_found:
|
|
print("Solution could not be found.")
|
|
state = WAIT_FOR_NEW_GOAL
|
|
arm.goal = end_effector
|
|
elif solution_found:
|
|
state = MOVING_TO_GOAL
|
|
elif state is MOVING_TO_GOAL:
|
|
if distance > 0.1 and (old_goal is goal_pos):
|
|
joint_angles = joint_angles + Kp*ang_diff(joint_goal_angles, joint_angles)*dt
|
|
else:
|
|
state = WAIT_FOR_NEW_GOAL
|
|
solution_found = False
|
|
|
|
arm.update_joints(joint_angles)
|
|
|
|
|
|
def inverse_kinematics(link_lengths, joint_angles, goal_pos):
|
|
"""
|
|
Calculates the inverse kinematics using the Jacobian inverse method.
|
|
"""
|
|
for iteration in range(N_ITERATIONS):
|
|
current_pos = forward_kinematics(link_lengths, joint_angles)
|
|
errors, distance = distance_to_goal(current_pos, goal_pos)
|
|
if distance < 0.1:
|
|
print("Solution found in %d iterations." % iteration)
|
|
return joint_angles, True
|
|
J = jacobian_inverse(link_lengths, joint_angles)
|
|
joint_angles = joint_angles + np.matmul(J, errors)
|
|
return joint_angles, False
|
|
|
|
def forward_kinematics(link_lengths, joint_angles):
|
|
x = y = 0
|
|
for i in range(1, N_LINKS+1):
|
|
x += link_lengths[i-1]*np.cos(np.sum(joint_angles[:i]))
|
|
y += link_lengths[i-1]*np.sin(np.sum(joint_angles[:i]))
|
|
return np.array([x, y]).T
|
|
|
|
def jacobian_inverse(link_lengths, joint_angles):
|
|
J = np.zeros((2, N_LINKS))
|
|
for i in range(N_LINKS):
|
|
J[0, i] = 0
|
|
J[1, i] = 0
|
|
for j in range(i, N_LINKS):
|
|
J[0, i] -= link_lengths[j]*np.sin(np.sum(joint_angles[:j]))
|
|
J[1, i] += link_lengths[j]*np.cos(np.sum(joint_angles[:j]))
|
|
|
|
return np.linalg.pinv(J)
|
|
|
|
def distance_to_goal(current_pos, goal_pos):
|
|
x_diff = goal_pos[0] - current_pos[0]
|
|
y_diff = goal_pos[1] - current_pos[1]
|
|
return np.array([x_diff, y_diff]).T, np.math.sqrt(x_diff**2 + y_diff**2)
|
|
|
|
def ang_diff(theta1, theta2):
|
|
"""
|
|
Returns the difference between two angles in the range -pi to +pi
|
|
"""
|
|
return (theta1 - theta2 + np.pi)%(2*np.pi) - np.pi
|
|
|
|
if __name__ == '__main__':
|
|
n_link_arm_ik() |