github/PythonRobotics
1
1
Fork 0
mirror of https://github.com/AtsushiSakai/PythonRobotics.git synced 2026-02-18 01:11:33 -05:00
Code Issues Packages Projects Releases Wiki Activity

mypy fix test

Browse Source
This commit is contained in:
Atsushi Sakai
2020-03-09 22:57:35 +09:00
parent c36aa27d60
commit 14ffc4a2d4
13 changed files with 150 additions and 366 deletions
Download Patch File Download Diff File Expand all files Collapse all files

21
ArmNavigation/n_joint_arm_3d/random_forward_kinematics.py
View File

@@ -3,9 +3,10 @@ Forward Kinematics for an n-link arm in 3D
Author: Takayuki Murooka (takayuki5168)
"""
import math
from NLinkArm import NLinkArm
from NLinkArm3d import NLinkArm
import random
def random_val(min_val, max_val):
return min_val + random.random() * (max_val - min_val)
@@ -14,17 +15,17 @@ 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.],
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)
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)