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

fix test

Browse Source
This commit is contained in:
Atsushi Sakai
2018-11-23 14:25:43 +09:00
parent 98cb5514ab
commit 404cc79e66
2 changed files with 7 additions and 7 deletions
Download Patch File Download Diff File Expand all files Collapse all files

0
ArmNavigation/__init__.py Normal file
View File

14
ArmNavigation/n_joint_arm_to_point_control/n_joint_arm_to_point_control.py
View File

@@ -34,7 +34,7 @@ def main():
state = WAIT_FOR_NEW_GOAL state = WAIT_FOR_NEW_GOAL
solution_found = False solution_found = False
while True: while True:
old_goal = goal_pos old_goal = np.ndarray(goal_pos)
goal_pos = arm.goal goal_pos = arm.goal
end_effector = arm.end_effector end_effector = arm.end_effector
errors, distance = distance_to_goal(end_effector, goal_pos) errors, distance = distance_to_goal(end_effector, goal_pos)
@@ -51,7 +51,7 @@ def main():
elif solution_found: elif solution_found:
state = MOVING_TO_GOAL state = MOVING_TO_GOAL
elif state is MOVING_TO_GOAL: elif state is MOVING_TO_GOAL:
if distance > 0.1 and (old_goal == goal_pos).all(): if distance > 0.1 and all(old_goal == goal_pos):
joint_angles = joint_angles + Kp * \ joint_angles = joint_angles + Kp * \
ang_diff(joint_goal_angles, joint_angles) * dt ang_diff(joint_goal_angles, joint_angles) * dt
else: else:
@@ -93,8 +93,8 @@ def animation():
i_goal = 0 i_goal = 0
while True: while True:
old_goal = goal_pos old_goal = np.array(goal_pos)
goal_pos = arm.goal goal_pos = np.array(arm.goal)
end_effector = arm.end_effector end_effector = arm.end_effector
errors, distance = distance_to_goal(end_effector, goal_pos) errors, distance = distance_to_goal(end_effector, goal_pos)
@@ -111,7 +111,7 @@ def animation():
elif solution_found: elif solution_found:
state = MOVING_TO_GOAL state = MOVING_TO_GOAL
elif state is MOVING_TO_GOAL: elif state is MOVING_TO_GOAL:
if distance > 0.1 and (old_goal == goal_pos).all(): if distance > 0.1 and all(old_goal == goal_pos):
joint_angles = joint_angles + Kp * \ joint_angles = joint_angles + Kp * \
ang_diff(joint_goal_angles, joint_angles) * dt ang_diff(joint_goal_angles, joint_angles) * dt
else: else:
@@ -160,5 +160,5 @@ def ang_diff(theta1, theta2):
if __name__ == '__main__': if __name__ == '__main__':
main() # main()
# animation() animation()