Skip to content

Commit 4fe4d8a

Browse files
jhavljhavl
committed
Merge branch 'master' of github.com:petercorke/robotics-toolbox-python
2 parents baa380a + dadc083 commit 4fe4d8a

File tree

12 files changed

+302
-46
lines changed

12 files changed

+302
-46
lines changed

‎.github/workflows/pythonpackage.yml

Lines changed: 1 addition & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -15,7 +15,6 @@ jobs:
1515
matrix:
1616
os: [windows-latest, ubuntu-latest, macos-latest]
1717
python-version: [3.6, 3.7, 3.8]
18-
1918
steps:
2019
- uses: actions/checkout@v2
2120
- name: Set up Python ${{ matrix.python-version }}
@@ -29,7 +28,7 @@ jobs:
2928
run: |
3029
pip install .[dev,collision,vpython]
3130
pip install pytest-timeout
32-
pytest --timeout=30 --timeout_method thread
31+
pytest --timeout=30 --timeout_method thread -s
3332
codecov:
3433
# If all tests pass:
3534
# Run coverage and upload to codecov

‎README.md

Lines changed: 10 additions & 8 deletions
Original file line numberDiff line numberDiff line change
@@ -229,6 +229,16 @@ The toolbox is incredibly useful for developing and prototyping algorithms for r
229229

230230
### Publication List
231231

