Skip to content

Commit 6a26c04

Browse files
Samuel-DrewSamuel-Drew
authored
Merge pull request petercorke#116 from petercorke/Sam-dev-3
Fixes made to graphics_robot for indexing issues
2 parents 92fc79e + b61857a commit 6a26c04

File tree

6 files changed

+53
-85
lines changed

6 files changed

+53
-85
lines changed

‎graphics/graphics_canvas.py

Lines changed: 5 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -453,6 +453,9 @@ def __setup_joint_sliders(self):
453453
"""
454454
i = 1
455455
for joint in self.__teachpanel[self.__selected_robot]:
456+
if joint[self.__idx_qlim_min] == joint[self.__idx_qlim_max]:
457+
# If a slider with (effectively) no values, skip it
458+
continue
456459
# Add a title
457460
self.scene.append_to_caption('Joint {0}:\t'.format(i))
458461
i += 1
@@ -597,12 +600,11 @@ def __joint_slider(self, s):
597600

598601
# Get all angles for the robot
599602
angles = []
600-
for joint_slider in self.__teachpanel_sliders:
601-
angles.append(joint_slider.value)
603+
for idx in range(len(self.__teachpanel_sliders)):
604+
angles.append(self.__teachpanel_sliders[idx].value)
602605

603606
# Run fkine
604607
poses = self.__robots[self.__selected_robot].fkine(angles)
605-
poses = poses[1:len(poses)] # Ignore the first item
606608

607609
# Update joints
608610
self.__robots[self.__selected_robot].set_joint_poses(poses)

‎graphics/graphics_robot.py

Lines changed: 22 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -3,6 +3,7 @@
33
from graphics.common_functions import *
44
from graphics.graphics_stl import set_stl_origin, import_object_from_numpy_stl
55
from time import perf_counter
6+
from spatialmath import SE3
67

78

89
class DefaultJoint:
@@ -474,16 +475,23 @@ def __init__(self, graphics_canvas, name, seriallink=None):
474475
# Get initial poses
475476
zero_angles = [0] * len(self.seriallink.links)
476477
all_poses = self.seriallink.fkine(zero_angles, alltout=True)
478+
# Create the base
479+
if seriallink.basemesh is not None:
480+
self.append_link("s", all_poses[0], str(seriallink.basemesh), [0, 0], 0)
481+
# else: assume no base joint
477482
# Create the joints
478483
i = 0
479484
for link in self.seriallink.links:
480485
# Get info
481486
j_type = link.jointtype # Type of
482-
pose = all_poses[i + 1] # Pose
483-
x1, x2 = all_poses[i].t[0], all_poses[i + 1].t[0]
484-
y1, y2 = all_poses[i].t[1], all_poses[i + 1].t[1]
485-
z1, z2 = all_poses[i].t[2], all_poses[i + 1].t[2]
486-
length = sqrt((x2 - x1) * (x2 - x1) + (y2 - y1) * (y2 - y1) + (z2 - z1) * (z2 - z1)) # Length
487+
pose = all_poses[i+1] # Pose
488+
if link.mesh is None:
489+
x1, x2 = all_poses[i].t[0], all_poses[i + 1].t[0]
490+
y1, y2 = all_poses[i].t[1], all_poses[i + 1].t[1]
491+
z1, z2 = all_poses[i].t[2], all_poses[i + 1].t[2]
492+
length = sqrt((x2 - x1) * (x2 - x1) + (y2 - y1) * (y2 - y1) + (z2 - z1) * (z2 - z1)) # Length
493+
else:
494+
length = str(link.mesh)
487495
angle_lims = link.qlim # Angle limits
488496
theta = link.theta # Current angle
489497
i += 1
@@ -616,12 +624,20 @@ def set_joint_poses(self, all_poses):
616624
if self.num_joints == 0:
617625
raise UserWarning("Robot has 0 joints. Create some using append_link()")
618626

627+
# If given a base pose, when there isn't one
628+
if self.num_joints == len(all_poses) - 1 and all_poses[0] == SE3():
629+
# Update the joints
630+
for idx in range(self.num_joints):
631+
self.joints[idx].update_pose(all_poses[idx+1])
632+
return
633+
634+
# If incorrect number of joints
619635
if self.num_joints != len(all_poses):
620636
err = "Number of given poses {0} does not equal number of joints {1}"
621637
raise UserWarning(err.format(len(all_poses), self.num_joints))
622638

623639
# Update the joints
624-
for idx in range(0, self.num_joints):
640+
for idx in range(self.num_joints):
625641
self.joints[idx].update_pose(all_poses[idx])
626642

627643
def animate(self, frame_poses, fps):

‎roboticstoolbox/models/Puma560.py

Lines changed: 7 additions & 57 deletions
Original file line numberDiff line numberDiff line change
@@ -80,7 +80,7 @@ def __init__(self):
8080
Tc=[0.395, -0.435], # actuator Coulomb friction coefficient for
8181
# direction [-,+] (measured at the motor)
8282
qlim=[-160*deg, 160*deg], # minimum and maximum joint angle
83-
mesh='UNIMATE/puma560/link0.stl')
83+
mesh='UNIMATE/puma560/link1.stl')
8484

8585
L1 = RevoluteDH(
8686
d=0, a=0.4318, alpha=0,
@@ -89,7 +89,7 @@ def __init__(self):
8989
m=17.4, Jm=200e-6, G=107.815,
9090
B=.817e-3, Tc=[0.126, -0.071],
9191
qlim=[-45*deg, 225*deg],
92-
mesh='UNIMATE/puma560/link1.stl')
92+
mesh='UNIMATE/puma560/link2.stl')
9393

9494
L2 = RevoluteDH(
9595
d=0.15005, a=0.0203, alpha=-pi/2,
@@ -98,7 +98,7 @@ def __init__(self):
9898
m=4.8, Jm=200e-6, G=-53.7063,
9999
B=1.38e-3, Tc=[0.132, -0.105],
100100
qlim=[-225*deg, 45*deg],
101-
mesh='UNIMATE/puma560/link2.stl')
101+
mesh='UNIMATE/puma560/link3.stl')
102102

103103
L3 = RevoluteDH(
104104
d=0.4318, a=0, alpha=pi/2,
@@ -107,7 +107,7 @@ def __init__(self):
107107
m=0.82, Jm=33e-6, G=76.0364,
108108
B=71.2e-6, Tc=[11.2e-3, -16.9e-3],
109109
qlim=[-110*deg, 170*deg],
110-
mesh='UNIMATE/puma560/link3.stl')
110+
mesh='UNIMATE/puma560/link4.stl')
111111

112112
L4 = RevoluteDH(
113113
d=0, a=0, alpha=-pi/2,
@@ -116,15 +116,15 @@ def __init__(self):
116116
Jm=33e-6, G=71.923, B=82.6e-6,
117117
Tc=[9.26e-3, -14.5e-3],
118118
qlim=[-100*deg, 100*deg],
119-
mesh='UNIMATE/puma560/link4.stl')
119+
mesh='UNIMATE/puma560/link5.stl')
120120

121121
L5 = RevoluteDH(
122122
d=0, a=0, alpha=0,
123123
I=[0.15e-3, 0.15e-3, 0.04e-3, 0, 0, 0],
124124
r=[0, 0, 0.032], m=0.09, Jm=33e-6,
125125
G=76.686, B=36.7e-6, Tc=[3.96e-3, -10.5e-3],
126126
qlim=[-266*deg, 266*deg],
127-
mesh='UNIMATE/puma560/link5.stl')
127+
mesh='UNIMATE/puma560/link6.stl')
128128

129129
L = [L0, L1, L2, L3, L4, L5]
130130

@@ -142,60 +142,10 @@ def __init__(self):
142142

143143
super(Puma560, self).__init__(
144144
L,
145-
toolmesh = "UNIMATE/puma560/link6.stl",
145+
basemesh = "UNIMATE/puma560/link0.stl",
146146
name="Puma 560",
147147
manufacturer="Unimation")
148148

149-
def plot(self, jointconfig, unit='rad'):
150-
"""
151-
Creates a 3D plot of the robot in your web browser
152-
:param jointconfig: takes an array or list of joint angles
153-
:param unit: unit of angles. radians if not defined
154-
:return: a vpython robot object.
155-
"""
156-
157-
if type(jointconfig) == list:
158-
jointconfig = argcheck.getvector(jointconfig)
159-
if unit == 'deg':
160-
jointconfig = jointconfig * pi / 180
161-
if jointconfig.size == self.length:
162-
poses = self.fkine(jointconfig, unit, alltout=True)
163-
164-
if self.roplot is None:
165-
# No current plot, create robot plot
166-
167-
self.g_canvas = gph.GraphicsCanvas3D()
168-
print("canvas created")
169-
170-
self.roplot = gph.GraphicalRobot(self.g_canvas, self.name)
171-
172-
colour = {
173-
0 : [0.5, 0.5, 0.5],
174-
1 : [1, 0, 0],
175-
2 : [0, 1, 0],
176-
3 : [0, 0, 1],
177-
4 : [1, 1, 0],
178-
5 : [0, 1, 1]
179-
}
180-
181-
for i in range(len(poses)):
182-
stl_obj_path = './roboticstoolbox/models/meshes/UNIMATE/puma560/link' + str(i) + '.stl'
183-
if i is 0:
184-
link = StaticJoint(SE3(), stl_obj_path, self.g_canvas, [0,0], 0)
185-
elif self.links[i-1].isrevolute():
186-
link = RotationalJoint(SE3(), stl_obj_path, self.g_canvas, self.links[i-1].qlim, jointconfig[i-1])
187-
# elif L[i-1].isprismatic():
188-
# link = PrismaticJoint(SE3(), stl_obj_path, self.g_canvas, self.links[i-1].qlim, jointconfig[i-1])
189-
190-
# Change color
191-
link.set_texture(colour=colour[i % 5])
192-
193-
self.roplot.append_made_link(link)
194-
195-
# Move plot
196-
self.roplot.set_joint_poses(poses)
197-
return
198-
199149
@property
200150
def qz(self):
201151
return self._qz

‎roboticstoolbox/robot/serial_link.py

Lines changed: 12 additions & 10 deletions
Original file line numberDiff line numberDiff line change
@@ -11,7 +11,7 @@
1111

1212
class SerialLink:
1313

14-
def __init__(self, links, name=None, base=None, tool=None, toolmesh=None, stl_files=None, q=None, param=None, manufacturer=None, comment=None, meshdir=None, configurations={}):
14+
def __init__(self, links, name=None, base=None, basemesh=None, tool=None, toolmesh=None, stl_files=None, q=None, param=None, manufacturer=None, comment=None, meshdir=None, configurations={}):
1515
"""
1616
Creates a SerialLink object.
1717
:param links: a list of links that will constitute SerialLink object.
@@ -43,6 +43,11 @@ def __init__(self, links, name=None, base=None, tool=None, toolmesh=None, stl_fi
4343
else:
4444
self.toolmesh = None
4545

46+
if basemesh:
47+
self.basemesh = PurePath(__file__).parent.parent / 'models' / 'meshes' / basemesh
48+
else:
49+
self.basemesh = None
50+
4651
# Following arguments initialised by plot function and animate functions only
4752
if stl_files is None:
4853
# Default stick figure model code goes here
@@ -180,17 +185,14 @@ def plot(self, jointconfig, unit='rad'):
180185

181186
if self.roplot is None:
182187
# No current plot, create robot plot
188+
self.g_canvas = gph.GraphicsCanvas3D()
189+
print("canvas created")
183190

184-
self.g_canvas = gph.GraphicsCanvas3D()
185-
print("canvas created")
191+
self.roplot = gph.GraphicalRobot(self.g_canvas, self.name, self)
186192

187-
self.roplot = gph.GraphicalRobot(self.g_canvas, self.name, self)
188-
189-
return
190-
else:
191-
# Move existing plot
192-
self.roplot.set_joint_poses(poses)
193-
return
193+
# Move existing plot
194+
self.roplot.set_joint_poses(poses)
195+
return
194196

195197
def animate(self, q1, q2, unit='rad', frames=10, fps=5):
196198
"""

‎roboticstoolbox/robot/uprighttl.py

Lines changed: 3 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -8,12 +8,13 @@
88

99
L = []
1010

11-
L.append(Link(a=0.1, d=1, alpha=pi/2, jointtype='R'))
11+
L.append(Link(a=0.1, alpha=pi/2, jointtype='R'))
12+
L.append(Link(a=1.0, jointtype='R'))
1213
L.append(Link(a=1.0, jointtype='R'))
1314
L.append(Link(a=0.5, jointtype='R'))
1415

1516

16-
qz = [0,0,-pi/3]
17+
qz = [0,0,0,0]
1718

1819
arm = SerialLink(L, name='Upright Arm')
1920

‎tests/_test_teachpanel.py

Lines changed: 4 additions & 7 deletions
Original file line numberDiff line numberDiff line change
@@ -3,16 +3,13 @@
33
from roboticstoolbox.models.Puma560 import Puma560
44

55
if __name__ == "__main__":
6-
# canvas = gph.GraphicsCanvas3D()
7-
8-
# Puma560
9-
puma560 = Puma560()
106

11-
puma560.plot(puma560.qz)
7+
puma = Puma560()
128

13-
# g_puma560 = gph.GraphicalRobot(canvas, '', seriallink=puma560)
9+
puma.plot(puma.qz)
1410

15-
# Two three-joint arms
11+
# canvas = gph.GraphicsCanvas3D()
12+
#
1613
# L = []
1714
# L2 = []
1815
#

0 commit comments

Comments
 (0)