Optimizing Steering Car Paths with PyTorch


Want to see another bizarre way to use PyTorch? If you've read some of my recent posts, you've seen the basic idea before. I'm using automatic differentiation and gradient descent, this time to optimize the path of a car. The model is similar to a Reeds-Shepp car, a simple steering car model. The state at time step \(i\) is defined by a 2D position and an angle, \((x_i, y_i, \theta_i)\). In each step of the simulation, the car's state is updated based on an input consisting of speed and steering angle \((v_i, \phi_i)\).

\(x_{i+1} = x_i + v_i \cos{\theta_i} dt\)
\(y_{i+1} = y_i + v_i \sin{\theta_i} dt\)
\(\theta_{i+1} = \theta_i + v_i \phi_i dt\)

I simulated the car for \(n_\text{steps}\) steps, with a target position and angle, \((x_t, y_t, \theta_t)\) and a cost function, \(c\), which depended on the distance from the target position and the distance from the target angle in the final step.

\(c= \left(\theta_t - \theta_n\right)^2 +\left(x_t - x_n\right)^2 + \left(y_t - y_n\right)^2 \)

I used automatic differentiation to find the derivates \(\frac{dc}{dv_i}\) and \(\frac{dc}{d\phi_i}\), which I used to update the inputs.

Check out some results. The animated plot below shows an example optimization, with the working solution evolving over time. The car starts at \((0,0)\), facing right and its target is \((3,3)\), facing down. The starting position and direction are represented by the red arrow and the target position and direction are represented by the green arrow.

In the first frame of the animation, the inputs are initialized to zero and the car does not move. In the ultimate solution, the car performs a wide left turn into its final position. The plots below shows the input that produced that trajectory and the evolution of the cost function.

Code below.

import torch
import numpy as np
import matplotlib.pyplot as plt
from scipy import optimize

n_steps = 200 # number of steps to test the car performance
speeds = torch.zeros(n_steps-1, requires_grad=True) # init speed control tensor
steers = torch.zeros(n_steps-1, requires_grad=True) # init steering control tensir
dt = .1 # time step for dynamics

target_pos = torch.tensor([3.,3.]) # target position for the car
target_angle = torch.tensor(3*np.pi/2.) # target angle for the car

# finds the trajectory from the speed and steering commands
def test_car(speeds, steers):
    angles =[torch.tensor(0.)]
    xs = [torch.tensor(0.)]
    ys = [torch.tensor(0.)]

    # simulate the car performance
    for i in range(1,n_steps):
        speed =  torch.clamp(speeds[i-1], -1., 1.) # limit the speed
        steer = torch.clamp(steers[i-1], -10., 10.) # limit the steering angle
        xs.append(xs[-1]+ dt * speed * torch.cos(angles[-1]))
        ys.append(ys[-1]+ dt * speed * torch.sin(angles[-1]))
        angles.append((angles[i-1] + dt * speed * steer)%(2*np.pi))
    return xs, ys, angles


# list to keep track of objective progress
costs = []
angle_costs = []
dist_costs = []

# cost plot
fig_cost = plt.figure()
ax_cost = fig_cost.add_subplot(111)
ax_cost.autoscale(enable=True, axis="y", tight=False)
ax_cost.set_xlabel("iteration")
ax_cost.set_ylabel("cost")

li_cost, = ax_cost.plot([],[])
li_cost_ang, = ax_cost.plot([],[])
li_cost_dist, = ax_cost.plot([],[])

# input plot
fig_in = plt.figure()
ax_in = fig_in.add_subplot(111)
ax_in.autoscale(enable=True, axis="y", tight=False)

li_speed, = ax_in.plot(speeds.detach().numpy())
li_steer, = ax_in.plot(steers.detach().numpy())

# trajectory plot
fig_traj = plt.figure()
ax_traj = fig_traj.add_subplot(111)

li_traj, = ax_traj.plot([])
l = 0.25
ax_traj.arrow(0,0,0,l, width=0.05, color="red")
ax_traj.arrow(*target_pos, l* torch.cos(target_angle), l*torch.sin(target_angle), width=0.05, color="green")
ax_traj.set_xlim(-1.,4.)
ax_traj.set_ylim(-1.,4.)

fig_cost.canvas.draw()
fig_traj.canvas.draw()
fig_in.canvas.draw()
plt.show(block=False)

optimizer =  torch.optim.SGD([speeds, steers], lr=0.1)
#optimizer =  torch.optim.Adam([speeds, steers])

