EECS16A: Homework 3
Problem 5: Segway Tours
Run the following block of code first to get all the dependencies.
In [2]:
# %load gauss_elim.py
from gauss_elim import gauss_elim
In [3]:
from numpy import zeros, cos, sin, arange, around, hstack
from matplotlib import pyplot as plt
from matplotlib import animation
from [Link] import Rectangle
import numpy as np
from [Link] import interp1d
import scipy as sp
Dynamics
In [4]:
# Dynamics: state to state
A = [Link]([[1, 0.05, -.01, 0],
[0, 0.22, -.17, -.01],
[0, 0.1, 1.14, 0.10],
[0, 1.66, 2.85, 1.14]]);
# Control to state
b = [Link]([.01, .21, -.03, -0.44])
nr_states = [Link][0]
# Initial state
state0 = [Link]([-0.3853493, 6.1032227, 0.8120005, -14])
# Final (terminal state)
stateFinal = [Link]([0, 0, 0, 0])
Part (d), (e), (f)
In [16]:
# You may use gauss_elim to help you find the row reduced echelo
n form.
# Part D
DCol1 = [Link](A, b)
DCol2 = b
DCol3 = stateFinal
DCol4 = stateFinal
DmatrixA = [Link]([DCol1, DCol2])
DmatrixA = [Link]([DmatrixA, DCol3])
DmatrixA = [Link]([DmatrixA, DCol4])
DvectorB = [Link](A, A)
DvectorB = [Link](DvectorB, b)
Dfinal = gauss_elim([Link]([Link]([DvectorB, DmatrixA])
))
print("Solution D")
print(Dfinal)
# Part E
ECol1 = [Link](A, A)
ECol1 = [Link](ECol1, b)
ECol2 = [Link](A, b)
ECol3 = b
ECol4 = stateFinal
EmatrixA = [Link]([ECol1, ECol2])
EmatrixA = [Link]([EmatrixA, ECol3])
EmatrixA = [Link]([EmatrixA, ECol4])
EvectorB = [Link](A, A)
EvectorB = [Link](EvectorB, A)
EvectorB = [Link](EvectorB, b)
Efinal = gauss_elim([Link]([Link]([EvectorB, EmatrixA])
))
print("Solution E")
print(Efinal)
# Part F
FCol1 = [Link](A, A)
FCol1 = [Link](FCol1, A)
FCol1 = [Link](FCol1, b)
FCol2 = [Link](A, A)
FCol2 = [Link](FCol2, b)
FCol3 = [Link](A, b)
FCol4 = b
FmatrixA = [Link]([FCol1, FCol2])
FmatrixA = [Link]([FmatrixA, FCol3])
FmatrixA = [Link]([FmatrixA, FCol4])
FvectorB = [Link](A, A)
FvectorB = [Link](FvectorB, A)
FvectorB = [Link](FvectorB, A)
FvectorB = [Link](FvectorB, b)
Ffinal = gauss_elim([Link]([Link]([FvectorB, FmatrixA])
))
print("Solution F")
print(Ffinal)
Solution D
[[1. 0. 0. 0. 0.]
[0. 1. 0. 0. 0.]
[0. 0. 1. 0. 0.]
[0. 0. 0. 0. 0.]]
Solution E
[[1. 0. 0. 0. 0.]
[0. 1. 0. 0. 0.]
[0. 0. 1. 0. 0.]
[0. 0. 0. 1. 0.]]
Solution F
[[ 1. 0. 0. 0.
-4.3394114 ]
[ 0. 1. 0. 0.
15.18793991]
[ 0. 0. 1. 0.
-17.5737483 ]
[ 0. 0. 0. 1.
7.72521979]]
Part (g)
Preamble
This function will take care of animating the segway.
In [17]:
# frames per second in simulation
fps = 20
# length of the segway arm/stick
stick_length = 1.
def animate_segway(t, states, controls, length):
#Animates the segway
# Set up the figure, the axis, and the plot elements we want
to animate
fig = [Link]()
# some config
segway_width = 0.4
segway_height = 0.2
# x coordinate of the segway stick
segwayStick_x = length * [Link](states[:, 0],sin(states[:, 2
]))
segwayStick_y = length * cos(states[:, 2])
# set the limits
xmin = min(around(states[:, 0].min() - segway_width / 2.0, 1
), around(segwayStick_x.min(), 1))
xmax = max(around(states[:, 0].max() + segway_height / 2.0,
1), around(segwayStick_y.max(), 1))
# create the axes
ax = [Link](xlim=(xmin-.2, xmax+.2), ylim=(-length-.1, len
gth+.1), aspect='equal')
# display the current time
time_text = [Link](0.05, 0.9, '', transform=[Link])
# display the current control
control_text = [Link](0.05, 0.8, '', transform=[Link]
)
# create rectangle for the segway
rect = Rectangle([states[0, 0] - segway_width / 2.0, -segway
_height / 2],
segway_width, segway_height, fill=True, color='gold', ec
='blue')
ax.add_patch(rect)
# blank line for the stick with o for the ends
stick_line, = [Link]([], [], lw=2, marker='o', markersize=6
, color='blue')
# vector for the control (force)
force_vec = [Link]([],[],[],[],angles='xy',scale_units='x
y',scale=1)
# initialization function: plot the background of each frame
def init():
time_text.set_text('')
control_text.set_text('')
rect.set_xy((0.0, 0.0))
stick_line.set_data([], [])
return time_text, rect, stick_line, control_text
# animation function: update the objects
def animate(i):
time_text.set_text('time = {:2.2f}'.format(t[i]))
control_text.set_text('force = {:2.3f}'.format(controls[
i]))
rect.set_xy((states[i, 0] - segway_width / 2.0, -segway_
height / 2))
stick_line.set_data([states[i, 0], segwayStick_x[i]], [0
, segwayStick_y[i]])
return time_text, rect, stick_line, control_text
# call the animator function
anim = [Link](fig, animate, frames=len(t),
init_func=init,
interval=1000/fps, blit=False, repeat=False)
return anim
# [Link]()
Plug in your controller here
In [18]:
controls = [Link]([-4.3394114,15.18793991,-17.5737483,7.725219
79]) # here
Simulation
In [19]:
# This will add an extra couple of seconds to the simulation aft
er the input controls with no control
# the effect of this is just to show how the system will continu
e after the controller "stops controlling"
controls = [Link](controls,[0, 0])
# number of steps in the simulation
nr_steps = [Link][0]
# We now compute finer dynamics and control vectors for smoother
visualization
Afine = [Link].fractional_matrix_power(A,(1/fps))
Asum = [Link](nr_states)
for i in range(1, fps):
Asum = Asum + [Link].matrix_power(Afine,i)
bfine = [Link](Asum).dot(b)
# We also expand the controls in the "intermediate steps" (only
for visualization)
controls_final = [Link](controls, [Link](fps)).flatten()
controls_final = [Link](controls_final, [0])
# We compute all the states starting from x0 and using the contr
ols
states = [Link]([fps*(nr_steps)+1, nr_states])
states[0,:] = state0;
for stepId in range(1,fps*(nr_steps)+1):
states[stepId, :] = [Link](Afine,states[stepId-1, :]) + cont
rols_final[stepId-1] * bfine
# Now create the time vector for simulation
t = [Link](1/fps,nr_steps,fps*(nr_steps),endpoint=True)
t = [Link]([0], t)
Visualization
In [11]:
%matplotlib nbagg
# %matplotlib qt
anim = animate_segway(t, states, controls_final, stick_length)
anim
Out[11]:
<[Link] at 0xd15e7d610>
In [ ]:
