5/10/2018 ME003-SimBot-2

http://localhost:8888/nbconvert/html/Downloads/SimRobot-2/ME003-SimBot-2.ipynb?download=false 1/19
5/10/2018 ME003-SimBot-2

In [2]: """Header cell, contains modules and functions to make the whole noteboo
k experience better"""
%matplotlib inline
# plots graphs within the notebook

from IPython.display import display,Image, Latex

from sympy.interactive import printing
printing.init_printing(use_latex='mathjax')
from IPython.display import clear_output

import time

from IPython.display import display,Image, Latex

from IPython.display import clear_output

import matplotlib.pyplot as plt

import numpy as np
import math
import scipy.constants as sc

import sympy as sym

font = {'family' : 'serif',

#'color' : 'black',
'weight' : 'normal',
'size' : 12,
}
fontlabel = {'family' : 'serif',
#'color' : 'black',
'weight' : 'normal',
'size' : 12,
}

from matplotlib.ticker import FormatStrFormatter

plt.rc('font', **font)

class PDF(object):
def __init__(self, pdf, size=(200,200)):
self.pdf = pdf
self.size = size

def _repr_html_(self):
return '<iframe src={0} width={1[0]} height={1[1]}></iframe>'.format
(self.pdf, self.size)

def _repr_latex_(self):
return r'\includegraphics[width=1.0\textwidth]{{{0}}}'.format(self.p
df)

class ListTable(list):
""" Overridden list class which takes a 2-dimensional list of
the form [[1,2,3],[4,5,6]], and renders an HTML Table in

http://localhost:8888/nbconvert/html/Downloads/SimRobot-2/ME003-SimBot-2.ipynb?download=false 2/19
5/10/2018 ME003-SimBot-2
IPython Notebook. """

def _repr_html_(self):
html = ["<table>"]
for row in self:
html.append("<tr>")

for col in row:

html.append("<td>{0}</td>".format(col))

html.append("</tr>")
html.append("</table>")
return ''.join(html)

http://localhost:8888/nbconvert/html/Downloads/SimRobot-2/ME003-SimBot-2.ipynb?download=false 3/19
5/10/2018 ME003-SimBot-2

In [3]: import matplotlib.pyplot as plt

import numpy as np
import math
import scipy.constants as sc

import sympy as sym

from math import sin, cos

class Robot(object):
"""Defines basic mobile robot properties"""
def __init__(self):
self.pos_x = 0.0
self.pos_y = 0.0
self.angle = 0.0
self.plot = False
self._delta = 0.01
self.step_plot = int(5)
self.mag_plot = 1.

# Movement
def step(self):
""" updates the x,y and angle """
self.deltax()
self.deltay()
self.deltaa()

def move(self, seconds):

""" Moves the robot for an 's' amount of seconds"""
for i in range(int(seconds/self._delta)):
self.step()
if i % self.step_plot == 0 and self.plot: # plot path every
3 steps
self.plot_xya()

# Printing-and-plotting:
def print_xya(self):
""" prints the x,y position and angle """
print ("x = " + str(self.pos_x) +" "+ "y = " + str(self.pos_y))
print ("a = " + str(self.angle))

def plot_robot(self):
""" plots a representation of the robot """
plt.arrow(self.pos_x, self.pos_y, 0.001
* cos(self.angle), 0.001 * sin(self.angle),
head_width=self.mag_plot*self.length, head_length=self
.mag_plot*self.length,
fc='k', ec='k')

def plot_xya(self):
""" plots a dot in the position of the robot """
plt.scatter(self.pos_x, self.pos_y, c='r', edgecolors='r')

http://localhost:8888/nbconvert/html/Downloads/SimRobot-2/ME003-SimBot-2.ipynb?download=false 4/19
5/10/2018 ME003-SimBot-2
class DDRobot(Robot):
"""Defines a differential drive robot"""

def __init__(self):
Robot.__init__(self)
self.radius = 0.1
self.length = 0.4

self.rt_spd_left = 0.0
self.rt_spd_right = 0.0

def deltax(self):
""" update x depending on l and r angular speeds """
self.pos_x += self._delta * (self.radius*0.5) \
* (self.rt_spd_right + self.rt_spd_left)*cos(self.angle)

def deltay(self):
""" update y depending on l and r angular speeds """
self.pos_y += self._delta * (self.radius*0.5) \
* (self.rt_spd_right + self.rt_spd_left)*sin(self.angle)

def deltaa(self):
""" update z depending on l and r angular speeds """
self.angle += self._delta * (self.radius/self.length) \
* (self.rt_spd_right - self.rt_spd_left)
# function to convert degrees to radians
def D2R(a):
return math.pi*a/180
def R2D(a):
return 180*a/math.pi
# mybot = DDRobot() # robot called 'enesbot'

# mybot.angle = 3.1416/4 # 45 degrees

# mybot.plot = True # plot the robot!
# mybot.plot_robot()

# mybot.rt_spd_left = 10
# mybot.rt_spd_right = 10 # straight line
# mybot.move(2) # move for 2 seconds

# mybot.rt_spd_left = 12.5664
# mybot.rt_spd_right = 18.8496 # (2m diameter circle)
# mybot.move(1) # move for 1 second

# mybot.rt_spd_left = 18.8496
# mybot.rt_spd_right = 12.5664 # (2m diameter circle)
# mybot.move(2.5) # move for 2.5 second

# mybot.rt_spd_left = 12.5664
# mybot.rt_spd_right = 18.8496 # (2m diameter circle)
# mybot.move(3.5) # move for 2.5 second

# mybot.plot_robot()

# plt.xlim([-1, 6]) # axis limits

# plt.ylim([-1, 6])

http://localhost:8888/nbconvert/html/Downloads/SimRobot-2/ME003-SimBot-2.ipynb?download=false 5/19
5/10/2018 ME003-SimBot-2

# plt.show()
In [4]: PDF('bot-sketch.pdf',size = (550,400))

Out[4]:

http://localhost:8888/nbconvert/html/Downloads/SimRobot-2/ME003-SimBot-2.ipynb?download=false 6/19
5/10/2018 ME003-SimBot-2

Differential Drive Robot - 2: Calibration

Alphabot 2 is a differential drive robot: Two independent motors drive the left and right wheels and the
differential speed between the two wheels control the speed and direction of the robot. The following code is
copied from http://enesbot.me/kinematic-model-of-a-differential-drive-robot.html (http://enesbot.me/kinematic-
model-of-a-differential-drive-robot.html)

Objective
The objective is to investigate methods of calibration of the robot.

Quick Theoretical Background

The robot parameters are:

Wheel radius: r .
Length between wheels: L
Angular velocity of the left and right wheels: ω L , ω R , respectively.
Angle from horizontal: α.
Position vector of the robot: (x, y).
Velocity vector of the robot: V = (ẋ , ẏ) .
Hereafter ȧ refers to the time derivative of variable a:

da
ȧ =
dt

The velocities of the wheels are therefore defined as:

VL = ω L r and VR = ω R r.

The velocity of the robot, taken at the center of the wheels, is simply:

VR + VL
V ⃗  = ⃗  + sin(αey
(cos(α)ex ⃗  ),
2

yielding the following equations of motions:

ωR + ωL
ẋ = r cos α
2

ωR + ωL
ẏ = r sin α
2

and
r
α̇ = (ω R − ω L )
L

Position of the Problem

http://localhost:8888/nbconvert/html/Downloads/SimRobot-2/ME003-SimBot-2.ipynb?download=false 7/19
5/10/2018 ME003-SimBot-2

In a perfect world if your code sets the same rotation speed for both wheels, the robot travels on a straight line.
In the present case of independent motors, this outcome is very unlikely. Actually one should not expect the
robot to drive straight. Below is a model of drift. Here the drift has two components: a constant drift (it could be
a signal offset issue), and complex mechanical friction modeled by a quadratic function of the rotation speed.
You will assume that you don't know the equation of the drift.

Goal

In [5]: a = np.random.random(2)# random drift amplitude

print(a)
def drift(omega):
f = a[0] + a[1] * (omega/5)**2
return f

[ 0.22200638 0.76317789]

http://localhost:8888/nbconvert/html/Downloads/SimRobot-2/ME003-SimBot-2.ipynb?download=false 8/19
5/10/2018 ME003-SimBot-2

In [6]: T = 10
omega = np.array([1, 2.5, 5, 7.5, 10])
end_angle = np.zeros(5)
omega_R = omega + drift(omega)
omega_L = omega
mybot = DDRobot()
mybot.mag_plot = 2 #coefficient of magnification of the arrow
mybot.step_plot = 100 # plot location every 100 iterations

for i in range(5):
mybot.pos_x = 0 #x
mybot.pos_y = 0 #y
mybot.angle = D2R(0) #alpha
mybot.length =0.4 #L
mybot.radius = 0.1 #r
mybot.rt_spd_left = omega_L[i]
mybot.rt_spd_right = omega_R[i]

mybot.plot = True #True if you want to plot the robot's trajectory

mybot.plot_robot() #draw an arrow for the location of the robot at t
=0

mybot.move(T) #move the robot for 10s

mybot.plot_robot() #draw the new location
end_angle[i] = mybot.angle

plt.xlim(-5,11)
plt.ylim(-5,11)
plt.xlabel(r"$x$")
plt.ylabel(r"$y$")
plt.show()

Note in the code above, the angle at the end of the motion is stored in the array end_angle. As shown below,
the drift appears to follow a parabola.

http://localhost:8888/nbconvert/html/Downloads/SimRobot-2/ME003-SimBot-2.ipynb?download=false 9/19
5/10/2018 ME003-SimBot-2

In [7]: plt.plot(omega,end_angle,'o',label="experiment")
plt.xlabel(r"$\omega$")
plt.ylabel(r"$\alpha$")
plt.legend(loc=3, bbox_to_anchor=[0, 1.], ncol=2, shadow=False, fancybox
=True)
plt.show()

Let's now fit a quadratic polynomial to these points. The creation of a polynomial in python is shown below
using numpy polyfit function. p2_coef are the coefficients pi of the polynomial p(x) = p0 x
2
+ p1 x + p2 (see
https://docs.scipy.org/doc/numpy-1.14.0/reference/generated/numpy.polyfit.html
(https://docs.scipy.org/doc/numpy-1.14.0/reference/generated/numpy.polyfit.html)).

In [8]: p2_coef = np.polyfit(omega,end_angle,2)

print(p2_coef)

[ 7.63177892e-02 4.69287995e-14 5.55015940e-01]

To generate a function, use the numpy function poly1d:

In [9]: p2 = np.poly1d(p2_coef)
p2 = np.poly1d(np.polyfit(omega,end_angle,2)) #equivalent compact form

You can now generate a fit over a range of your choice, here 0 ≤ ω ≤ 10

http://localhost:8888/nbconvert/html/Downloads/SimRobot-2/ME003-SimBot-2.ipynb?download=false 10/19
5/10/2018 ME003-SimBot-2

In [10]: N = 51
om = np.linspace(0,10,N)

plt.plot(omega,end_angle,'o',label="experiment")
plt.plot(om,p2(om),label='fit')
plt.xlabel(r"$\omega$")
plt.ylabel(r"$\alpha$")
plt.legend(loc=3, bbox_to_anchor=[0, 1.], ncol=2, shadow=False, fancybox
=True)
plt.show()

In the previous assignment, the time necessary to rotate the robot by an angle α was found to be:
αo L
T =
r(ω R − ω L )

Introduce the drift at δ , i.e. ω L = ω and ω R = ω + δ, and derive a correction for the drift at as a function of the
array end_angle. Using the knowledge of the drift for all 5 rotation speed in the array omega, create a
polynomial to compute the drift for ω ∈ [0, 10] and introduce this polynomial into the code below to correct for
the drift.

In [11]: # your code here

dft = (end_angle * mybot.length) / (T * mybot.radius)
print(dft)
fixed = np.poly1d(np.polyfit(omega,dft,2))
print(fixed)

[ 0.25253349 0.41280085 0.98518427 1.93915663 3.27471794]

2
0.03053 x + 1.889e-14 x + 0.222

http://localhost:8888/nbconvert/html/Downloads/SimRobot-2/ME003-SimBot-2.ipynb?download=false 11/19
5/10/2018 ME003-SimBot-2

In [12]: omega = np.array([1, 2.5, 5, 7.5, 10])

end_angle = np.zeros(5)
omega_L = omega
omega_R = omega + drift(omega) # DO NOT MODIFY
mybot = DDRobot()
mybot.mag_plot = 2 #coefficient of magnification of the arrow
mybot.step_plot = 100 # plot location every 100 iterations

for i in range(5):
mybot.pos_x = 0 #x
mybot.pos_y = 0 #y
mybot.angle = D2R(0) #alpha
mybot.length =0.4 #L
mybot.radius = 0.1 #r
mybot.rt_spd_left = omega_L[i]
mybot.rt_spd_right = omega_R[i] - fixed(omega[i]) #write your correc
tion here

mybot.plot = True #True if you want to plot the robot's trajectory

mybot.plot_robot() #draw an arrow for the location of the robot at t
=0

mybot.move(10) #move the robot for 10s

mybot.plot_robot() #draw the new location
end_angle[i] = mybot.angle

plt.xlim(-1,11)
plt.ylim(-1,11)
plt.xlabel(r"$x$")
plt.ylabel(r"$y$")
plt.show()

Now apply your correction to the following example. In this example the robot experiences 5 step motions of
different durations and speed. The robot is supposed to move in a straight line. Print the angle of the robot at
the end of each step. Does your correction work? What would it take the correction to work for positive AND
negative rotation speed?

http://localhost:8888/nbconvert/html/Downloads/SimRobot-2/ME003-SimBot-2.ipynb?download=false 12/19
5/10/2018 ME003-SimBot-2

In [13]: omega = np.array([1.5, 2.4, 10, 3 , 7])

T = np.array([2, 3, 4, 2, 3])
end_angle = np.zeros(5)
omega_L = omega
omega_R = omega + drift(omega)
mybot = DDRobot()
mybot.mag_plot = 2 #coefficient of magnification of the arrow
mybot.step_plot = 100 # plot location every 100 iterations
mybot.pos_x = 0 #x
mybot.pos_y = 0 #y
mybot.angle = D2R(0) #alpha
mybot.length =0.4 #L
mybot.radius = 0.1 #r
for i in range(5):

mybot.rt_spd_left = omega_L[i]
mybot.rt_spd_right = omega_R[i] - fixed(omega[i]) # write your corre
ction here

mybot.plot = True #True if you want to plot the robot's trajectory

mybot.plot_robot() #draw an arrow for the location of the robot at t
=0

mybot.move(T[i]) #move the robot for 10s

mybot.plot_robot() #draw the new location
end_angle[i] = mybot.angle

plt.xlim(-5,11)
plt.ylim(-5,11)
plt.xlabel(r"$x$")
plt.ylabel(r"$y$")
plt.show()

In [14]: print(R2D(end_angle))

[ 1.20861078e-13 -8.90555311e-14 3.26961021e-12 3.00244362e-12

3.11694359e-12]

http://localhost:8888/nbconvert/html/Downloads/SimRobot-2/ME003-SimBot-2.ipynb?download=false 13/19
5/10/2018 ME003-SimBot-2

Create a complex motion (with rotations and speed variations). Show the result with and without correction. At
minimum the motion should include one rotation and two different wheel rotation speed. See example below

http://localhost:8888/nbconvert/html/Downloads/SimRobot-2/ME003-SimBot-2.ipynb?download=false 14/19
5/10/2018 ME003-SimBot-2

In [16]: Nmoves = 2 # make sure to increase this number if you increase the numbe
r of moves
omega = np.array([6, 9])
T = np.array([12,8])
rotation_angle = np.array([90, 90])
rotation_angle = D2R(rotation_angle)
end_angle = np.zeros(Nmoves)
omega_L = omega
omega_R = omega + drift(omega)
t_rotation = rotation_angle*mybot.length/(mybot.radius*(omega_R+omega_L
))
mybot = DDRobot()
mybot.mag_plot = 2 #coefficient of magnification of the arrow
mybot.step_plot = 100 # plot location every 100 iterations
mybot.pos_x = 0 #x
mybot.pos_y = 0 #y
mybot.angle = D2R(0) #alpha
mybot.length =0.4 #L
mybot.radius = 0.1 #r
mybot.plot = True #True if you want to plot the robot's trajectory
mybot.plot_robot() #draw an arrow for the location of the robot at t=0
for i in range(Nmoves):

mybot.rt_spd_left = omega_L[i]
mybot.rt_spd_right = omega_R[i] - fixed(omega[i]) # write your corre
ction here

mybot.move(T[i]) #move the robot

# rotation
mybot.rt_spd_left = -omega_L[i]
mybot.rt_spd_right = omega_R[i] - fixed(omega[i]) # write your corre
ction here
mybot.move(t_rotation[i]) #move the robot
mybot.plot_robot() #draw the new location
end_angle[i] = mybot.angle

plt.xlim(-5,11)
plt.ylim(-5,11)
plt.xlabel(r"$x$")
plt.ylabel(r"$y$")
plt.show()

http://localhost:8888/nbconvert/html/Downloads/SimRobot-2/ME003-SimBot-2.ipynb?download=false 15/19
5/10/2018 ME003-SimBot-2

http://localhost:8888/nbconvert/html/Downloads/SimRobot-2/ME003-SimBot-2.ipynb?download=false 16/19
5/10/2018 ME003-SimBot-2

In [23]: omega = np.array([1, 2.5, 5, 7.5, 10])

end_angle = np.zeros(5)
omega_L = omega
omega_R = omega + drift(omega) # DO NOT MODIFY
mybot = DDRobot()
mybot.mag_plot = 2 #coefficient of magnification of the arrow
mybot.step_plot = 100 # plot location every 100 iterations

for i in range(5):
mybot.pos_x = 0 #x
mybot.pos_y = 0 #y
mybot.angle = D2R(0) #alpha
mybot.length =0.4 #L
mybot.radius = 0.1 #r
mybot.rt_spd_left = omega_L[i]
mybot.rt_spd_right = 2*omega_R[i] #write your correction here

mybot.plot = True #True if you want to plot the robot's trajectory

mybot.plot_robot() #draw an arrow for the location of the robot at t
=0

mybot.move(10) #move the robot for 10s

mybot.plot_robot() #draw the new location
end_angle[i] = mybot.angle

plt.xlim(-5,5)
plt.ylim(-5,5)
plt.xlabel(r"$x$")
plt.ylabel(r"$y$")
plt.show()

http://localhost:8888/nbconvert/html/Downloads/SimRobot-2/ME003-SimBot-2.ipynb?download=false 17/19
5/10/2018 ME003-SimBot-2

In [28]: Nmoves = 2 # make sure to increase this number if you increase the numbe
r of moves
omega = np.array([6, 9])
T = np.array([12,8])
rotation_angle = np.array([90, 90])
rotation_angle = D2R(rotation_angle)
end_angle = np.zeros(Nmoves)
omega_L = omega
omega_R = omega + drift(omega)
t_rotation = rotation_angle*mybot.length/(mybot.radius*(omega_R+omega_L
))
mybot = DDRobot()
mybot.mag_plot = 2 #coefficient of magnification of the arrow
mybot.step_plot = 100 # plot location every 100 iterations
mybot.pos_x = 0 #x
mybot.pos_y = 0 #y
mybot.angle = D2R(0) #alpha
mybot.length =0.4 #L
mybot.radius = 0.1 #r
mybot.plot = True #True if you want to plot the robot's trajectory
mybot.plot_robot() #draw an arrow for the location of the robot at t=0
for i in range(Nmoves):

mybot.rt_spd_left = omega_L[i]
mybot.rt_spd_right = omega_R[i] - fixed(omega[i]) # write your corre
ction here

mybot.move(T[i]) #move the robot

# rotation
mybot.rt_spd_left = omega_L[i]
mybot.rt_spd_right = omega_R[i] - fixed(omega[i]) # write your corre
ction here
mybot.move(t_rotation[i]) #move the robot
mybot.plot_robot() #draw the new location
end_angle[i] = mybot.angle

plt.xlim(-5,11)
plt.ylim(-5,11)
plt.xlabel(r"$x$")
plt.ylabel(r"$y$")
plt.show()

http://localhost:8888/nbconvert/html/Downloads/SimRobot-2/ME003-SimBot-2.ipynb?download=false 18/19
5/10/2018 ME003-SimBot-2

http://localhost:8888/nbconvert/html/Downloads/SimRobot-2/ME003-SimBot-2.ipynb?download=false 19/19