Pose interpolation code

In [1]:
import geometry as geo
import numpy as np
import duckietown_world as dw
INFO:dt-world:duckietown-world 1.0.14
In [2]:
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:

In [3]:
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)

PlacedObject example

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.

In [4]:
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)]

Animation example

Create the interpolated poses:

In [5]:
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:

In [6]:
root = dw.PlacedObject()

Create a SampledSequence of the pose:

In [7]:
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.

In [8]:
root.set_object("me", Person(), ground_truth=seq_me)

Finally, draw the animation:

In [9]:
area = dw.RectangularArea((-1, -3), (3, 1))

draw_html(root, outdir='animation', area=area)
INFO:dt-world:area: RectangularArea(pmin=[-1. -3.],pmax=[ 3.  1.])
INFO:dt-world:Written SVG to animation/drawing.svg
INFO:dt-world:Written HTML to animation/drawing.html
In [10]: