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 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)
goal_pos = [N_LINKS, 0]
arm = NLinkArm(link_lengths, joint_angles, goal_pos, show_animation)
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 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)
goal_pos = [N_LINKS, 0]
arm = NLinkArm(link_lengths, joint_angles, goal_pos, show_animation)
state = WAIT_FOR_NEW_GOAL
solution_found = False
while True:
old_goal = np.array(goal_pos)
goal_pos = np.array(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 all(old_goal == 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 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 = np.array(goal_pos)
goal_pos = np.array(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 all(old_goal == 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)
"""
Forward Kinematics for an n-link arm in 3D
Author: Takayuki Murooka (takayuki5168)
"""
import math
from NLinkArm import NLinkArm
import random
def random_val(min_val, max_val):
return min_val + random.random() * (max_val - min_val)
if __name__ == "__main__":
print("Start solving Forward Kinematics 10 times")
# init NLinkArm with Denavit-Hartenberg parameters of PR2
n_link_arm = NLinkArm([[0., -math.pi / 2, .1, 0.],
[math.pi / 2, math.pi / 2, 0., 0.],
[0., -math.pi / 2, 0., .4],
[0., math.pi / 2, 0., 0.],
[0., -math.pi / 2, 0., .321],
[0., math.pi / 2, 0., 0.], [0., 0., 0., 0.]])
# execute FK 10 times
for i in range(10):
n_link_arm.set_joint_angles(
[random_val(-1, 1) for j in range(len(n_link_arm.link_list))])
ee_pose = n_link_arm.forward_kinematics(plot=True)
import math
from NLinkArm import NLinkArm
import random
def random_val(min_val, max_val):
return min_val + random.random() * (max_val - min_val)
if __name__ == "__main__":
print("Start solving Inverse Kinematics 10 times")
# init NLinkArm with Denavit-Hartenberg parameters of PR2
n_link_arm = NLinkArm([[0., -math.pi / 2, .1, 0.],
[math.pi / 2, math.pi / 2, 0., 0.],
[0., -math.pi / 2, 0., .4],
[0., math.pi / 2, 0., 0.],
[0., -math.pi / 2, 0., .321],
[0., math.pi / 2, 0., 0.], [0., 0., 0., 0.]])
# execute IK 10 times
for i in range(10):
n_link_arm.inverse_kinematics([
random_val(-0.5, 0.5),
random_val(-0.5, 0.5),
random_val(-0.5, 0.5),
random_val(-0.5, 0.5),
random_val(-0.5, 0.5),
random_val(-0.5, 0.5)
],
plot=True)
def get_random_goal():
from random import random
SAREA = N_LINKS
goal = [SAREA * random() - SAREA / 2.0, SAREA * random() - SAREA / 2.0]
print("goal position: ({}, {})".format(goal[0], goal[1]))
return goal
if __name__ == '__main__':
link_lengths = np.array([1] * N_LINKS)
joint_angles = np.array([0] * N_LINKS)
goal_pos = get_random_goal()
arm = NLinkArm(link_lengths, joint_angles, goal_pos)
joint_goal_angles, solution_found = inverse_kinematics(
link_lengths, joint_angles, goal_pos)
while True:
if solution_found:
joint_angles = joint_angles + Kp * ang_diff(
joint_goal_angles, joint_angles) * dt
arm.update_joints(joint_angles)
end_effector = arm.end_effector
errors, distance = distance_to_goal(end_effector, goal_pos)
if distance < DIS_THRE:
break
else:
print('\n')
print("Solution can not found!")