8000 Merge pull request #36 from petercorke/sam-dev · lordkeks/robotics-toolbox-python@ed98d24 · GitHub
[go: up one dir, main page]
Skip to content

Commit ed98d24

Browse files
Samuel-DrewSamuel-Drew
authored
Merge pull request petercorke#36 from petercorke/sam-dev
Sam dev
2 parents e145bd9 + a1cc4f7 commit ed98d24

File tree

5 files changed

+119
-15
lines changed

5 files changed

+119
-15
lines changed

‎roboticstoolbox/robot/Link.py

Lines changed: 11 additions & 14 deletions
Original file line numberDiff line numberDiff line change
@@ -1,10 +1,6 @@
11
"""
22
Link object.
3-
Python implementation by: Luis Fernando Lara Tobar and Peter Corke.
4-
Based on original Robotics Toolbox for Matlab code by Peter Corke.
5-
Permission to use and copy is granted provided that acknowledgement of
6-
the authors is made.
7-
@author: Luis Fernando Lara Tobar and Peter Corke
3+
Python implementation by Samuel Drew
84
"""
95

106
from numpy import *
@@ -19,8 +15,8 @@ class Link(list):
1915

2016
def __init__(self, *argv):
2117
"""
22-
%Link Create robot link object
23-
%
18+
Link Create robot link object
19+
2420
% This the class constructor which has several call signatures.
2521
%
2622
% L = Link() is a Link object with default parameters.
@@ -165,7 +161,7 @@ def __init__(self, *argv):
165161
# format input into argparse
166162
argstr = ""
167163
known = ['theta', 'a', 'd', 'alpha', 'G', 'B', 'Tc', 'Jm', 'I', 'm', 'r',
168-
'offset', 'qlim', 'type', 'convention', 'sym', 'flip']
164+
'offset', 'qlim', 'type', 'convention', 'sym', 'flip', 'help']
169165
for arg in argv:
170166
if arg in known:
171167
argstr += "--" + arg + " "
@@ -226,15 +222,15 @@ def __init__(self, *argv):
226222

227223
if opt.theta != 0:
228224
# constant value of theta means it must be prismatic
229-
self.theta = opt.theta
230225
self.jointtype = 'P'
231226
print('Prismatic joint, theta =', opt.theta)
232227
if opt.d != 0:
233228
# constant value of d means it must be revolute
234-
self.d = opt.d
235229
self.jointtype = 'R'
236230
print('Revolute joint, d =', opt.d)
237231

232+
self.theta = opt.theta
233+
self.d = opt.d
238234
self.a = opt.a
239235
self.alpha = opt.alpha
240236

@@ -291,10 +287,11 @@ def __init__(self, *argv):
291287
self.B = 0
292288
self.Tc = [0, 0]
293289
self.qlim = 0
294-
if argv[1] == 'modified':
295-
self.mdh = 1
296-
else:
297-
self.mdh = 0
290+
291+
self.mdh = 0
292+
if len(argv) == 2:
293+
if argv[1] == 'modified':
294+
self.mdh == 1
298295

299296
def __repr__(self):
300297

‎roboticstoolbox/robot/serial_link.py

Lines changed: 33 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -4,6 +4,7 @@
44
from roboticstoolbox.robot.Link import *
55
from spatialmath.pose3d import *
66
from scipy.optimize import minimize
7+
from graphics.graphics_test_features import *
78

89
class SerialLink:
910

@@ -95,7 +96,7 @@ def fkine(self, jointconfig, unit='rad', alltout=False):
9596
t[k] = tt * SE3(self.tool)
9697

9798
if alltout:
98-
return t, allt
99+
return allt
99100
else:
100101
return t
101102

@@ -108,6 +109,8 @@ def ikine(self, T, q0=None, unit='rad'):
108109
:return: a list of 6 joint angles.
109110
"""
110111
assert T.shape == (4, 4)
112+
if type(T) == SE3:
113+
T = T.A
111114
bounds = [(link.qlim[0], link.qlim[1]) for link in self]
112115
reach = 0
113116
for link in self:
@@ -125,3 +128,32 @@ def objective(x):
125128
return sol.x * 180 / pi
126129
else:
127130
return sol.x
131+
132+
def plot(self, jointconfig, unit='rad'):
133+
"""
134+
Creates a 3D plot of the robot in your web browser
135+
:param jointconfig: takes an array or list of joint angles
136+
:param unit: unit of angles. radians if not defined
137+
:return: a vpython robot object.
138+
"""
139+
if type(jointconfig) == list:
140+
jointconfig = argcheck.getvector(jointconfig)
141+
if unit == 'deg':
142+
jointconfig = jointconfig * pi / 180
143+
if jointconfig.size == self.length:
144+
poses = self.fkine(jointconfig, unit, alltout=True)
145+
t = list(range(len(poses)))
146+
for i in range(len(poses)):
147+
t[i] = poses[i].t
148+
# add the base
149+
t.insert(0, SE3(self.base).t)
150+
grjoints = list(range(len(t)-1))
151+
152+
canvas_grid = init_canvas()
153+
154+
for i in range(1, len(t)):
155+
grjoints[i-1] = RotationalJoint(vector(t[i-1][0], t[i-1][1], t[i-1][2]), vector(t[i][0], t[i][1], t[i][2]))
156+
print(grjoints)
157+
x = GraphicalRobot(grjoints)
158+
159+
return x