for i in range(170):
    # run simulation
    xs, ys, angles = test_car(speeds, steers)

    # calculate costs
    angle_error = target_angle - angles[-1]
    angle_cost = torch.min(torch.min((angle_error+2*np.pi)**2, (angle_error-2*np.pi)**2), angle_error**2)

    dist_cost = (target_pos[0] - xs[-1])**2 + (target_pos[1] - ys[-1])**2   

    cost = dist_cost + angle_cost

    # save costs
    costs.append(cost)
    angle_costs.append(angle_cost)
    dist_costs.append(dist_cost)

    #calculate gradients
    optimizer.zero_grad()
    cost.backward()

    # update inputs
    optimizer.step()

    # update plots
    li_cost.set_data(range(len(costs)),costs)
    li_cost_ang.set_data(range(len(angle_costs)),angle_costs)
    li_cost_dist.set_data(range(len(dist_costs)),dist_costs)
    li_speed.set_ydata(speeds.detach().numpy())
    li_steer.set_ydata(steers.detach().numpy())
    li_traj.set_data(xs, ys)
    
    ax_cost.relim()
    ax_cost.autoscale_view()
    ax_cost.legend(["total", "angle", "dist"])

    ax_in.relim()
    ax_in.autoscale_view()
    ax_in.legend(["speed", "steer"])

    fig_traj.canvas.draw()
    fig_cost.canvas.draw()
    fig_in.canvas.draw()
»


Reinventing the Wheel: Discovering the Optimal Rolling Shape with PyTorch


It is thought that the wheel was invented more than five thousand years ago, yet modern machine learning tools were only developed in the past few decades. We have all wondered about this paradox at some point. How could ancient Sumerian wheel builders have calculated the ideal shape without modern computers and software? We may never know. Luckily, with today's technology, it is relatively easy to rediscover the wheel's optimal shape. I'll show you how I did it.

The goal of this goofy project was to find the optimal shape of a wheel, which is probably a circle. I parameterized the wheel as a twenty sided polygon with a variable radius for each vertex, \(r_i\). The performance of the wheel was based on the final speed achieved by an accelerating imaginary car, \(v_f\). The wheel was driven with a constant torque, \(\tau\), and no slipping. I used PyTorch to simulate the effect of a wheel and calculate the gradients in the final speed with respect to the radii, \(\frac{dv_f}{dr_i}\). I used these gradients to update the radii using gradient descent. Above, you can see the shape of an initially randomly generated wheel the wheel evolving, along with the changing objecive function.

I represented the wheel with a relatively simple model. In each time step, I updated the speed of the car based on the force from the wheel, then rotated the wheel based on the speed. The lowest vertex was touching the ground and was fixed in space. The wheel generated a force on the car \(\tau/r_i\), where \(r_i\) is the radius of the vertex. A component of this force, shown in the diagram below, accelerated the car and increased its speed.



Next, I think it would be fun to try surfaces that aren't flat. If I used that rolling square surface, would the optimal shape be a square?

import torch
import numpy as np
import matplotlib.pyplot as plt

n = 20 # number of wheel points
n_steps = 1000 # number of steps to test the wheel performance
#radii = torch.tensor([2.] + [1.] * (n-1), requires_grad=True) # init wheel radii with one bump
radii = torch.tensor(np.random.normal(1., .4, size=n).astype(np.float32), requires_grad=True) # init random wheel point radii
dtheta = 2. * np.pi / n # angle between wheel points
thetas = torch.arange(0,2*np.pi, dtheta) # list of wheel point angles
dt = 0.1 # time step for dynamics
torque = 1. # torque on the wheel
mass = 10. # mass of the car
gravity = .1 # acceleration due to gravity

# finds the final speed of the wheel from radii
def test_wheel(radii):
    # normalize the radii so that changing the size has no effect
    norm_radii = radii / torch.mean(radii)

    # find the coordinates of the wheel points
    vertex_positions = wheel_coordinates(norm_radii)

    speeds = torch.zeros(n_steps) + 1. # speed of the cart after each step
    angle = 0

    # simulate the wheel performance
    for i in range(1,n_steps):
        # find pivot properties
        pivot_index = torch.argmin(vertex_positions[:,1])
        pivot_radius = norm_radii[pivot_index]
        pivot_vector = vertex_positions[pivot_index]
        pivot_angle = torch.atan2(pivot_vector[1], pivot_vector[0])

        # don't use pivot angle, just take the height of the pivot
        
        # calculate force at the pivot point
        force = torque / pivot_radius

        #force_gravity = gravity * mass * torch.sin(pivot_angle + np.pi/2)
        force_gravity = 0

        # calculate the component of pivot force that pushes the car forward
        force_component = torch.cos(pivot_angle + np.pi/2)

        # calculate the new speed of the car and the rotational speed of the wheel
        speeds[i] = speeds[i-1] + ((force * force_component + force_gravity) * dt / mass)
        rotational_speed = speeds[i] * force_component / pivot_radius
        
        dangle = rotational_speed * dt
        angle += dangle
        
        # rotate the wheel
        vertex_positions = rotate(vertex_positions, dangle)

    #print(angle)
    return speeds

