-
Notifications
You must be signed in to change notification settings - Fork 1
/
2 joint config link.py
47 lines (40 loc) · 1.81 KB
/
2 joint config link.py
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
# -*- coding: utf-8 -*-
"""
Created on Fri Jun 12 11:39:50 2020
@author: usth2
"""
import matplotlib.pyplot as plt
import numpy as np
# Define a function to compute the arm configuration
def compute_arm_config(link1_length, link2_length, joint0_angle, joint1_angle):
# Compute the (x, y) position of the p1 joint and the end effector at p2.
joint1_x = np.cos(joint0_angle) * link1_length
joint1_y = np.sin(joint0_angle) * link1_length
p2_x = joint1_x + np.cos(joint0_angle + joint1_angle) * link2_length
p2_y = joint1_y + np.sin(joint0_angle + joint1_angle) * link2_length
return joint1_x, joint1_y, p2_x, p2_y
# Generate random link lengths and joint angles
# Note: because these are randomly generated on each run
# Every time you run the code you'll get a different result!
link1_length = np.random.random() * 30 + 20
link2_length = np.random.random() * 30 + 20
joint0_angle = np.random.random() * 2 * np.pi
joint1_angle = np.random.random() * 2 * np.pi
joint1_x, joint1_y, p2_x, p2_y = compute_arm_config(link1_length, link2_length, joint0_angle, joint1_angle)
print("joint0_angle =", round(joint0_angle * 180 / np.pi, 1), "degrees")
print("joint1_angle =", round(joint1_angle * 180 / np.pi, 1),"degrees")
print("End Effector at x =", round(p2_x, 1),"y =", round(p2_y, 1))
base_x = 0
base_y = 0
# Plot the links
plt.plot([base_x, joint1_x, p2_x], [base_y, joint1_y, p2_y])
# Plot the base as a blue square
plt.plot(base_x, base_y, 'bs', markersize=15, label='Base')
# Plot Joint-1 as a red circle
plt.plot(joint1_x, joint1_y, 'ro', markersize=15, label='Joint-1')
# Plot End Effector as a green triangle
plt.plot(p2_x, p2_y, 'g^', markersize=15, label='End Effector')
plt.xlim(-100, 100)
plt.ylim(-100, 100)
plt.legend(fontsize=15)
plt.show() #Uncomment to run locally