mirror of
https://github.com/AtsushiSakai/PythonRobotics.git
synced 2026-02-16 15:15:17 -05:00
36 lines
1.2 KiB
Python
36 lines
1.2 KiB
Python
"""
|
|
Inverse Kinematics for an n-link arm in 3D
|
|
Author: Takayuki Murooka (takayuki5168)
|
|
"""
|
|
import math
|
|
from NLinkArm3d import NLinkArm
|
|
import random
|
|
|
|
|
|
def random_val(min_val, max_val):
|
|
return min_val + random.random() * (max_val - min_val)
|
|
|
|
|
|
def 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 _ 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)
|
|
|
|
|
|
if __name__ == "__main__":
|
|
main()
|