232+
**NEO: A Novel Expeditious Optimisation Algorithm for Reactive Motion Control of Manipulators**, J. Haviland and P. Corke. 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.
233+
234+
[[Paper](https://arxiv.org/abs/2010.08686)] [[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)]
235+
236+
<p>
237+
<a href="https://youtu.be/jSLPJBr8QTY">
238+
<img src="https://github.com/petercorke/robotics-toolbox-python/blob/master/docs/figs/neo_youtube.png" width="560">
239+
</a>
240+
</p>
241+
232242
**A Purely-Reactive Manipulability-Maximising Motion Controller**, J. Haviland and P. Corke. In the video, the robot is controlled using the Robotics toolbox for Python.
233243

234244
[[Paper](https://arxiv.org/abs/2002.11901)] [[Project Website](https://jhavl.github.io/mmc/)] [[Video](https://youtu.be/Vu_rcPlaADI)] [[Code Example](https://github.com/petercorke/robotics-toolbox-python/blob/master/examples/mmc.py)]
@@ -241,12 +251,4 @@ The toolbox is incredibly useful for developing and prototyping algorithms for r
241251

242252
<br>
243253

244-
**NEO: A Novel Expeditious Optimisation Algorithm for Reactive Motion Control of Manipulators**, J. Haviland and P. Corke. 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.
245254

246-
[[Paper](https://arxiv.org/abs/2010.08686)] [[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)]
247-
248-
<p>
249-
<a href="https://youtu.be/jSLPJBr8QTY">
250-
<img src="https://github.com/petercorke/robotics-toolbox-python/blob/master/docs/figs/neo_youtube.png" width="560">
251-
</a>
252-
</p>

‎examples/swifty.py

Lines changed: 68 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -5,6 +5,7 @@
55

66
import roboticstoolbox as rtb
77
import spatialmath as sm
8+
import numpy as np
89

910
# Launch the simulator Swift
1011
# env = rtb.backends.Swift()
@@ -67,9 +68,70 @@
6768
# print(panda.fkine(panda.q))
6869
# print(T)
6970

70-
r = rtb.models.Puma560()
71-
# r.q = [-0.5, -0.5, 0.5, 0.5, 0.5, 0.5]
72-
env = rtb.backends.Swift()
73-
env.launch()
74-
env.add(r)
75-
env.hold()
71+
# r = rtb.models.Puma560()
72+
# # r.q = [-0.5, -0.5, 0.5, 0.5, 0.5, 0.5]
73+
# env = rtb.backends.Swift()
74+
# env.launch()
75+
# env.add(r)
76+
# env.hold()
77+
78+
r = rtb.models.ETS.Panda()
79+
r.q = r.qr
80+
81+
q1 = r.qr
82+
q2 = q1 + 0.1
83+
84+
85+
def derivative(f, a, method='central', h=0.01):
86+
'''Compute the difference formula for f'(a) with step size h.
87+
88+
Parameters
89+
----------
90+
f : function
91+
Vectorized function of one variable
92+
a : number
93+
Compute derivative at x = a
94+
method : string
95+
Difference formula: 'forward', 'backward' or 'central'
96+
h : number
97+
Step size in difference formula
98+
99+
Returns
100+
-------
101+
float
102+
Difference formula:
103+
central: f(a+h) - f(a-h))/2h
104+
forward: f(a+h) - f(a))/h
105+
backward: f(a) - f(a-h))/h
106+
'''
107+
if method == 'central':
108+
return (f(a + h) - f(a - h))/(2*h)
109+
elif method == 'forward':
110+
return (f(a + h) - f(a))/h
111+
elif method == 'backward':
112+
return (f(a) - f(a - h))/h
113+
else:
114+
raise ValueError("Method must be 'central', 'forward' or 'backward'.")
115+
116+
117+
# Numerical hessian wrt q[0]
118+
# d = derivative(lambda x: r.jacob0(np.r_[x, q1[1:]]), q1[0], h=0.1)
119+
# print(np.round(h1[:, :, 0], 3))
120+
# print(np.round(d, 3))
121+
122+
# Numerical third wrt q[0]
123+
d = derivative(lambda x: r.hessian0(np.r_[x, q1[1:]]), q1[0], h=0.01)
124+
print(np.round(d[3:, :, 0], 3))
125+
print(np.round(r.third(q1)[3:, :, 0, 0], 3))
126+
127+
l = r.deriv(q1, 3)
128+
print(np.round(l[3:, :, 0, 0], 3))
129+
130+
# def runner():
131+
# for i in range(10000):
132+
# r.jacob0(r.q)
133+
# # r.hessian0(r.q)
134+
135+
136+
# import cProfile
137+
# cProfile.run('runner()')

‎examples/vehicle1.py

Lines changed: 30 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,30 @@
1+
from roboticstoolbox import Bicycle, RandomPath, VehiclePolygon, VehicleIcon
2+
from spatialmath import * # lgtm [py/polluting-import]
3+
4+
from math import pi
5+
6+
dim = 10
7+
8+
# v = VehiclePolygon()
9+
anim = VehicleIcon('greycar', scale=2)
10+
11+
veh = Bicycle(animation=anim, control=RandomPath(dim=dim), dim=dim, verbose=False)
12+
print(veh)
13+
14+
# odo = veh.step(1, 0.3)
15+
# print(odo)
16+
17+
# print(veh.x)
18+
19+
# print(veh.f([0, 0, 0], odo))
20+
21+
# def control(v, t, x):
22+
# goal = (6,6)
23+
# goal_heading = atan2(goal[1]-x[1], goal[0]-x[0])
24+
# d_heading = base.angdiff(goal_heading, x[2])
25+
# v.stopif(base.norm(x[0:2] - goal) < 0.1)
26+
27+
# return (1, d_heading)
28+
29+
p = veh.run(1000, plot=True)
30+
print(p)

‎roboticstoolbox/data/test.mat

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1 @@
1+
hello

‎roboticstoolbox/data/test.urdf

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1 @@
1+
hello

‎roboticstoolbox/mobile/drivers.py

Lines changed: 7 additions & 12 deletions
Original file line numberDiff line numberDiff line change
@@ -172,7 +172,7 @@ def vehicle(self, v):
172172
"""
173173
self._veh = v
174174

175-
def init(self, animate=False):
175+
def init(self, ax=None):
176176
"""
177177
Initialize random path driving agent
178178
@@ -187,12 +187,10 @@ def init(self, animate=False):
187187
% See also RANDSTREAM.
188188
"""
189189
self._goal = None
190-
self._animate = animate
191190
# delete(driver.h_goal); % delete the goal
192191
# driver.h_goal = [];
193-
if self._goal_marker is not None:
194-
self
195-
self._goal_marker = None
192+
if ax is not None:
193+
self._goal_marker = plt.plot(np.nan, np.nan, **self._goal_marker_style)[0]
196194

197195
def demand(self):
198196
"""
@@ -210,7 +208,7 @@ def demand(self):
210208
# if nearly at goal point, choose the next one
211209
d = np.linalg.norm(self._veh._x[0:2] - self._goal)
212210
if d < self._dthresh:
213-
self.new_goal()
211+
self._new_goal()
214212
# elif d > 2 * self._d_prev:
215213
# self.choose_goal()
216214
# self._d_prev = d
@@ -247,12 +245,9 @@ def _new_goal(self):
247245
print(f"set goal: {self._goal}")
248246

249247
# update the goal marker
250-
if self._animate:
251-
if self._animate and self._goal_marker is None:
252-
self._goal_marker = plt.plot(self._goal[0], self._goal[1], **self._goal_marker_style)[0]
253-
else:
254-
self._goal_marker.set_xdata(self._goal[0])
255-
self._goal_marker.set_ydata(self._goal[1])
248+
if self._goal_marker is not None:
249+
self._goal_marker.set_xdata(self._goal[0])
250+
self._goal_marker.set_ydata(self._goal[1])
256251

257252
# ========================================================================= #
258253

‎roboticstoolbox/mobile/vehicle.py

Lines changed: 6 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -361,11 +361,9 @@ def init(self, x0=None, animation=None, plot=False, control=None):
361361
if control is not None:
362362
self._control = control
363363

364-
if isinstance(self._control, VehicleDriver):
365-
self._control.init()
366-
367364
self._t = 0
368365

366+
# initialize the graphics
369367
self._plot = plot
370368
if plot:
371369

@@ -384,10 +382,15 @@ def init(self, x0=None, animation=None, plot=False, control=None):
384382
plt.xlabel('x')
385383
plt.ylabel('y')
386384
self._ax.set_aspect('equal')
385+
self._ax.figure.canvas.set_window_title(f"Robotics Toolbox for Python (Figure {self._ax.figure.number})")
387386

388387
animation.add() # add vehicle animation to axis
389388
self._timer = plt.figtext(0.85, 0.95, '') # display time counter
390389

390+
# initialize the driver
391+
if isinstance(self._control, VehicleDriver):
392+
self._control.init(ax=self._ax)
393+
391394
def step(self, u1=None, u2=None):
392395
"""
393396
Step simulator by one time step (superclass method)

0 commit comments

Comments
 (0)