‎roboticstoolbox/robot/threelink.py

Lines changed: 24 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,24 @@
1+
"""
2+
Defines the object 'tl' in the current workspace
3+
4+
Also define the vector qz = [0 0 0] which corresponds to the zero joint
5+
angle configuration.
6+
7+
@author: Luis Fernando Lara Tobar and Peter Corke
8+
9+
Edited June 2020 by Samuel Drew
10+
"""
11+
12+
from roboticstoolbox.robot.serial_link import *
13+
14+
L = []
15+
16+
L.append(Link('a', 1, 'type', 'revolute'))
17+
L.append(Link('a', 1, 'type', 'revolute'))
18+
L.append(Link('a', 1, 'type', 'revolute'))
19+
20+
21+
qz = [0,0,0]
22+
23+
tl = SerialLink(L, name='Simple three link')
24+

‎roboticstoolbox/robot/uprighttl.py

Lines changed: 19 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,19 @@
1+
"""
2+
Defines the object 'arm' in the current workspace
3+
4+
Also define the vector qz = [pi/4,0,-pi/3]
5+
"""
6+
7+
from roboticstoolbox.robot.serial_link import *
8+
9+
L = []
10+
11+
L.append(Link('a', 0.1, 'd', 1, 'alpha', pi/2, 'type', 'revolute'))
12+
L.append(Link('a', 1, 'type', 'revolute'))
13+
L.append(Link('a', 0.5, 'type', 'revolute'))
14+
15+
16+
qz = [pi/4,0,-pi/3]
17+
18+
arm = SerialLink(L, name='Upright Arm')
19+

‎tests/Sam1.py

Lines changed: 32 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,32 @@
1+
"""
2+
NOT A UNIT TEST
3+
Simple Example for Serial_link plot function
4+
Starts by creating a robot arm.
5+
uncomment and copy code to console as needed
6+
"""
7+
8+
from roboticstoolbox.robot.serial_link import *
9+
10+
L = []
11+
12+
# create links
13+
L.append(Link('a', 0.1, 'd', 1, 'alpha', pi/2, 'type', 'revolute'))
14+
L.append(Link('a', 1, 'type', 'revolute'))
15+
L.append(Link('a', 0.5, 'type', 'revolute'))
16+
17+
# create initial joint array to be passed into plot as joint configuration
18+
qz = [pi/4,0,-pi/3]
19+
20+
# create serial link robot
21+
arm = SerialLink(L, name='Upright Arm')
22+
23+
# plot robot
24+
plotbot = arm.plot(qz)
25+
26+
## Use this code to change the joint angles
27+
## lift arm
28+
# plotbot.set_joint_angle(1, -pi/2)
29+
30+
## rotate base:
31+
# plotbot.set_joint_angle(0, pi/2)
32+

0 commit comments

Comments
 (0)
0