8000 Merge branch 'master' of github.com:petercorke/robotics-toolbox-python · yobzhuu/robotics-toolbox-python@ebc1035 · GitHub
[go: up one dir, main page]
Skip to content

Commit ebc1035

Browse files
jhavljhavl
committed
Merge branch 'master' of github.com:petercorke/robotics-toolbox-python
2 parents 6e97f70 + 7cfa754 commit ebc1035

File tree

24 files changed

+406
-285
lines changed

24 files changed

+406
-285
lines changed

‎examples/VPython.py

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -12,7 +12,7 @@
1212

1313
# PUMA560
1414
puma = rtb.models.DH.Puma560()
15-
env.add(0, 'Puma', puma)
15+
env.add(puma)
1616

1717
for i in range(1000):
1818
env.step(puma)

‎examples/baur.py

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -89,4 +89,4 @@
8989
panda.qd[:n] = qd[:n]
9090

9191
# Step the simulator by 50 ms
92-
env.step(50)
92+
env.step(0.05)

‎examples/neo.py

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -108,8 +108,8 @@
108108
# object on the robot to the collision in the scene
109109
c_Ain, c_bin = panda.link_collision_damper(
110110
collision, panda.q[:n], 0.3, 0.05, 1.0,
111-
startlink=panda.link_dict['panda_link1'],
112-
endlink=panda.link_dict['panda_hand'])
111+
start=panda.link_dict['panda_link1'],
112+
end=panda.link_dict['panda_hand'])
113113

114114
# If there are any parts of the robot within the influence distance
115115
# to the collision in the scene

‎examples/park.py

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -67,4 +67,4 @@
6767
panda.qd[:n] = qd[:n]
6868

6969
# Step the simulator by 50 ms
70-
env.step(50)
70+
env.step(0.05)

‎examples/puma_jtraj.py

Lines changed: 40 additions & 12 deletions
Original file line numberDiff line numberDiff line change
@@ -1,18 +1,46 @@
11
import roboticstoolbox as rtb
22
from spatialmath import * # lgtm [py/polluting-import]
3+
import argparse
4+
import sys
35

6+
parser = argparse.ArgumentParser(description="Puma trajectory demo")
7+
parser.add_argument('--backend', '-b', dest='backend', default='pyplot',
8+
help='choose backend: pyplot (default), swift, vpython', action='store')
9+
parser.add_argument('--model', '-m', dest='model', default='DH', action='store',
10+
help='choose model: DH (default), URDF')
11+
args = parser.parse_args()
12+
13+
if args.model.lower() == 'dh':
14+
robot = rtb.models.DH.Puma560()
15+
elif args.model.lower() == 'urdf':
16+
robot = rtb.models.URDF.Puma560()
17+
else:
18+
raise ValueError('unknown model')
419

5-
#robot = rtb.models.URDF.Puma560()
6-
robot = rtb.models.DH.Puma560()
720
print(robot)
8-
print(robot.fkine(robot.qz))
9-
# tw, T0 = p560.twists(p560.qn)
10-
# print(tw)
21+
1122
qt = rtb.tools.trajectory.jtraj(robot.qz, robot.qr, 200)
12-
robot.plot(qt.q, dt=0.1, block=True) #movie='puma_sitting.gif')
13-
#swift = rtb.backend.Swift()
14-
#swift.launch()
15-
#swift.add(robot)
16-
#for q in qt.q:
17-
#robot.q = q
18-
#swift.step()
23+
24+
if args.backend.lower() == 'pyplot':
25+
if args.model.lower() != 'dh':
26+
print('PyPlot only supports DH models for now')
27+
sys.exit(1)
28+
backend = rtb.backends.PyPlot()
29+
elif args.backend.lower() == 'vpython':
30+
if args.model.lower() != 'dh':
31+
print('VPython only supports DH models for now')
32+
sys.exit(1)
33+
backend = rtb.backends.VPython()
34+
elif args.backend.lower() == 'swift':
35+
if args.model.lower() != 'urdf':
36+
print('Swift only supports URDF models for now')
37+
sys.exit(1)
38+
backend = rtb.backends.Swift()
39+
else:
40+
raise ValueError('unknown backend')
41+
42+
backend.launch()
43+
backend.add(robot)
44+
for q in qt.q:
45+
robot.q = q
46+
backend.step(0.1)

