Professional Documents
Culture Documents
ME003 SimBot 2
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
import time
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>")
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
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()
# 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.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()
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
Objective
The objective is to investigate methods of calibration of the robot.
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
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
ωR + ωL
ẋ = r cos α
2
ωR + ωL
ẏ = r sin α
2
and
r
α̇ = (ω R − ω L )
L
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
[ 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]
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 [9]: p2 = np.poly1d(p2_coef)
p2 = np.poly1d(np.polyfit(omega,end_angle,2)) #equivalent compact form
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.
http://localhost:8888/nbconvert/html/Downloads/SimRobot-2/ME003-SimBot-2.ipynb?download=false 11/19
5/10/2018 ME003-SimBot-2
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
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
mybot.rt_spd_left = omega_L[i]
mybot.rt_spd_right = omega_R[i] - fixed(omega[i]) # write your corre
ction here
plt.xlim(-5,11)
plt.ylim(-5,11)
plt.xlabel(r"$x$")
plt.ylabel(r"$y$")
plt.show()
In [14]: print(R2D(end_angle))
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
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
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
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
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