github/PythonRobotics
1
1
Fork 0
mirror of https://github.com/AtsushiSakai/PythonRobotics.git synced 2026-04-22 03:00:22 -04:00
Code Issues Packages Projects Releases Wiki Activity

upgrade numpy to 1.25.0 and fix bugs (#861)

Browse Source
This commit is contained in:
Atsushi Sakai
2023-07-01 15:30:32 +09:00
committed by GitHub
parent 67edaf8b02
commit 4d71470631
18 changed files with 66 additions and 58 deletions
Download Patch File Download Diff File Expand all files Collapse all files

11
ArmNavigation/two_joint_arm_to_point_control/two_joint_arm_to_point_control.py
View File

@@ -14,9 +14,10 @@ Ref: P. I. Corke, "Robotics, Vision & Control", Springer 2017,
import matplotlib.pyplot as plt
import numpy as np
import math
# Similation parameters
# Simulation parameters
Kp = 15
dt = 0.01
@@ -50,15 +51,15 @@ 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))
tmp = np.math.atan2(l2 * np.sin(theta2_goal),
tmp = math.atan2(l2 * np.sin(theta2_goal),
(l1 + l2 * np.cos(theta2_goal)))
theta1_goal = np.math.atan2(y, x) - tmp
theta1_goal = math.atan2(y, x) - tmp
if theta1_goal < 0:
theta2_goal = -theta2_goal
tmp = np.math.atan2(l2 * np.sin(theta2_goal),
tmp = math.atan2(l2 * np.sin(theta2_goal),
(l1 + l2 * np.cos(theta2_goal)))
theta1_goal = np.math.atan2(y, x) - tmp
theta1_goal = math.atan2(y, x) - tmp
theta1 = theta1 + Kp * ang_diff(theta1_goal, theta1) * dt
theta2 = theta2 + Kp * ang_diff(theta2_goal, theta2) * dt