github/PythonRobotics
1
1
Fork 0
mirror of https://github.com/AtsushiSakai/PythonRobotics.git synced 2026-01-13 15:48:13 -05:00
Code Issues Packages Projects Releases Wiki Activity

add animation

Browse Source
This commit is contained in:
Atsushi Sakai
2018-08-17 21:29:47 +09:00
parent 6509bdb651
commit cf4ab5a768
3 changed files with 103 additions and 30 deletions
Download Patch File Download Diff File Expand all files Collapse all files

37
ArmNavigation/n_joint_to_point_control/NLinkArm.py
View File

@@ -7,50 +7,61 @@ Author: Daniel Ingram
import numpy as np
import matplotlib.pyplot as plt
class NLinkArm(object):
def __init__(self, link_lengths, joint_angles, goal):
def __init__(self, link_lengths, joint_angles, goal, show_animation):
self.show_animation = show_animation
self.n_links = len(link_lengths)
if self.n_links is not len(joint_angles):
raise ValueError()
self.link_lengths = np.array(link_lengths)
self.joint_angles = np.array(joint_angles)
self.points = [[0, 0] for _ in range(self.n_links+1)]
self.points = [[0, 0] for _ in range(self.n_links + 1)]
self.lim = sum(link_lengths)
self.goal = np.array(goal).T
self.fig = plt.figure()
self.fig.canvas.mpl_connect('button_press_event', self.click)
if show_animation:
self.fig = plt.figure()
self.fig.canvas.mpl_connect('button_press_event', self.click)
plt.ion()
plt.show()
plt.ion()
plt.show()
self.update_points()
def update_joints(self, joint_angles):
self.joint_angles = joint_angles
self.update_points()
def update_points(self):
for i in range(1, self.n_links+1):
self.points[i][0] = self.points[i-1][0] + self.link_lengths[i-1]*np.cos(np.sum(self.joint_angles[:i]))
self.points[i][1] = self.points[i-1][1] + self.link_lengths[i-1]*np.sin(np.sum(self.joint_angles[:i]))
for i in range(1, self.n_links + 1):
self.points[i][0] = self.points[i - 1][0] + \
self.link_lengths[i - 1] * \
np.cos(np.sum(self.joint_angles[:i]))
self.points[i][1] = self.points[i - 1][1] + \
self.link_lengths[i - 1] * \
np.sin(np.sum(self.joint_angles[:i]))
self.end_effector = np.array(self.points[self.n_links]).T
self.plot()
if self.show_animation:
self.plot()
def plot(self):
plt.cla()
for i in range(self.n_links+1):
for i in range(self.n_links + 1):
if i is not self.n_links:
plt.plot([self.points[i][0], self.points[i+1][0]], [self.points[i][1], self.points[i+1][1]], 'r-')
plt.plot([self.points[i][0], self.points[i + 1][0]],
[self.points[i][1], self.points[i + 1][1]], 'r-')
plt.plot(self.points[i][0], self.points[i][1], 'ko')
plt.plot(self.goal[0], self.goal[1], 'gx')
plt.plot([self.end_effector[0], self.goal[0]], [self.end_effector[1], self.goal[1]], 'g--')
plt.plot([self.end_effector[0], self.goal[0]], [
self.end_effector[1], self.goal[1]], 'g--')
plt.xlim([-self.lim, self.lim])
plt.ylim([-self.lim, self.lim])

BIN
ArmNavigation/n_joint_to_point_control/animation.gif Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 1.5 MiB

96
ArmNavigation/n_joint_to_point_control/n_joint_to_point_control.py
View File

@@ -2,31 +2,35 @@
Inverse kinematics for an n-link arm using the Jacobian inverse method
Author: Daniel Ingram (daniel-s-ingram)
Atsushi Sakai (@Atsushi_twi)
"""
import matplotlib.pyplot as plt
import numpy as np
from NLinkArm import NLinkArm
#Simulation parameters
# Simulation parameters
Kp = 2
dt = 0.1
N_LINKS = 10
N_ITERATIONS = 10000
#States
# States
WAIT_FOR_NEW_GOAL = 1
MOVING_TO_GOAL = 2
def n_link_arm_ik():
show_animation = True
def main():
"""
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)
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)
arm = NLinkArm(link_lengths, joint_angles, goal_pos, show_animation)
state = WAIT_FOR_NEW_GOAL
solution_found = False
while True:
@@ -35,10 +39,11 @@ def n_link_arm_ik():
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
# 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)
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
@@ -47,13 +52,14 @@ def n_link_arm_ik():
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
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):
"""
@@ -69,34 +75,90 @@ def inverse_kinematics(link_lengths, joint_angles, goal_pos):
joint_angles = joint_angles + np.matmul(J, errors)
return joint_angles, False
def get_random_goal():
from random import random
SAREA = 15.0
return [SAREA * random() - SAREA / 2.0,
SAREA * random() - SAREA / 2.0]
def animation():
link_lengths = [1] * N_LINKS
joint_angles = np.array([0] * N_LINKS)
goal_pos = get_random_goal()
arm = NLinkArm(link_lengths, joint_angles, goal_pos, show_animation)
state = WAIT_FOR_NEW_GOAL
solution_found = False
i_goal = 0
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 = get_random_goal()
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.goal = get_random_goal()
i_goal += 1
if i_goal >= 5:
break
arm.update_joints(joint_angles)
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]))
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]))
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
return (theta1 - theta2 + np.pi) % (2 * np.pi) - np.pi
if __name__ == '__main__':
n_link_arm_ik()
main()
# animation()