Skip to content

Navigation Menu

Sign in
Appearance settings

Search code, repositories, users, issues, pull requests...

Provide feedback

We read every piece of feedback, and take your input very seriously.

Saved searches

Use saved searches to filter your results more quickly
Appearance settings

Commit 901f470

Browse filesBrowse files
jhavljhavl
committed
Merge branch 'future'
2 parents 4d838d5 + f07643f commit 901f470
Copy full SHA for 901f470
Expand file treeCollapse file tree

24 files changed

+769
-46
lines changed

‎README.md

Copy file name to clipboardExpand all lines: README.md
+35-26Lines changed: 35 additions & 26 deletions
Original file line numberDiff line numberDiff line change
@@ -27,39 +27,38 @@ A Python implementation of the <a href="https://github.com/petercorke/robotics-t
2727
</tr>
2828
</table>
2929

30-
3130
## Synopsis
3231

3332
This toolbox brings robotics-specific functionality to Python, and leverages
3433
Python's advantages of portability, ubiquity and support, and the capability of
35-
the open-source ecosystem for linear algebra (numpy, scipy), graphics
34+
the open-source ecosystem for linear algebra (numpy, scipy), graphics
3635
(matplotlib, three.js, WebGL), interactive development (jupyter, jupyterlab,
3736
mybinder.org), and documentation (sphinx).
3837

3938
The Toolbox provides tools for representing the kinematics and dynamics of
40-
serial-link manipulators - you can easily create your own in Denavit-Hartenberg
39+
serial-link manipulators - you can easily create your own in Denavit-Hartenberg
4140
form, import a URDF file, or use over 30 supplied models for well-known
4241
contemporary robots from Franka-Emika, Kinova, Universal Robotics, Rethink as
4342
well as classical robots such as the Puma 560 and the Stanford arm.
4443

