10000 Merge branch · hanm2019/robotics-toolbox-python@f5b503c · GitHub
[go: up one dir, main page]
Skip to content

Commit f5b503c

Browse files
jhavljhavl
committed
Merge branch
2 parents 9bd463c + c244cb5 commit f5b503c

29 files changed

+1249
-835
lines changed

‎roboticstoolbox/blocks/uav.py

Lines changed: 119 additions & 24 deletions
Original file line numberDiff line numberDiff line change
@@ -4,7 +4,7 @@
44
import matplotlib.pyplot as plt
55
import time
66

7-
from bdsim.components import TransferBlock
7+
from bdsim.components import TransferBlock, FunctionBlock
88
from bdsim.graphics import GraphicsBlock
99
# ------------------------------------------------------------------------ #
1010

@@ -135,11 +135,13 @@ def __init__(self, model, *inputs, groundcheck=True, speedcheck=True, x0=None, *
135135
self.speedcheck = speedcheck
136136

137137
self.D = np.zeros((3,self.nrotors))
138+
self.theta = np.zeros((self.nrotors,))
138139
for i in range(0, self.nrotors):
139140
theta = i / self.nrotors * 2 * pi
140141
# Di Rotor hub displacements (1x3)
141142
# first rotor is on the x-axis, clockwise order looking down from above
142143
self.D[:,i] = np.r_[ model['d'] * cos(theta), model['d'] * sin(theta), model['h']]
144+
self.theta[i] = theta
143145

144146
self.a1s = np.zeros((self.nrotors,))
145147
self.b1s = np.zeros((self.nrotors,))
@@ -181,6 +183,7 @@ def output(self, t=None):
181183
out['w'] = iW @ self._x[9:12] # RPY rates mapped to body frame
182184
out['a1s'] = self.a1s
183185
out['b1s'] = self.b1s
186+
out['X'] = self._x
184187

185188
return [out]
186189

@@ -295,12 +298,91 @@ def deriv(self):
295298

296299
do = np.linalg.inv(model['J']) @ (np.cross(-o, model['J'] @ o) + np.sum(tau, axis=1) + np.sum(Q, axis=1)) # row sum of torques
297300

298-
# stash the flapping information for plotting
299-
self.a1s = a1s
300-
self.b1s = b1s
301+
# # stash the flapping information for plotting
302+
# self.a1s = a1s
303+
# self.b1s = b1s
301304

302305
return np.r_[dz, dn, dv, do] # This is the state derivative vector
303306

307+
# ------------------------------------------------------------------------ #
308+
309+
class MultiRotorMixer(FunctionBlock):
310+
"""
311+
:blockname:`MULTIROTORMIXER`
312+
313+
.. table::
314+
:align: left
315+
316+
+--------+---------+---------+
317+
| inputs | outputs | states |
318+
+--------+---------+---------+
319+
| 4 | 1 | 0 |
320+
+--------+---------+---------+
321+
| float | | |
322+
+--------+---------+---------+
323+
"""
324+
325+
nin = 4
326+
nout = 1
327+
inlabels = ('𝛕r', '𝛕p', '𝛕y', 'T')
328+
outlabels = ('ω',)
329+
330+
def __init__(self, maxw=1000, minw=5, **kwargs):
331+
"""
332+
Create a block that displays/animates a multi-rotor flying vehicle.
333+
334+
:param maxw: maximum rotor speed in rad/s, defaults to 1000
335+
:type maxw: float
336+
:param minw: minimum rotor speed in rad/s, defaults to 5
337+
:type minw: float
338+
:param ``**kwargs``: common Block options
339+
:return: a MULTIROTORMIXER block
340+
:rtype: MultiRotorMixer instance
341+
342+
343+
**Block ports**
344+
345+
:input 𝛕r: roll torque
346+
:input 𝛕p: pitch torque
347+
:input 𝛕y: yaw torque
348+
:input T: total thrust
349+
350+
:output ω: 1D array of rotor speeds
351+
352+
Derived from Simulink model by Pauline Pounds 2004
353+
"""
354+
super().__init__(inputs=inputs, **kwargs)
355+
self.type = 'multirotormixer'
356+
self.minw = minw
357+
self.maxw = maxw
358+
359+
def output(self, t):
360+
w = np.zeros((self.nrotors,))
361+
tau = self.inputs
362+
for i in self.nrotors:
363+
# roll and pitch coupling
364+
w[i] += -tau[ 57AE 0] * sin(self.theta[i]) + tau[1] * cos(self.theta[i])
365+
366+
# yaw coupling
367+
sign = 1 if i % 1 == 0 else -1
368+
w[i] += sign * tau[2]
369+
370+
# overall thrust
371+
w[i] += tau[3] / self.nrotors
372+
373+
# clip the rotor speeds to the range [minw, maxw]
374+
w = np.clip(w, self.minw, self.maxw)
375+
376+
# convert to thrust
377+
w = np.sqrt(w) / self.model['b']
378+
379+
# negate alterate rotors to indicate counter-rotation
380+
for i in self.nrotors:
381+
if i % 1 == 0:
382+
w[i] = -w[i]
383+
384+
return [w]
385+
304386

305387
# ------------------------------------------------------------------------ #
306388

@@ -360,15 +442,18 @@ def __init__(self, model, *inputs, scale=[-2, 2, -2, 2, 10], flapscale=1, projec
360442
:param projection: 3D projection, one of: 'ortho' [default], 'perspective'
361443
:type projection: str
362444
:param ``**kwargs``: common Block options
363-
:return: a MULTIROTOPLOT block
364-
:rtype: MultiRobotPlot instance
445+
:return: a MULTIROTORPLOT block
446+
:rtype: MultiRotorPlot instance
365447
366448
367449
**Block ports**
368450
369-
:input ω: a dictionary signal that includes the item:
451+
:input y: a dictionary signal that includes the item:
370452
371453
- ``x`` pose in the world frame as :math:`[x, y, z, \theta_Y, \theta_P, \theta_R]`
454+
- ``X`` pose in the world frame as :math:`[x, y, z, \theta_Y, \theta_P, \theta_R]`
455+
- ``a1s``
456+
- ``b1s``
372457
373458
.. figure:: ../../figs/multirotorplot.png
374459
:width: 500px
@@ -386,7 +471,7 @@ def __init__(self, model, *inputs, scale=[-2, 2, -2, 2, 10], flapscale=1, projec
386471
self.projection = projection
387472
self.flapscale = flapscale
388473

389-
def start(self):
474+
def start(self, state):
390475
quad = self.model
391476

392477
# vehicle dimensons
@@ -411,15 +496,20 @@ def start(self):
411496
self.ax.set_xlabel('X')
412497
self.ax.set_ylabel('Y')
413498
self.ax.set_zlabel('-Z (height above ground)')
499+
500+
self.panel = self.ax.text2D(0.05, 0.95, '', transform=self.ax.transAxes,
501+
fontsize=10, family='monospace', verticalalignment='top',
502+
bbox=dict(boxstyle='round', facecolor='white', edgecolor='black'))
503+
414504

415505
# TODO allow user to set maximum height of plot volume
416506
self.ax.set_xlim(self.scale[0], self.scale[1])
417507
self.ax.set_ylim(self.scale[2], self.scale[3])
418508
self.ax.set_zlim(0, self.scale[4])
419509

420510
# plot the ground boundaries and the big cross
421-
self.ax.plot([self.scale[0], self.scale[2]], [self.scale[1], self.scale[3]], [0, 0], 'b-')
422-
self.ax.plot([self.scale[0], self.scale[3]], [self.scale[1], self.scale[2]], [0, 0], 'b-')
511+
self.ax.plot([self.scale[0], self.scale[1]], [self.scale[2], self.scale[3]], [0, 0], 'b-')
512+
self.ax.plot([self.scale[0], self.scale[1]], [self.scale[3], self.scale[2]], [0, 0], 'b-')
423513
self.ax.grid(True)
424514

425515
self.shadow, = self.ax.plot([0, 0], [0, 0], 'k--')
@@ -440,32 +530,32 @@ def start(self):
440530
self.a1s = np.zeros((self.nrotors,))
441531
self.b1s = np.zeros((self.nrotors,))
442532

443-
def step(self):
533+
534+
def step(self, state):
444535

445536
def plot3(h, x, y, z):
446-
h.set_data(x, y)
447-
h.set_3d_properties(z)
537+
h.set_data_3d(x, y, z)
538+
# h.set_data(x, y)
539+
# h.set_3d_properties(np.r_[z])
448540

449541
# READ STATE
450-
z = self.inputs[0][0:3]
451-
n = self.inputs[0][3:6]
542+
z = self.inputs[0]['x'][0:3]
543+
n = self.inputs[0]['x'][3:6]
452544

453545
# TODO, check input dimensions, 12 or 12+2N, deal with flapping
454546

455-
a1s = self.a1s
456-
b1s = self.b1s
547+
a1s = self.inputs[0]['a1s']
548+
b1s = self.inputs[0]['b1s']
457549

458550
quad = self.model
459551

460552
# vehicle dimensons
461-
d = quad['d']; # Hub displacement from COG
462-
r = quad['r']; # Rotor radius
553+
d = quad['d'] # Hub displacement from COG
554+
r = quad['r'] # Rotor radius
463555

464556
# PREPROCESS ROTATION MATRIX
465-
phi = n[0]; # Euler angles
466-
the = n[1];
467-
psi = n[2];
468-
557+
phi, the, psi = n # Euler angles
558+
469559
# BBF > Inertial rotation matrix
470560
R = np.array([
471561
[cos(the) * cos(phi), sin(psi) * sin(the) * cos(phi) - cos(psi) * sin(phi), cos(psi) * sin(the) * cos(phi) + sin(psi) * sin(phi)],
@@ -512,7 +602,7 @@ def plot3(h, x, y, z):
512602
hub0 = F @ z # centre of vehicle
513603
for i in range(0, self.nrotors):
514604
# line from hub to centre plot3([hub(1,N) hub(1,S)],[hub(2,N) hub(2,S)],[hub(3,N) hub(3,S)],'-b')
515-
plot3(self.arm[i], [hub[0,i], hub0[0]],[hub[1,i], hub0[1]],[hub[2,i], hub0[2]])
605+
plot3(self.arm[i], [hub[0,i], hub0[0]], [hub[1,i], hub0[1]], [hub[2,i], hub0[2]])
516606

517607
# plot a circle at the hub itself
518608
#plot3([hub(1,i)],[hub(2,i)],[hub(3,i)],'o')
@@ -521,4 +611,9 @@ def plot3(h, x, y, z):
521611
plot3(self.shadow, [z[0], 0], [-z[1], 0], [0, 0])
522612
plot3(self.groundmark, z[0], -z[1], 0)
523613

614+
textstr = f"t={state.t: .2f}\nh={z[2]: .2f}\nγ={n[0]: .2f}"
615+
self.panel.set_text(textstr)
616+
617+
super().step(state=state)
618+
524619

‎roboticstoolbox/examples/swift_recording.py

Lines changed: 5 additions & 8 deletions
Original file line numberDiff line numberDiff line change
@@ -19,7 +19,7 @@
1919
env = swift.Swift()
2020

2121
# Launch the sim in chrome as only chrome supports webm videos
22-
env.launch(browser="google-chrome", realtime=True)
22+
env.launch('google-chrome')
2323

2424
# Create a Panda robot object
2525
panda = rtb.models.Panda()
@@ -36,10 +36,10 @@
3636
# Start recording with a framerate of 1/dt and
3737
# call the video panda_swift_recording
3838
# Export video as a webm file (this only works in Chrome)
39-
env.start_recording("panda_swift_recording", 1 / dt)
39+
env.start_recording('panda_swift_recording', 1 / dt)
4040

4141
# To export as a gif replace the above line with
42-
# env.start_recording("panda_swift_recording", 1 / dt, format="gif")
42+
# env.start_recording('panda_swift_recording', 1 / dt, format='gif')
4343

4444
# To export as a tar folder of jpg captures replace with
4545
# env.start_recording('panda_swift_recording', 1 / dt, format='jpg')
@@ -68,7 +68,7 @@
6868

6969
# Calulate the required end-effector spatial velocity for the robot
7070
# to approach the goal. Gain is set to 1.0
71-
v, arrived = rtb.p_servo(Te, Tep, np.ones(6))
71+
v, arrived = rtb.p_servo(Te, Tep, 1.0)
7272

7373
# Gain term (lambda) for control minimisation
7474
Y = 0.01
@@ -83,7 +83,7 @@
8383
Q[n:, n:] = (1 / e) * np.eye(6)
8484

8585
# The equality contraints
86-
Aeq = np.c_[panda.jacob0(panda.q), np.eye(6)]
86+
Aeq = np.c_[panda.jacobe(panda.q), np.eye(6)]
8787
beq = v.reshape((6,))
8888

8989
# The inequality constraints for joint limit avoidance
@@ -119,6 +119,3 @@
119119

120120
# Stop recording and save the video (to the downloads folder)
121121
env.stop_recording()
122-
123-
# Hold is required for gifs which take extra time to process
124-
# env.hold()

0 commit comments

Comments
 (0)
0