|
5 | 5 | # This is the code for the examples in the paper published at ICRA2021. |
6 | 6 | # |
7 | 7 |
|
8 | | -import swift.Swift |
| 8 | +from swift import Swift |
9 | 9 | import spatialmath.base.symbolic as sym |
10 | 10 | from roboticstoolbox import ETS as ET |
11 | 11 | from roboticstoolbox import * |
|
17 | 17 | # # III.SPATIAL MATHEMATICS |
18 | 18 |
|
19 | 19 | from spatialmath.base import * |
20 | | -T = transl(0.5, 0.0, 0.0) @ rpy2tr(0.1, 0.2, |
21 | | - 0.3, order='xyz') @ trotx(-90, 'deg') |
| 20 | + |
| 21 | +T = transl(0.5, 0.0, 0.0) @ rpy2tr(0.1, 0.2, 0.3, order="xyz") @ trotx(-90, "deg") |
22 | 22 | print(T) |
23 | 23 |
|
24 | | -T = SE3(0.5, 0.0, 0.0) * SE3.RPY([0.1, 0.2, 0.3], |
25 | | - order='xyz') * SE3.Rx(-90, unit='deg') |
| 24 | +T = SE3(0.5, 0.0, 0.0) * SE3.RPY([0.1, 0.2, 0.3], order="xyz") * SE3.Rx(-90, unit="deg") |
26 | 25 | print(T) |
27 | 26 |
|
28 | 27 | T.eul() |
29 | 28 |
|
30 | 29 | T.R |
31 | 30 |
|
32 | | -T.plot(color='red', label='2') |
| 31 | +T.plot(color="red", label="2") |
33 | 32 |
|
34 | 33 | UnitQuaternion.Rx(0.3) |
35 | 34 | UnitQuaternion.AngVec(0.3, [1, 0, 0]) |
|
47 | 46 | d4 = 0.380 |
48 | 47 | d6 = 0.065 |
49 | 48 |
|
50 | | -robot = DHRobot([ |
51 | | - RevoluteDH(d=d1, a=a1, alpha=-pi / 2), |
52 | | - RevoluteDH(a=a2), |
53 | | - RevoluteDH(alpha=pi / 2), |
54 | | -], name="my IRB140") |
| 49 | +robot = DHRobot( |
| 50 | + [ |
| 51 | + RevoluteDH(d=d1, a=a1, alpha=-pi / 2), |
| 52 | + RevoluteDH(a=a2), |
| 53 | + RevoluteDH(alpha=pi / 2), |
| 54 | + ], |
| 55 | + name="my IRB140", |
| 56 | +) |
55 | 57 |
|
56 | 58 | print(robot) |
57 | 59 |
|
|
73 | 75 | l5 = 0.0837 |
74 | 76 | l6 = 0.4318 |
75 | 77 |
|
76 | | -e = ET.tz(l1) * ET.rz() * ET.ty(l2) * ET.ry() \ |
77 | | - * ET.tz(l3) * ET.tx(l4) * ET.ty(l5) * ET.ry() \ |
78 | | - * ET.tz(l6) * ET.rz() * ET.ry() * ET.rz() |
| 78 | +e = ( |
| 79 | + ET.tz(l1) |
| 80 | + * ET.rz() |
| 81 | + * ET.ty(l2) |
| 82 | + * ET.ry() |
| 83 | + * ET.tz(l3) |
| 84 | + * ET.tx(l4) |
| 85 | + * ET.ty(l5) |
| 86 | + * ET.ry() |
| 87 | + * ET.tz(l6) |
| 88 | + * ET.rz() |
| 89 | + * ET.ry() |
| 90 | + * ET.rz() |
| 91 | +) |
79 | 92 |
|
80 | 93 | robot = ERobot(e) |
81 | 94 | print(robot) |
|
99 | 112 |
|
100 | 113 | # ## C. Symbolic manipulation |
101 | 114 |
|
102 | | -phi, theta, psi = sym.symbol('φ, ϴ, ψ') |
| 115 | +phi, theta, psi = sym.symbol("φ, ϴ, ψ") |
103 | 116 | rpy2r(phi, theta, psi) |
104 | 117 |
|
105 | 118 | q = sym.symbol("q_:6") # q = (q_1, q_2, ... q_5) |
|
145 | 158 |
|
146 | 159 |
|
147 | 160 | obstacle = Box([1, 1, 1], base=SE3(1, 0, 0)) |
148 | | -iscollision = panda.collided(obstacle) # boolean |
| 161 | +iscollision = panda.collided(panda.q, obstacle) # boolean |
149 | 162 | iscollision = panda.links[0].collided(obstacle) |
150 | 163 |
|
151 | | -d, p1, p2 = panda.closest_point(obstacle) |
| 164 | +d, p1, p2 = panda.closest_point(panda.q, obstacle) |
152 | 165 | print(d, p1, p2) |
153 | 166 | d, p1, p2 = panda.links[0].closest_point(obstacle) |
154 | 167 | print(d, p1, p2) |
|
158 | 171 | panda.plot(panda.qr, block=False) |
159 | 172 |
|
160 | 173 | backend = Swift() |
161 | | -backend.launch() # create graphical world |
| 174 | +backend.launch() # create graphical world |
162 | 175 | backend.add(panda) # add robot to the world |
163 | | -panda.q = panda.qr # update the robot |
164 | | -backend.step() # display the world |
| 176 | +panda.q = panda.qr # update the robot |
| 177 | +backend.step() # display the world |
165 | 178 |
|
166 | 179 |
|
167 | 180 | # |
0 commit comments