def draw_wheel(vertex_positions):
    xs, ys = vertex_positions.transpose(0,1).detach().numpy()
    plt.scatter(xs, ys)
    
# returns a rotation matrix
def rotation_matrix(theta):
    return torch.tensor([[torch.cos(theta), torch.sin(theta)],[-torch.sin(theta), torch.cos(theta)]])

# returns the wheel coordinates based on the radii
def wheel_coordinates(radii):
    vertex_positions = (radii.unsqueeze(1) * torch.cat((torch.cos(thetas.reshape((-1,1))),torch.sin(thetas.reshape((-1,1)))), 1))
    return vertex_positions

# returns a rotated set of vertex positions
def rotate(vertex_positions, theta):
    rot_matrix = rotation_matrix(theta)
    vertex_positions = torch.matmul(rot_matrix, vertex_positions.transpose(0,1)).transpose(0,1)
    return vertex_positions

# list to keep track of objective progress
final_speeds = []

# objective plot
fig_obj = plt.figure()
ax_obj = fig_obj.add_subplot(111)
ax_obj.autoscale(enable=True, axis="y", tight=False)
ax_obj.set_xlabel("iteration")
ax_obj.set_ylabel("final speed")

li_obj, = ax_obj.plot(final_speeds)

# radii plot
fig_rad = plt.figure()
ax_rad = fig_rad.add_subplot(111)

li_rad, = ax_rad.plot(radii.detach().numpy())

# wheel shape plot
fig_wheel = plt.figure()
ax_wheel = fig_wheel.add_subplot(111)
ax_wheel.set_aspect('equal', 'datalim')

vertex_positions = wheel_coordinates(radii)
vertex_xs, vertex_ys = vertex_positions.transpose(0,1).detach().numpy()
li_wheel, = ax_wheel.fill(vertex_xs, vertex_ys)
li_hub, = ax_wheel.plot(0, 0, 'b.')

fig_obj.canvas.draw()
fig_rad.canvas.draw()
fig_wheel.canvas.draw()
plt.show(block=False)

lr = 0.03 # learning rate
for i in range(100000):
    # simulate wheel
    speeds = test_wheel(radii)

    # cost
    final_speeds.append(speeds[-1])
    cost = -speeds[-1]

    # calculate gradients
    cost.backward(retain_graph=False)

    # gradient descent
    with torch.no_grad():
        radii -= radii.grad * lr
        radii.grad = None

    # update plots
    li_obj.set_data(range(len(final_speeds)),final_speeds)
    li_rad.set_ydata(radii.detach().numpy())

    ax_obj.relim()
    ax_obj.autoscale_view()


    vertex_positions = wheel_coordinates(radii)
    vertex_xs, vertex_ys = vertex_positions.transpose(0,1).detach().numpy()
    ax_wheel.clear()
    ax_wheel.set_aspect('equal', 'datalim')
    ax_wheel.fill(vertex_xs, vertex_ys)
    ax_wheel.plot(0, 0, 'b.')

    fig_obj.canvas.draw()
    fig_rad.canvas.draw()
    fig_wheel.canvas.draw()

    fig_wheel.savefig("vid_3/wheel_{i}.jpg".format(i=i))
    fig_obj.savefig("vid_3/obj.jpg")

     
»


Haskell Integer to English

Inspired by a programming interview question I heard about, here's some cute code I wrote for converting (positive) integers to words. It breaks the number into groups of three digits, converts each group into English, and intersperses words like million and billion where necessary. I did the three digit number conversion using a cascading pattern matching system.

I had never thought about this problem much and was surprised by how many little quirks there are.

import Data.List

suffixes = ["" ,"thousand", "million", "billion", "trillion", "quadrillion"]

toDigits :: Int -> [Int]
toDigits 0 = []
toDigits x = toDigits (x `div` 10) ++ [x `mod` 10]

(+++) :: String -> String -> String
"" +++ "" = ""
a +++ "" = a
"" +++ a = a
a +++ b = a ++ " " ++ b

digitGroups ::Int -> [(Int, Int, Int)]
digitGroups n = digitGroups' $ reverse $ toDigits $ n

digitGroups' :: [Int] -> [(Int, Int, Int)]
digitGroups' (a:b:c:ds) = (c,b,a):(digitGroups' ds)
digitGroups' (a:b:[]) = (0,b,a):[]
digitGroups' (a:[]) = (0,0,a):[]
digitGroups' [] = []

groupToWords :: (Int, Int, Int) -> String -> String
groupToWords digits suffix = (toWords3 digits) +++ suffix

toWords :: Int -> String
toWords 0 = "zero"
toWords n = foldl (+++) "" (reverse (zipWith groupToWords  (digitGroups n) suffixes))

toWords3 :: (Int, Int, Int) -> String
toWords3 (0,a,b) = toWords2 (a,b)
toWords3 (a,b,c) = (toWords1 a) +++ "hundred" +++ (toWords2 (b,c))

toWords2 :: (Int, Int) -> String
toWords2 (0,a) = toWords1 a
toWords2 (1,0) = "ten"
toWords2 (1,1) = "eleven"
toWords2 (1,2) = "twelve"
toWords2 (1,3) = "thirteen"
toWords2 (1,5) = "fifteen"
toWords2 (1,8) = "eighteen"
toWords2 (1,a) = (toWords1 a) ++ "teen"
toWords2 (2,a) = "twenty" +++ (toWords1 a) 
toWords2 (3,a) = "thirty" +++ (toWords1 a) 
toWords2 (4,a) = "forty" +++ (toWords1 a) 
toWords2 (5,a) = "fifty" +++ (toWords1 a) 
toWords2 (6,a) = "sixty" +++ (toWords1 a) 
toWords2 (7,a) = "seventy" +++ (toWords1 a) 
toWords2 (8,a) = "eighty" +++ (toWords1 a) 
toWords2 (9,a) = "ninety" +++ (toWords1 a) 

toWords1 :: Int -> String
toWords1 0 = ""
toWords1 1 = "one"
toWords1 2 = "two"
toWords1 3 = "three"
toWords1 4 = "four"
toWords1 5 = "five"
toWords1 6 = "six"
toWords1 7 = "seven"
toWords1 8 = "eight"
toWords1 9 = "nine"
»


Hanayama O'Gear Puzzle

Declan showed me a brain teaser his friend sent him. Apparently, it's known as the Hanayama O'Gear Puzzle. It consists of a bronze colored metal cube with a captive gear which can roll from face to face on the cube. The gear can also rotate between two directions on each face. Some edges of the cube are made so that the gear can't roll over them. One tooth of the gear and one face of the cube have a special cut-out. Only when the special tooth is engaged with the special face can the gear be freed. The object of the puzzle is to manipulate the gear into this orientation. Here's a video of the puzzle in action.



I tried solving the puzzle for about fifteen minutes before deciding to use a computer. My basic strategy was to treat the problem as a graph search, where the nodes of the graph are the different states the cube and gear can be in. At any time, you can only do three things: rotate, roll forward, and roll backward. I figured that the limited set of moves and possible positions would prevent the search space from getting too big.

I marked up the cube with stickers, naming each face and defining its 'north' direction. I also marked the gear by labeling each tooth and setting a forward rotation direction.

I don't know Haskell very well, so I thought using it would be a fun challenge. Plus, I had Haskell guru Phil to ask for help and advice. A lot of the code is just defining the connectivity of the puzzle. I used a list to keep track of the links between different faces and a pattern-matched function to keep track of the possible rotations. I defined north on each face so that these rotations were always the same. A north-facing gear could only rotate to the east, and a south-facing one could only rotate west.

Beyond the trickiness of defining directions and connectivity, I just implemented a straightforward breadth-first search. The program creates a list of all states accessible from the starting state. Because the list is generated with successive expansions of the search frontier, it is naturally sorted by search depth. I take advantage of Haskell's lazy evaluation, creating an infinite list of accessible states but only ever calculating a few of them. Code below.

import Data.Maybe (catMaybes)
import Data.List (find)
import Data.Tuple (swap)

data Face = Top | Bot | Lef | Ri | Front | Back deriving (Show, Eq)
data Dir = North | South | East | West deriving (Show, Eq)

data GearState = GearState{ face :: Face, dir :: Dir, tooth :: Int, history :: [(Face, Dir)] } deriving Show