‎examples/readme.py

Lines changed: 7 additions & 7 deletions
Original file line numberDiff line numberDiff line change
@@ -9,14 +9,14 @@
99
# IK
1010
from spatialmath import SE3
1111

12-
T = SE3(0.8, 0.2, 0.1) * SE3.OA([0, 1, 0], [0, 0, -1])
13-
q_pickup, *_ = robot.ikine(T) # solve IK, ignore additional outputs
14-
print(q_pickup)# display joint angles
15-
print(robot.fkine(q_pickup)) # FK shows that desired end-effector pose was achieved
12+
T = SE3(0.7, 0.2, 0.1) * SE3.OA([0, 1, 0], [0, 0, -1])
13+
sol = robot.ikine_LMS(T) # solve IK, ignore additional outputs
14+
print(sol.q) # display joint angles
15+
print(robot.fkine(sol.q)) # FK shows that desired end-effector pose was achieved
1616

1717

18-
qt = rtb.trajectory.jtraj(robot.qz, q_pickup, 50)
19-
robot.plot(qt.q, movie="panda1.gif")
18+
qtraj = rtb.jtraj(robot.qz, sol.q, 50)
19+
robot.plot(qtraj.q, movie="panda1.gif")
2020

2121
# URDF + Swift version
2222
dt = 0.050 # simulation timestep in seconds
@@ -27,7 +27,7 @@
2727
env.launch("chrome") # activate it
2828
env.add(robot) # add robot to the 3D scene
2929
env.start_recording("panda2", 1 / dt)
30-
for qk in qt.q: # for each joint configuration on trajectory
30+
for qk in qtraj.q: # for each joint configuration on trajectory
3131
robot.q = qk # update the robot state
3232
env.step() # update visualization
3333
env.stop_recording()

‎examples/rtb.py renamed to ‎examples/rtbtool

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -39,7 +39,7 @@
3939
help='input prompt')
4040
parser.add_argument('--resultprefix', '-r', default=None,
4141
help='execution result prefix, include {} for execution count number')
42-
parser.add_argument('--showassign', '-a', default=False,
42+
parser.add_argument('--showassign', '-a', default=False, action='store_true',
4343
help='display the result of assignments')
4444
args = parser.parse_args()
4545

‎examples/swift_recording.py

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -57,7 +57,7 @@
5757
while not arrived:
5858

5959
# The pose of the Panda's end-effector
60-
Te = panda.fkine()
60+
Te = panda.fkine(panda.q)
6161

6262
# Transform from the end-effector to desired pose
6363
eTep = Te.inv() * Tep

‎examples/vellipse.py

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -30,8 +30,8 @@
3030
while not arrived:
3131
start = time.time()
3232

33-
v, arrived = rp.p_servo(panda.fkine(), Tep, 0.5)
34-
panda.qd = np.linalg.pinv(panda.jacobe()) @ v
33+
v, arrived = rp.p_servo(panda.fkine(panda.q), Tep, 0.5)
34+
panda.qd = np.linalg.pinv(panda.jacobe(panda.q)) @ v
3535
env.step(50)
3636
stop = time.time()
3737

‎roboticstoolbox/backends/Swift/Swift.py

Lines changed: 4 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -373,11 +373,11 @@ def _step_shapes(self, dt):
373373
for shape in self.shapes:
374374

375375
T = shape.base
376-
t = T.t
377-
r = T.rpy('rad')
376+
t = T.t.astype('float64')
377+
r = T.rpy('rad').astype('float64')
378378

379-
t += shape.v[:3] * (dt)
380-
r += shape.v[3:] * (dt)
379+
t += shape.v[:3] * dt
380+
r += shape.v[3:] * dt
381381

382382
shape.base = sm.SE3(t) * sm.SE3.RPY(r)
383383

0 commit comments

Comments
 (0)
0