10000 Add dh parameter visualisation · lmark1/robotics-toolbox-python@c6cd6a9 · GitHub
[go: up one dir, main page]
Skip to content

Commit c6cd6a9

Browse files
lmark1lmark1
committed
Add dh parameter visualisation
1 parent bc1d40d commit c6cd6a9

File tree

3 files changed

+218
-0
lines changed

3 files changed

+218
-0
lines changed

‎dh_camera.py

Lines changed: 73 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,73 @@
1+
#!/usr/bin/env python
2+
3+
import numpy as np
4+
from roboticstoolbox import DHRobot, SerialLink, DHLink
5+
import matplotlib.pyplot as plt
6+
from matplotlib.widgets import Slider
7+
8+
a_0 = 0.1
9+
a_1 = 0.1
10+
a_2 = 0.1
11+
a_3 = 0.1
12+
a_4 = 0.1
13+
a_5 = 0.1
14+
a_6 = 0.05
15+
16+
# Create a list of links
17+
links = [ \
18+
DHLink(alpha=np.pi/2, a=0, d=0., theta=-np.pi/2, sigma=1), \
19+
DHLink(alpha=0, a=-a_1, d=0., theta=0, sigma=0), \
20+
DHLink(alpha=np.pi/2, a=0, d=a_2, theta=0, sigma=0), \
21+
DHLink(alpha=np.pi/2, a=0, d=a_3, theta=0, sigma=0), \
22+
DHLink(alpha=np.pi, a=0, d=0, theta=0, sigma=0), \
23+
]
24+
25+
# Theta
26+
q0 = np.zeros(len(links))
27+
if len(q0) > 2:
28+
q0[2] = np.pi/2
29+
# q0[4] = -np.pi/2
30+
31+
# Create a DHRobot object using the list of links
32+
robot = DHRobot(links)
33+
34+
# Create a SerialLink object from the DHRobot object
35+
robot_link = SerialLink(links)
36+
37+
# Define the initial joint configuration for the robot
38+
q = q0
39+
40+
# Create a 3D plot
41+
fig = plt.figure()
42+
ax = fig.add_subplot(111, projection='3d')
43+
44+
# Define the slider axes
45+
slider_axes = []
46+
cnt = -1
47+
ignore = [0, 2, 4, 7]
48+
for i in range(len(q0)):
49+
offset = 0
50+
if i in ignore:
51+
offset = 500
52+
else:
53+
cnt = cnt + 1
54+
slider_ax = plt.axes([0.7 + offset, 0.1 + cnt*0.03, 0.2, 0.03])
55+
slider_axes.append(slider_ax)
56+
57+
# Define the sliders
58+
sliders = []
59+
for i in range(len(q0)):
60+
slider = Slider(slider_axes[i], f'q{i+1}', -np.pi, np.pi, valinit=q0[i], zorder=10)
61+
sliders.append(slider)
62+
63+
# Update the robot when the slider values are changed
64+
def update(val):
65+
q = np.array([slider.val for slider in sliders])
66+
robot_link.plot(q, block=False, fig=fig, dt=1.0)
67+
68+
# Connect the sliders to the update function
69+
for slider in sliders:
70+
slider.on_changed(update)
71+
72+
# Visualize the robot in a GUI window
73+
robot_link.plot(q, block=True, fig=fig, dt=1.0)

‎dh_manip.py