-- a list defining connections between faces
forward_connections = 
    [ ((Top, East),(Ri, South))
    , ((Top, West),(Lef, South))

    , ((Bot, North),(Front, West))
    , ((Bot, South),(Back, East))
    , ((Bot, West),(Ri, North)) 

    , ((Front, South),(Bot, North)) 
    , ((Front, West),(Back, West)) 

    , ((Back, South),(Bot, North)) 
    , ((Back, East),(Ri, East)) 
    , ((Back, West),(Lef, West)) 

    , ((Lef, North),(Top, East)) 
    , ((Lef, East),(Front, East))
    , ((Lef, West),(Back, West)) 
    
    , ((Ri, North),(Top, West)) 
    , ((Ri, South),(Bot, East)) 
    , ((Ri, East),(Back, East))  
    ]

-- mapping the reverse tuple function over the connections gives the connections for reverse gear moves
backward_connections = map swap forward_connections

-- a function which returns a gear moved forward or backward
-- takes a list of connections between faces and a function for finding the next tooth
-- forward_connections and incr for forward moves
-- backward_connections and decr for backward moves
move :: GearState -> [((Face, Dir),(Face, Dir))] -> (Int -> Int) -> Maybe GearState
move (GearState face dir tooth history) connections nextTooth = 
    case (lookup (face, dir) connections) of -- looks for a connection
        Nothing -> Nothing -- if no forward connection exists from the current state
        (Just (new_face, new_dir)) -> Just (GearState new_face new_dir (nextTooth tooth) ((new_face,new_dir):history)) -- returns a new state, incrementing the tooth and adding to the history

-- a function which applies a turn to the gear
turn :: GearState -> GearState
turn (GearState face North tooth history) = GearState face East tooth ((face,North):history)
turn (GearState face East tooth history) = GearState face North tooth ((face,East):history)

turn (GearState face South tooth history) = GearState face West tooth ((face,South):history)
turn (GearState face West tooth history) = GearState face South tooth ((face,West):history)

-- some mod 5 math for the teeth
mod5 :: Int -> Int
mod5 = (flip mod) 5

incr :: Int -> Int
incr x = mod5 (x+1)

decr :: Int -> Int
decr x = mod5 (x-1)

-- determines if the gear can leave the cube
isWinningState :: GearState -> Bool
isWinningState (GearState Top North 0 _) = True
isWinningState _ = False

-- creates a list of all the states accessible from the current state
nextStates :: GearState -> [GearState]
nextStates state = catMaybes [Just (turn state), (move state forward_connections incr), (move state backward_connections decr)] -- removes Nothings and strips the values out of Just

-- creates a new search frontier by replacing each state in the search frontier with all the states accessible from that state and flattening
newFrontier :: [GearState] -> [GearState]
newFrontier frontier = (concatMap nextStates frontier)

-- creates a list of all future frontiers
reachableStates :: GearState -> [GearState]
reachableStates start = concat (iterate newFrontier [start])

-- finds the winning state
solve :: GearState -> Maybe [(Face, Dir)]
solve start = fmap history (find isWinningState (reachableStates start))

You can run it with a command like, solve (GearState Top South 4 []). This code, and a different Python solver I wrote are available in my Github.

»


Couch Table

Matt and I needed a quick project for Christmas time. We came up with a small table to fit over the arm of a couch.



It's an original design with a u-shaped base that allows the table to slide forward and out of the way of the arm rest by wrapping around the leg of the couch. We made it out of red oak with two small walnut accents. Plans below.



First we built the base. We ripped 2" nominal thick red oak into 1.5" strips and chopped them to length. We cut shallow angles into the parallel parts of the base using a band saw and hand plane. We used hand tools to cut mitered bridle joint corners. These were the toughest part of the project, and we spent a lot of time fussing over the precise fit.







We made the top of the table by chopping a piece of 1x12" red oak. We beveled three edges using a hand plane and cut the through mortices.



Next, we made the vertical legs of the table. We ripped 1.25" strips of 1" nominal red oak and chopped them to length. We cut through tenons for the top and bottom with 1/8" shoulders.



We decided to try wedged tenons for a nice look on the top of the table. By eye, we cut some slim wedges out of walnut on the band saw. We also cut slots into the top tenon and flaired out the corresponding mortices slightly to accomodate the extra width from the wedges.



We glued everything together and gently hammered the wedges in.



Here's what they looked like after we sawed off the excess.



Finally, we planed all of the joint surfaces flat and sanded down to 200 grit. For a finish, we decided to use danish oil. I highly recommend it. At least for red oak, it make the grain stand out and gives the whole thing a wonderful color. We also applied a layer of finishing wax.



This table was fun to build and came out looking good. It doesn't feel too unstable, either. With some simple length adjustments, it would be easy to fit this design to different couches and chairs.

»