import geometry as geo
import numpy as np
import duckietown_world as dw
def interpolate(q0, q1, alpha):
v = geo.SE2.algebra_from_group(geo.SE2.multiply(geo.SE2.inverse(q0), q1))
vi = v * alpha
q = np.dot(q0, geo.SE2.group_from_algebra(vi))
return q
Utility function to draw in IPython:
def draw_html(po, outdir=None, area=None):
if outdir is None:
outdir = 'out-%s' % id(po)
dw.draw_static(po, outdir, area=area)
from IPython.display import IFrame, display
iframe = IFrame(src=outdir + '/drawing.html', width='100%', height=600)
display(iframe)
All objects in the map are instances of PlacedObject
.
To create a new object, subclass PlacedObject
and implement the drawing method draw_svg
and the extent_points
method.
class Person(dw.PlacedObject):
def draw_svg(self, drawing, g):
# drawing is done using the library svgwrite
c = drawing.circle(center=(0, 0), r=0.3, fill='pink')
g.add(c)
dw.draw_axes(drawing, g)
def extent_points(self):
L = 0.3
return [(-L, -L), (+L, +L)]
Create the interpolated poses:
q0 = geo.SE2_from_translation_angle([0, 0], 0)
q1 = geo.SE2_from_translation_angle([2, -2], np.deg2rad(-90))
n = 10
seqs = []
steps = np.linspace(0, 1, num=n)
for alpha in steps:
q = interpolate(q0, q1, alpha)
seqs.append(q)
Create a root PlacedObject:
root = dw.PlacedObject()
Create a SampledSequence of the pose:
timestamps = range(len(seqs))
transforms = [dw.SE2Transform.from_SE2(_) for _ in seqs]
seq_me = dw.SampledSequence(timestamps, transforms)
Add the object me
to the root, saying it's a Person, and with the
sequence above given as ground truth for the pose.
root.set_object("me", Person(), ground_truth=seq_me)
Finally, draw the animation:
area = dw.RectangularArea((-1, -3), (3, 1))
draw_html(root, outdir='animation', area=area)