Lines changed: 74 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,74 @@
1+
#!/usr/bin/env python
2+
3+
import numpy as np
4+
from roboticstoolbox import DHRobot, SerialLink, DHLink
5+
import matplotlib.pyplot as plt
6+
from matplotlib.widgets import Slider
7+
8+
a_0 = 0.1
9+
a_1 = 0.1
10+
a_2 = 0.1
11+
a_3 = 0.1
12+
a_4 = 0.1
13+
a_5 = 0.1
14+
a_6 = 0.05
15+
16+
# Create a list of links
17+
links = [ \
18+
DHLink(alpha=0, a=a_0, d=0., theta=0, sigma=1), \
19+
DHLink(alpha=0, a=a_1, d=0., theta=np.pi/2.0, sigma=1), \
20+
DHLink(alpha=0, a=0, d=-a_2, theta=0, sigma=0), \
21+
DHLink(alpha=np.pi/2, a=0, d=0, theta=np.pi/2, sigma=1), \
22+
DHLink(alpha=0, a=a_3, d=0, theta=0, sigma=0), \
23+
DHLink(alpha=0, a=a_4, d=0, theta=0, sigma=0), \
24+
DHLink(alpha=0, a=a_5, d=0, theta=0, sigma=0), \
25+
DHLink(alpha=np.pi/2, a=0, d=0, theta=np.pi/2, sigma=1)
26+
]
27+
28+
# Theta
29+
q0 = np.zeros(len(links))
30+
q0[4] = -np.pi/2
31+
32+
# Create a DHRobot object using the list of links
33+
robot = DHRobot(links)
34+
35+
# Create a SerialLink object from the DHRobot object
36+
robot_link = SerialLink(links)
37+
38+
# Define the initial joint configuration for the robot
39+
q = q0
40+
41+
# Create a 3D plot
42+
fig = plt.figure()
43+
ax = fig.add_subplot(111, projection='3d')
44+
45+
# Define the slider axes
46+
slider_axes = []
47+
cnt = -1
48+
ignore = [0, 1, 3, 7]
49+
for i in range(len(q0)):
50+
offset = 0
51+
if i in ignore:
52+
offset = 500
53+
else:
54+
cnt = cnt + 1
55+
slider_ax = plt.axes([0.7 + offset, 0.1 + cnt*0.03, 0.2, 0.03])
56+
slider_axes.append(slider_ax)
57+
58+
# Define the sliders
59+
sliders = []
60+
for i in range(len(q0)):
61+
slider = Slider(slider_axes[i], f'q{i+1}', -np.pi, np.pi, valinit=q0[i], zorder=10)
62+
sliders.append(slider)
63+
64+
# Update the robot when the slider values are changed
65+
def update(val):
66+
q = np.array([slider.val for slider in sliders])
67+
robot_link.plot(q, block=False, fig=fig, dt=1.0)
68+
69+
# Connect the sliders to the update function
70+
for slider in sliders:
71+
slider.on_changed(update)
72+
73+
# Visualize the robot in a GUI window
74+
robot_link.plot(q, block=True, fig=fig, dt=1.0)

‎dh_tilt_rotor.py

Lines changed: 71 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,71 @@
1+
#!/usr/bin/env python
2+
3+
import numpy as np
4+
from roboticstoolbox import DHRobot, SerialLink, DHLink
5+
import matplotlib.pyplot as plt
6+
from matplotlib.widgets import Slider
7+
8+
a_0 = 0.1
9+
a_1 = 0.1
10+
a_2 = 0.1
11+
a_3 = 0.1
12+
a_4 = 0.1
13+
a_5 = 0.1
14+
a_6 = 0.05
15+
16+
# Create a list of links
17+
links = [ \
18+
DHLink(alpha=-np.pi/2, a=0, d=0., theta=0, sigma=0), \
19+
DHLink(alpha=0, a=a_1, d=0., theta=0, sigma=0), \
20+
DHLink(alpha=0, a=0, d=0., theta=0, sigma=0), \
21+
DHLink(alpha=np.pi/2, a=0, d=0., theta=np.pi/2, sigma=1) \
22+
# DHLink(alpha=0, a=-a_2, d=0., theta=0, sigma=0), \
23+
# DHLink(alpha=np.pi/2, a=0, d=0., theta=-np.pi/2, sigma=1) \
24+
]
25+
26+
# Theta
27+
q0 = np.zeros(len(links))
28+
29+
# Create a DHRobot object using the list of links
30+
robot = DHRobot(links)
31+
32+
# Create a SerialLink object from the DHRobot object
33+
robot_link = SerialLink(links)
34+
35+
# Define the initial joint configuration for the robot
36+
q = q0
37+
38+
# Create a 3D plot
39+
fig = plt.figure()
40+
ax = fig.add_subplot(111, projection='3d')
41+
42+
# Define the slider axes
43+
slider_axes = []
44+
cnt = -1
45+
ignore = [3, 5]
46+
for i in range(len(q0)):
47+
offset = 0
48+
if i in ignore:
49+
offset = 500
50+
else:
51+
cnt = cnt + 1
52+
slider_ax = plt.axes([0.7 + offset, 0.1 + cnt*0.03, 0.2, 0.03])
53+
slider_axes.append(slider_ax)
54+
55+
# Define the sliders
56+
sliders = []
57+
for i in range(len(q0)):
58+
slider = Slider(slider_axes[i], f'q{i+1}', -np.pi, np.pi, valinit=q0[i], zorder=10)
59+
sliders.append(slider)
60+
61+
# Update the robot when the slider values are changed
62+
def update(val):
63+
q = np.array([slider.val for slider in sliders])
64+
robot_link.plot(q, block=False, fig=fig, dt=1.0)
65+
66+
# Connect the sliders to the update function
67+
for slider in sliders:
68+
slider.on_changed(update)
69+
70+
# Visualize the robot in a GUI window
71+
robot_link.plot(q, block=True, fig=fig, dt=1.0)

0 commit comments

Comments
 (0)
0