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

use pytest for test runner (#452)

Browse Source
This commit is contained in:
Atsushi Sakai
2020-12-31 22:54:40 +09:00
committed by GitHub
parent 581b3cf27a
commit c2debe3b05
8 changed files with 151 additions and 112 deletions
Download Patch File Download Diff File Expand all files Collapse all files

26
ArmNavigation/two_joint_arm_to_point_control/two_joint_arm_to_point_control.py
View File

@@ -5,8 +5,10 @@ Left-click the plot to set the goal position of the end effector
Author: Daniel Ingram (daniel-s-ingram)
Atsushi Sakai (@Atsushi_twi)
Ref: P. I. Corke, "Robotics, Vision & Control", Springer 2017, ISBN 978-3-319-54413-7 p102
- [Robotics, Vision and Control \| SpringerLink](https://link.springer.com/book/10.1007/978-3-642-20144-8)
Ref: P. I. Corke, "Robotics, Vision & Control", Springer 2017,
ISBN 978-3-319-54413-7 p102
- [Robotics, Vision and Control]
(https://link.springer.com/book/10.1007/978-3-642-20144-8)
"""
@@ -37,6 +39,7 @@ def two_joint_arm(GOAL_TH=0.0, theta1=0.0, theta2=0.0):
When out of bounds, rewrite x and y with last correct values
"""
global x, y
x_prev, y_prev = None, None
while True:
try:
if x is not None and y is not None:
@@ -47,18 +50,20 @@ def two_joint_arm(GOAL_TH=0.0, theta1=0.0, theta2=0.0):
else:
theta2_goal = np.arccos(
(x**2 + y**2 - l1**2 - l2**2) / (2 * l1 * l2))
theta1_goal = np.math.atan2(y, x) - np.math.atan2(l2 *
np.sin(theta2_goal), (l1 + l2 * np.cos(theta2_goal)))
tmp = np.math.atan2(l2 * np.sin(theta2_goal),
(l1 + l2 * np.cos(theta2_goal)))
theta1_goal = np.math.atan2(y, x) - tmp
if theta1_goal < 0:
theta2_goal = -theta2_goal
theta1_goal = np.math.atan2(
y, x) - np.math.atan2(l2 * np.sin(theta2_goal), (l1 + l2 * np.cos(theta2_goal)))
tmp = np.math.atan2(l2 * np.sin(theta2_goal),
(l1 + l2 * np.cos(theta2_goal)))
theta1_goal = np.math.atan2(y, x) - tmp
theta1 = theta1 + Kp * ang_diff(theta1_goal, theta1) * dt
theta2 = theta2 + Kp * ang_diff(theta2_goal, theta2) * dt
except ValueError as e:
print("Unreachable goal")
print("Unreachable goal"+e)
except TypeError:
x = x_prev
y = y_prev
@@ -66,6 +71,7 @@ def two_joint_arm(GOAL_TH=0.0, theta1=0.0, theta2=0.0):
wrist = plot_arm(theta1, theta2, x, y)
# check goal
d2goal = None
if x is not None and y is not None:
d2goal = np.hypot(wrist[0] - x, wrist[1] - y)
@@ -73,7 +79,7 @@ def two_joint_arm(GOAL_TH=0.0, theta1=0.0, theta2=0.0):
return theta1, theta2
def plot_arm(theta1, theta2, x, y): # pragma: no cover
def plot_arm(theta1, theta2, target_x, target_y): # pragma: no cover
shoulder = np.array([0, 0])
elbow = shoulder + np.array([l1 * np.cos(theta1), l1 * np.sin(theta1)])
wrist = elbow + \
@@ -89,8 +95,8 @@ def plot_arm(theta1, theta2, x, y): # pragma: no cover
plt.plot(elbow[0], elbow[1], 'ro')
plt.plot(wrist[0], wrist[1], 'ro')
plt.plot([wrist[0], x], [wrist[1], y], 'g--')
plt.plot(x, y, 'g*')
plt.plot([wrist[0], target_x], [wrist[1], target_y], 'g--')
plt.plot(target_x, target_y, 'g*')
plt.xlim(-2, 2)
plt.ylim(-2, 2)