4544
The toolbox will also support mobile robots with functions for robot motion models
46-
(unicycle, bicycle), path planning algorithms (bug, distance transform, D*,
45+
(unicycle, bicycle), path planning algorithms (bug, distance transform, D\*,
4746
PRM), kinodynamic planning (lattice, RRT), localization (EKF, particle filter),
4847
map building (EKF) and simultaneous localization and mapping (EKF).
4948

5049
The Toolbox provides:
5150

52-
* code that is mature and provides a point of comparison for other
53-
implementations of the same algorithms;
54-
* routines which are generally written in a straightforward manner which
55-
allows for easy understanding, perhaps at the expense of computational
56-
efficiency;
57-
* source code which can be read for learning and teaching;
58-
* backward compatability with the Robotics Toolbox for MATLAB
51+
- code that is mature and provides a point of comparison for other
52+
implementations of the same algorithms;
53+
- routines which are generally written in a straightforward manner which
54+
allows for easy understanding, perhaps at the expense of computational
55+
efficiency;
56+
- source code which can be read for learning and teaching;
57+
- backward compatability with the Robotics Toolbox for MATLAB
5958

6059
The Toolbox leverages the [Spatial Maths Toolbox for Python](https://github.com/petercorke/spatialmath-python) to
6160
provide support for data types such as SO(n) and SE(n) matrices, quaternions, twists and spatial vectors.
62-
61+
6362
## Code Example
6463

6564
We will load a model of the Franka-Emika Panda robot defined classically using
@@ -97,14 +96,15 @@ print(robot)
9796
T = robot.fkine(robot.qz) # forward kinematics
9897
print(T)
9998

100-
0.707107 0.707107 0 0.088
101-
0.707107 -0.707107 0 0
102-
0 0 -1 0.823
103-
0 0 0 1
99+
0.707107 0.707107 0 0.088
100+
0.707107 -0.707107 0 0
101+
0 0 -1 0.823
102+
0 0 0 1
104103
```
104+
105105
(Python prompts are not shown to make it easy to copy+paste the code, console output is indented)
106106

107-
We can solve inverse kinematics very easily. We first choose an SE(3) pose
107+
We can solve inverse kinematics very easily. We first choose an SE(3) pose
108108
defined in terms of position and orientation (end-effector z-axis down (A=-Z) and finger
109109
orientation parallel to y-axis (O=+Y)).
110110

@@ -119,11 +119,11 @@ print(sol)
119119
q_pickup = sol.q
120120
print(robot.fkine(q_pickup)) # FK shows that desired end-effector pose was achieved
121121

122-
Out[35]:
123-
-1 9.43001e-14 2.43909e-12 0.7
124-
9.43759e-14 1 7.2574e-13 0.2
125-
-2.43913e-12 7.2575e-13 -1 0.1
126-
0 0 0 1
122+
Out[35]:
123+
-1 9.43001e-14 2.43909e-12 0.7
124+
9.43759e-14 1 7.2574e-13 0.2
125+
-2.43913e-12 7.2575e-13 -1 0.1
126+
0 0 0 1
127127
```
128128

129129
Note that because this robot is redundant we don't have any control over the arm configuration apart from end-effector pose, ie. we can't control the elbow height.
@@ -137,9 +137,9 @@ robot.plot(qt.q, movie='panda1.gif')
137137

138138
![Panda trajectory animation](./docs/figs/panda1.gif)
139139

140-
which uses the default matplotlib backend. Grey arrows show the joint axes and the colored frame shows the end-effector pose.
140+
which uses the default matplotlib backend. Grey arrows show the joint axes and the colored frame shows the end-effector pose.
141141

142-
Let's now load a URDF model of the same robot. The kinematic representation is no longer
142+
Let's now load a URDF model of the same robot. The kinematic representation is no longer
143143
based on Denavit-Hartenberg parameters, it is now a rigid-body tree.
144144

145145
```python
@@ -172,7 +172,7 @@ print(robot) # display the model
172172
The symbol `@` indicates the link as an end-effector, a leaf node in the rigid-body
173173
tree.
174174

175-
We can instantiate our robot inside a browser-based 3d-simulation environment.
175+
We can instantiate our robot inside a browser-based 3d-simulation environment.
176176

177177
```python
178178
from roboticstoolbox.backends.Swift import Swift # instantiate 3D browser-based visualizer
@@ -238,6 +238,16 @@ The toolbox is incredibly useful for developing and prototyping algorithms for r
238238

239239
### Publication List
240240

241+
J. Haviland, N. Sünderhauf and P. Corke, "**A Holistic Approach to Reactive Mobile Manipulation**,". In the video, the robot is controlled using the Robotics toolbox for Python and features a recording from the [Swift](https://github.com/jhavl/swift) Simulator.
242+
243+
[[Arxiv Paper](https://arxiv.org/abs/2109.04749)] [[Project Website](https://jhavl.github.io/holistic/)] [[Video](https://youtu.be/-DXBQPeLIV4)] [[Code Example](https://github.com/petercorke/robotics-toolbox-python/blob/master/examples/holistic_mm_non_holonomic.py)]
244+
245+
<p>
246+
<a href="https://youtu.be/-DXBQPeLIV4">
247+
<img src="https://github.com/petercorke/robotics-toolbox-python/raw/master/docs/figs/holistic_youtube.png" width="560">
248+
</a>
249+
</p>
250+
241251
J. Haviland and P. Corke, "**NEO: A Novel Expeditious Optimisation Algorithm for Reactive Motion Control of Manipulators**," in _IEEE Robotics and Automation Letters_, doi: 10.1109/LRA.2021.3056060. In the video, the robot is controlled using the Robotics toolbox for Python and features a recording from the [Swift](https://github.com/jhavl/swift) Simulator.
242252

243253
[[Arxiv Paper](https://arxiv.org/abs/2010.08686)] [[IEEE Xplore](https://ieeexplore.ieee.org/document/9343718)] [[Project Website](https://jhavl.github.io/neo/)] [[Video](https://youtu.be/jSLPJBr8QTY)] [[Code Example](https://github.com/petercorke/robotics-toolbox-python/blob/master/examples/neo.py)]
@@ -263,4 +273,3 @@ J. Haviland and P. Corke, "**NEO: A Novel Expeditious Optimisation Algorithm for
263273
## Common Issues
264274

265275
See the common issues with fixes [here](https://github.com/petercorke/robotics-toolbox-python/wiki/Common-Issues).
266-

‎docs/figs/holistic_youtube.png

Copy file name to clipboard
1.24 MB
Loading

‎roboticstoolbox/blocks/Icons/250x250/armplot.png

Copy file name to clipboard
5.78 KB
Loading

‎roboticstoolbox/blocks/Icons/250x250/bicycle.png

Copy file name to clipboard
8.78 KB
Loading

‎roboticstoolbox/blocks/Icons/250x250/camera.png

Copy file name to clipboard
1.6 KB
Loading

‎roboticstoolbox/blocks/Icons/250x250/fkine.png

Copy file name to clipboard
-128 Bytes
Loading

‎roboticstoolbox/blocks/Icons/250x250/ikine.png

Copy file name to clipboard
128 Bytes
Loading

‎roboticstoolbox/blocks/Icons/250x250/multirotor.png

Copy file name to clipboard
16.6 KB
Loading

‎roboticstoolbox/blocks/Icons/250x250/multirotormixer.png

Copy file name to clipboard
2.49 KB
Loading

‎roboticstoolbox/blocks/Icons/250x250/unicycle.png

Copy file name to clipboard
4.3 KB
Loading

‎roboticstoolbox/blocks/Icons/250x250/vehicleplot.png

Copy file name to clipboard
3.64 KB
Loading

‎roboticstoolbox/blocks/Icons/armplot.png

Copy file name to clipboard
333 Bytes
Loading

‎roboticstoolbox/blocks/Icons/fkine.png

Copy file name to clipboard
-25 Bytes
Loading

‎roboticstoolbox/blocks/Icons/ikine.png

Copy file name to clipboard
25 Bytes
Loading

‎roboticstoolbox/blocks/Icons/multirotor.png

Copy file name to clipboard
515 Bytes
Loading

‎roboticstoolbox/blocks/Icons/multirotormixer.png

Copy file name to clipboard
671 Bytes
Loading

‎roboticstoolbox/blocks/Icons/vehicleplot.png

Copy file name to clipboard
829 Bytes
Loading

‎roboticstoolbox/examples/holistic_mm_non_holonomic.py

Copy file name to clipboard
+124Lines changed: 124 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,124 @@
1+
#!/usr/bin/env python
2+
"""
3+
@author Jesse Haviland
4+
"""
5+
6+
import swift
7+
import roboticstoolbox as rtb
8+
import spatialgeometry as sg
9+
import spatialmath as sm
10+
import qpsolvers as qp
11+
import numpy as np
12+
import math
13+
14+
15+
def step_robot(r, Tep):
16+
17+
wTe = r.fkine(r.q, fast=True)
18+
19+
eTep = np.linalg.inv(wTe) @ Tep
20+
21+
# Spatial error
22+
et = np.sum(np.abs(eTep[:3, -1]))
23+
24+
# Gain term (lambda) for control minimisation
25+
Y = 0.01
26+
27+
# Quadratic component of objective function
28+
Q = np.eye(r.n + 6)
29+
30+
# Joint velocity component of Q
31+
Q[: r.n, : r.n] *= Y
32+
Q[:2, :2] *= 1.0 / et
33+
34+
# Slack component of Q
35+
Q[r.n :, r.n :] = (1.0 / et) * np.eye(6)
36+
37+
v, _ = rtb.p_servo(wTe, Tep, 1.5)
38+
39+
v[3:] *= 1.3
40+
41+
# The equality contraints
42+
Aeq = np.c_[r.jacobe(r.q, fast=True), np.eye(6)]
43+
beq = v.reshape((6,))
44+
45+
# The inequality constraints for joint limit avoidance
46+
Ain = np.zeros((r.n + 6, r.n + 6))
47+
bin = np.zeros(r.n + 6)
48+
49+
# The minimum angle (in radians) in which the joint is allowed to approach
50+
# to its limit
51+
ps = 0.1
52+
53+
# The influence angle (in radians) in which the velocity damper
54+
# becomes active
55+
pi = 0.9
56+
57+
# Form the joint limit velocity damper
58+
Ain[: r.n, : r.n], bin[: r.n] = r.joint_velocity_damper(ps, pi, r.n)
59+
60+
# Linear component of objective function: the manipulability Jacobian
61+
c = np.concatenate(
62+
(np.zeros(2), -r.jacobm(start=r.links[4]).reshape((r.n - 2,)), np.zeros(6))
63+
)
64+
65+
# Get base to face end-effector
66+
kε = 0.5
67+
bTe = r.fkine(r.q, include_base=False, fast=True)
68+
θε = math.atan2(bTe[1, -1], bTe[0, -1])
69+
ε = kε * θε
70+
c[0] = -ε
71+
72+
# The lower and upper bounds on the joint velocity and slack variable
73+
lb = -np.r_[r.qdlim[: r.n], 10 * np.ones(6)]
74+
ub = np.r_[r.qdlim[: r.n], 10 * np.ones(6)]
75+
76+
# Solve for the joint velocities dq
77+
qd = qp.solve_qp(Q, c, Ain, bin, Aeq, beq, lb=lb, ub=ub)
78+
qd = qd[: r.n]
79+
80+
if et > 0.5:
81+
qd *= 0.7 / et
82+
else:
83+
qd *= 1.4
84+
85+
if et < 0.02:
86+
return True, qd
87+
else:
88+
return False, qd
89+
90+
91+
env = swift.Swift()
92+
env.launch(realtime=True)
93+
94+
ax_goal = sg.Axes(0.1)
95+
env.add(ax_goal)
96+
97+
frankie = rtb.models.Frankie()
98+
frankie.q = frankie.qr
99+
env.add(frankie)
100+
101+
arrived = False
102+
dt = 0.025
103+
104+
# Behind
105+
env.set_camera_pose([-2, 3, 0.7], [-2, 0.0, 0.5])
106+
wTep = frankie.fkine(frankie.q) * sm.SE3.Rz(np.pi)
107+
wTep.A[:3, :3] = np.diag([-1, 1, -1])
108+
wTep.A[0, -1] -= 4.0
109+
wTep.A[2, -1] -= 0.25
110+
ax_goal.base = wTep
111+
env.step()
112+
113+
114+
while not arrived:
115+
116+
arrived, frankie.qd = step_robot(frankie, wTep.A)
117+
env.step(dt)
118+
119+
# Reset bases
120+
base_new = frankie.fkine(frankie._q, end=frankie.links[2], fast=True)
121+
frankie._base.A[:] = base_new
122+
frankie.q[:2] = 0
123+
124+
env.hold()

‎roboticstoolbox/examples/holistic_mm_omni.py

Copy file name to clipboard
+124Lines changed: 124 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,124 @@
1+
#!/usr/bin/env python
2+
"""
3+
@author Jesse Haviland
4+
"""
5+
6+
import swift
7+
import roboticstoolbox as rtb
8+
import spatialgeometry as sg
9+
import spatialmath as sm
10+
import qpsolvers as qp
11+
import numpy as np
12+
import math
13+
14+
15+
def step_robot(r, Tep):
16+
17+
wTe = r.fkine(r.q, fast=True)
18+
19+
eTep = np.linalg.inv(wTe) @ Tep
20+
21+
# Spatial error
22+
et = np.sum(np.abs(eTep[:3, -1]))
23+
24+
# Gain term (lambda) for control minimisation
25+
Y = 0.01
26+
27+
# Quadratic component of objective function
28+
Q = np.eye(r.n + 6)
29+
30+
# Joint velocity component of Q
31+
Q[: r.n, : r.n] *= Y
32+
Q[:3, :3] *= 1.0 / et
33+
34+
# Slack component of Q
35+
Q[r.n :, r.n :] = (1.0 / et) * np.eye(6)
36+
37+
v, _ = rtb.p_servo(wTe, Tep, 1.5)
38+
39+
v[3:] *= 1.3
40+
41+
# The equality contraints
42+
Aeq = np.c_[r.jacobe(r.q, fast=True), np.eye(6)]
43+
beq = v.reshape((6,))
44+
45+
# The inequality constraints for joint limit avoidance
46+
Ain = np.zeros((r.n + 6, r.n + 6))
47+
bin = np.zeros(r.n + 6)
48+
49+
# The minimum angle (in radians) in which the joint is allowed to approach
50+
# to its limit
51+
ps = 0.1
52+
53+
# The influence angle (in radians) in which the velocity damper
54+
# becomes active
55+
pi = 0.9
56+
57+
# Form the joint limit velocity damper
58+
Ain[: r.n, : r.n], bin[: r.n] = r.joint_velocity_damper(ps, pi, r.n)
59+
60+
# Linear component of objective function: the manipulability Jacobian
61+
c = np.concatenate(
62+
(np.zeros(3), -r.jacobm(start=r.links[5]).reshape((r.n - 3,)), np.zeros(6))
63+
)
64+
65+
# Get base to face end-effector
66+
kε = 0.5
67+
bTe = r.fkine(r.q, include_base=False, fast=True)
68+
θε = math.atan2(bTe[1, -1], bTe[0, -1])
69+
ε = kε * θε
70+
c[0] = -ε
71+
72+
# The lower and upper bounds on the joint velocity and slack variable
73+
lb = -np.r_[r.qdlim[: r.n], 10 * np.ones(6)]
74+
ub = np.r_[r.qdlim[: r.n], 10 * np.ones(6)]
75+
76+
# Solve for the joint velocities dq
77+
qd = qp.solve_qp(Q, c, Ain, bin, Aeq, beq, lb=lb, ub=ub)
78+
qd = qd[: r.n]
79+
80+
if et > 0.5:
81+
qd *= 0.7 / et
82+
else:
83+
qd *= 1.4
84+
85+
if et < 0.02:
86+
return True, qd
87+
else:
88+
return False, qd
89+
90+
91+
env = swift.Swift()
92+
env.launch(realtime=True)
93+
94+
ax_goal = sg.Axes(0.1)
95+
env.add(ax_goal)
96+
97+
frankie = rtb.models.FrankieOmni()
98+
frankie.q = frankie.qr
99+
env.add(frankie)
100+
101+
arrived = False
102+
dt = 0.025
103+
104+
# Behind
105+
env.set_camera_pose([-2, 3, 0.7], [-2, 0.0, 0.5])
106+
wTep = frankie.fkine(frankie.q) * sm.SE3.Rz(np.pi)
107+
wTep.A[:3, :3] = np.diag([-1, 1, -1])
108+
wTep.A[0, -1] -= 4.0
109+
wTep.A[2, -1] -= 0.25
110+
ax_goal.base = wTep
111+
env.step()
112+
113+
114+
while not arrived:
115+
116+
arrived, frankie.qd = step_robot(frankie, wTep.A)
117+
env.step(dt)
118+
119+
# Reset bases
120+
base_new = frankie.fkine(frankie._q, end=frankie.links[3], fast=True)
121+
frankie._base.A[:] = base_new
122+
frankie.q[:3] = 0
123+
124+
env.hold()

0 commit comments

Comments
0 (0)
Morty Proxy This is a proxified and sanitized view of the page, visit original site.