def test_import1(self):
# create a subsystem
ss = bdsim.BlockDiagram(name='subsystem1')
f = ss.FUNCTION(lambda x: x)
inp = ss.INPORT(1)
outp = ss.OUTPORT(1)
ss.connect(inp, f)
ss.connect(f, outp)
# create main system
bd = bdsim.BlockDiagram()
const = bd.CONSTANT(1)
scope = bd.SCOPE()
f = bd.SUBSYSTEM(ss, name='subsys')
bd.connect(const, f)
bd.connect(f, scope)
bd.compile()
self.assertEqual(len(bd.blocklist), 3)
self.assertEqual(len(bd.wirelist), 2)
def test_import2(self):
# create a subsystem
ss = bdsim.BlockDiagram(name='subsystem1')
f = ss.FUNCTION(lambda x: x)
inp = ss.INPORT(1)
outp = ss.OUTPORT(1)
ss.connect(inp, f)
ss.connect(f, outp)
# create main system
bd = bdsim.BlockDiagram()
const = bd.CONSTANT(1)
scope1 = bd.SCOPE()
scope2 = bd.SCOPE()
f1 = bd.SUBSYSTEM(ss, name='subsys1')
f2 = bd.SUBSYSTEM(ss, name='subsys2')
bd.connect(const, f1, f2)
bd.connect(f1, scope1)
bd.connect(f2, scope2)
bd.compile()
def test_connect_1(self):
bd = bdsim.BlockDiagram()
src = bd.CONSTANT(2)
dst = bd.OUTPORT(1) # 1 port
bd.connect(src, dst)
bd.compile()
bd.evaluate(x=[], t=0)
self.assertEqual(dst.inputs[0], 2)
def test_multiply2(self):
bd = bdsim.BlockDiagram()
dst = bd.OUTPORT(1) # 1 ports
dst[0] = bd.CONSTANT(2) * bd.GAIN(3) * bd.GAIN(4)
bd.compile()
bd.evaluate(x=[], t=0)
self.assertEqual(dst.inputs, [24])
def test_slice1(self):
bd = bdsim.BlockDiagram()
src = bd.CONSTANT(2)
dst = bd.OUTPORT(2) # 1 port
bd.connect(src, dst[0])
bd.connect(src, dst[1])
bd.compile()
bd.evaluate(x=[], t=0)
self.assertEqual(dst.inputs, [2, 2])
def test_inline2(self):
bd = bdsim.BlockDiagram()
dst = bd.OUTPORT(1) # 1 ports
const1 = bd.CONSTANT(2)
const2 = bd.CONSTANT(3)
dst[0] = bd.SUM('++', const1, const2) * bd.GAIN(2)
bd.compile()
bd.evaluate(x=[], t=0)
self.assertEqual(dst.inputs, [10])
def test_assignment11(self):
bd = bdsim.BlockDiagram()
src = bd.CONSTANT(2)
dst = bd.OUTPORT(1) # 1 port
dst[0] = src
bd.compile()
bd.evaluate(x=[], t=0)
self.assertEqual(dst.inputs[0], 2)
def test_ports1(self):
bd = bdsim.BlockDiagram()
const1 = bd.CONSTANT(2)
const2 = bd.CONSTANT(3)
dst = bd.OUTPORT(2) # 2 ports
bd.connect(const1, dst[0])
bd.connect(const2, dst[1])
bd.compile()
bd.evaluate(x=[], t=0)
self.assertEqual(dst.inputs, [2, 3])
def test_slice6(self):
bd = bdsim.BlockDiagram()
const = bd.CONSTANT([2, 3, 4, 5])
src = bd.DEMUX(4)
bd.connect(const, src)
dst = bd.OUTPORT(4) # 4 ports
bd.connect(src[3:-1:-1], dst[3:-1:-1])
bd.compile()
bd.evaluate(x=[], t=0)
self.assertEqual(dst.inputs, [2, 3, 4, 5])
def test_assignment3(self):
bd = bdsim.BlockDiagram()
const = bd.CONSTANT([2, 3, 4, 5])
src = bd.DEMUX(4)
bd.connect(const, src)
dst = bd.OUTPORT(4) # 4 ports
dst[3:-1:-1] = src[0:4]
bd.compile()
bd.evaluate(x=[], t=0)
self.assertEqual(dst.inputs, [5, 4, 3, 2])
def test_multiply3(self):
bd = bdsim.BlockDiagram()
const = bd.CONSTANT([2, 3])
src = bd.DEMUX(2)
bd.connect(const, src)
dst = bd.OUTPORT(2) # 2 ports
dst[0] = src[0] * bd.GAIN(2)
dst[1] = src[1] * bd.GAIN(3)
bd.compile()
bd.evaluate(x=[], t=0)
self.assertEqual(dst.inputs, [4, 9])
def test_assignment2(self):
bd = bdsim.BlockDiagram()
const1 = bd.CONSTANT(2)
const2 = bd.CONSTANT(3)
dst = bd.OUTPORT(2) # 2 ports
dst[0] = const1
dst[1] = const2
bd.compile()
bd.evaluate(x=[], t=0)
self.assertEqual(dst.inputs, [2, 3])
def test_ports2(self):
bd = bdsim.BlockDiagram()
const = bd.CONSTANT([2, 3])
src = bd.DEMUX(2)
bd.connect(const, src)
dst1 = bd.OUTPORT(1) # 1 port
dst2 = bd.OUTPORT(1) # 1 port
bd.connect(src[0], dst1)
bd.connect(src[1], dst2)
bd.compile()
bd.evaluate(x=[], t=0)
self.assertEqual(dst.inputs, [2, 3, 4, 5])
#!/usr/bin/env python3
# run with command line -a switch to show animation
import bdsim
import math
bd = bdsim.BlockDiagram()
def background_graphics(ax):
ax.plot(5, 5, '*')
ax.plot(5, 2, 'o')
goal = bd.CONSTANT([5, 5])
error = bd.SUM('+-')
d2goal = bd.FUNCTION(lambda d: math.sqrt(d[0]**2 + d[1]**2))
h2goal = bd.FUNCTION(lambda d: math.atan2(d[1], d[0]))
heading_error = bd.SUM('+-', angles=True)
Kv = bd.GAIN(0.5)
Kh = bd.GAIN(4)
bike = bd.BICYCLE(x0=[5, 2, 0])
vplot = bd.VEHICLEPLOT(scale=[0, 10],
size=0.7,
shape='box',
init=background_graphics,
movie='rvc4_4.mp4')
vscope = bd.SCOPE(name='velocity')
hscope = bd.SCOPE(name='heading')
mux = bd.MUX(2)
import bdsim
from bdsim.blocks.transfers import Integrator
## Lecture 10.4
# In this notebook, we will learn to model a dynamical system using the bdsim simulation class.
# Learn more at:\
# https://petercorke.github.io/bdsim/
# We will begin by modeling the mass-spring system which resembles the behvior of motors and thus it is appropriate to understand for robotics.
# First create a bdsim object instance
bd = bdsim.BlockDiagram(None)
# After that, create all necessary objects used in the dynamical system.
# define the blocks
demand = bd.STEP(T=1, pos=(0, 0), name='demand') # input step signal
sum = bd.SUM('+-', pos=(1, 0)) # addition operation
minv = bd.GAIN(0.5, pos=(1, 2)) # 1/m gain
b = bd.GAIN(-2.0, pos=(2, 0)) # -b gain
k = bd.GAIN(-10.0, pos=(3, -1)) # -K gain
int1 = bd.add_block(Integrator, name="xdd2xd")
int2 = bd.add_block(Integrator, name="xd2x")
scope = bd.SCOPE(name='x', styles=['k', 'r--'], pos=(6, 2))
### Connections
# In bdsim all wires are point to point, a one-to-many connection is implemented by many wires.
dt = 0.1
tacc = 1
# create the path
path = np.array([
[10, 10],
[10, 60],
[80, 80],
[50, 10]
])
robot_traj = rtb.mstraj(path[1:,:], qdmax=speed, q0=path[0,:], dt=0.1, tacc=tacc).q
total_time =robot_traj.shape[0] * dt + look_ahead / speed
bd = bdsim.BlockDiagram(graphics=False)
def background_graphics(ax):
ax.plot(path[:,0], path[:,1], 'r', linewidth=3, alpha=0.7)
def pure_pursuit(cp, R=None, traj=None):
# find closest point on the path to current point
d = np.linalg.norm(traj-cp, axis=1) # rely on implicit expansion
i = np.argmin(d)
# find all points on the path at least R away
k, = np.where(d[i+1:] >= R) # find all points beyond horizon
if len(k) == 0:
# no such points, we must be near the end, goal is the end
pstar = traj[-1,:]
else:
