robot simulator

robot_sim.py

from math import sin, cos, pi, sqrt, atan2
from time import sleep
from random import uniform
import sys

# coords and transform are three-tuples of x, y, phi
# coords specifies the coordinates to be transformed
# transform specifies the transformation properties to be applied (what translation/rotation)
def transform_robot_to_sim(coords, transform):
    coords_x = coords[0]
    coords_y = coords[1]
    coords_phi = coords[2]
    transform_x = transform[0]
    transform_y = transform[1]
    transform_phi = transform[2]
    result_x = (cos(transform_phi)*coords_x
               +sin(transform_phi)*coords_y
               +cos(transform_phi)*(-transform_x)
               +sin(transform_phi)*(-transform_y))
    result_y = (-sin(transform_phi)*coords_x
               +cos(transform_phi)*coords_y
               +sin(transform_phi)*(transform_x)
               +cos(transform_phi)*(-transform_y));
    result_phi = coords_phi - transform_phi
    result_phi = normalize_angle(result_phi)
    return (result_x, result_y, result_phi)

# normalizing the angle - shouldnt be necessary but also shouldnt change anything
def normalize_angle(alpha):
    if alpha >  pi: alpha -= 2.0 * pi
    if alpha < -pi: alpha += 2.0 * pi
    return alpha

class Robot:
    def __init__(self, wheel_distance=0.5):
        self.pos = (0,0,0)
        self.wheel_distance = wheel_distance

    def drive(self, u, omega, timedelta):
        """
        drive robot with certain speed on left and right speed and phi as its
        global orientation (for some (?) transformation)
        """
        # changed
        leftspeed  = u - omega/2.0*self.wheel_distance
        rightspeed = u
        radius = self.wheel_distance*0.5*(leftspeed+rightspeed)/(rightspeed-leftspeed);
        alpha = timedelta*(rightspeed-leftspeed)/self.wheel_distance;
        # changed here
        dx = radius*sin(alpha);
        dy = radius - radius*cos(alpha);
        # and here
        x, y, phi = self.pos
        x += dx*cos(-phi)+dy*sin(-phi);
        y += -dx*sin(-phi)+dy*cos(-phi);
        phi += alpha
        phi = normalize_angle(phi)
        self.pos = (x, y, phi)
        pass

    def get_odom(self):
        return self.pos

class GnuPlot:
    def __init__(self, x_range=(-10.0, 10.0), y_range=(-10.0, 10.0), title=None):
        self.data = []
        self.title = title
        print "set xrange [%f:%f]"%x_range
        print "set yrange [%f:%f]"%y_range
        print "set term x11"
        print "set grid"
        print "set size square"
        print "set style data vector"
        print "# x y"

    def plot(self, pos):
        if self.title:
            print "plot \"-\" using 1:2:3:4 title \"%s\""%self.title
        else:
            print "plot \"-\" using 1:2:3:4 notitle"
        self.data.append(pos)
        for d in self.data:
            x = d[0]
            y = d[1]
            dx = cos(d[2])*0.6
            dy = sin(d[2])*0.6
            print "%f %f %f %f"%(x, y, dx, dy)
        print "e"

class Simulator:
    def __init__(self, model, goal_pos, max_sim_iter=1000, timedelta=0.1, speedup=-1, printevery=-1):
        self.max_iter = max_sim_iter;
        self.timedelta = timedelta
        self.speedup = speedup
        self.printevery = printevery
        self.goal_pos = goal_pos # goal position and orientation relative to the robot
        self.robot_pos = transform_robot_to_sim((0,0,0), goal_pos) # position of robot in goal frame
        self.robot = Robot(self.timedelta)
        self.model = model
        max_plot = max(15, abs(1.5*max(goal_pos[0], goal_pos[1])))
        self.plot = GnuPlot((-max_plot, max_plot), (-max_plot, max_plot),
                            "goal position in robot frame: x=%f, y=%f, phi=%f"%self.goal_pos)

    def run(self):
        for i in xrange(self.max_iter):
            # convert the robot pos that odometry returns into goal frame
            self.robot_pos = transform_robot_to_sim(self.robot.get_odom(), self.goal_pos)

            # according to the chosen model and giving the robot position in
            # the goal frame, calculate velocity and angle to take
            u, omega = self.model.calc_next_velocity(self.robot_pos)

            self.robot.drive(u, omega, self.timedelta)

            if self.printevery == -1 or i%self.printevery == 0:
                #self.plot.plot(self.robot.get_odom())
                self.plot.plot(self.robot_pos)

            # success if below goal threshold
            if abs(self.robot_pos[0]) < 0.1 and abs(self.robot_pos[1]) < 0.1:
                break

            # abort if robot drives completely off
            if abs(self.robot_pos[0]) > 40 or abs(self.robot_pos[1]) > 40:
                sys.stderr.write("drove off %s\n"%str(self.goal_pos))
                exit(1)

            if self.speedup != -1:
                sleep(self.timedelta/self.speedup)

        if i == self.max_iter-1:
            sys.stderr.write("reached max iter: %s\n"%str(self.goal_pos))
            exit(1)

# the model classes contain the calc_next_velocities function takes the
# robot pos in global goal coordinates and returns speed and angle

class ModelCircle():
    def __init__(self):
        pass
    def calc_next_velocity(self, pos):
        return (1.0, -0.15)

class ModelGiovanni():
    def __init__(self):
        pass
    def calc_next_velocity(self, pos):
        x, y, phi = pos

        #h > 1; 2 < beta < h + 1 according to lyaponv criteria
        # has to be choosen fron eq 17
        gamma = 1.0
        beta = 2.9
        h = 2.0

        epsilon = 0.00001;

        e = sqrt(x**2 + y**2) # distance to goal

        if e > epsilon:
            theta = atan2(-y,-x)    # orientation to goal
            theta = normalize_angle(theta)

            alpha = theta - phi     # diff steering angle and orientation
            alpha = normalize_angle(alpha)

            u = gamma * e           # compute the velocity

            if abs(alpha) > epsilon:
                c = (sin(alpha) + (h * theta * sin(alpha) / alpha + beta * alpha)) / e; 
            else:
                c = (alpha + alpha * beta + h * theta) / e;

            omega = c * u           # compute the turning velocity
        else:
            u = 0.0;
            omega = 0.0;

        return (u, omega)

if __name__ == "__main__":
    if len(sys.argv) <= 1:
        i = 0
        while (i < 1000):
            x, y, a = (uniform(-10, 10), uniform(-10, 10), uniform(-3.2, 3.2))
            if abs(x) < 2.0 or abs(y) < 2.0:
                continue
            sim = Simulator(ModelGiovanni(), goal_pos=(x, y, a))
            sim.run()
            i += 1
    elif len(sys.argv) == 4:
        pos = [float(a) for a in sys.argv[1:4]]
        sim = Simulator(ModelGiovanni(), tuple(pos))
        sim.run()
    elif len(sys.argv) == 8:
        pos = [float(a) for a in sys.argv[1:4]]
        maxiter = int(sys.argv[4])
        timedelta = float(sys.argv[5])
        speedup = float(sys.argv[6])
        printevery = int(sys.argv[7])
        sim = Simulator(ModelGiovanni(), tuple(pos), maxiter, timedelta, speedup, printevery)
        sim.run()
    else:
        print "usage:"
        print "\tno arguments: run tests"
        print "\t3 arguments: %s x y phi"%sys.argv[0]
        print "\t7 arguments: %s x y phi maxiter timedelta speedup printevery"%sys.argv[0]
        print ""
        print "example (using defaults):"
        print "\t%s 10 10 0.0 1000 0.1 -1 -1"%sys.argv[0]
        print ""
        print " - speedup scales the sleep() duration each loop cycle to something"
        print "   different than timedelta. -1 indicates no sleep(). greater values"
        print "   are faster"
        print " - printevery controls the plotting frequency. -1 indicates to plot"
        print "   at every loop cycle"

robot_sim_follow.py

from math import sin, cos, pi, sqrt, atan2
from time import sleep
from random import uniform
import sys

# normalizing the angle - shouldnt be necessary but also shouldnt change anything
def normalize_angle(alpha):
    if alpha >  pi: alpha -= 2.0 * pi
    if alpha < -pi: alpha += 2.0 * pi
    return alpha

class Robot:
    def __init__(self, wheel_distance=0.5):
        self.u_max = 1.0;
        self.pos = (0,0,0)
        self.wheel_distance = wheel_distance

    def drive(self, u, omega, timedelta):
        """
        drive robot with certain speed on left and right speed and phi as its
        global orientation (for some (?) transformation)
        """
        if u > self.u_max:
            u = self.u_max

        # changed
        leftspeed  = u - omega/2.0*self.wheel_distance
        rightspeed = u
        if (leftspeed == 0 and rightspeed == 0) or leftspeed == rightspeed:
            radius = 0.0
            alpha = 0.0
        else:
            radius = self.wheel_distance*0.5*(leftspeed+rightspeed)/(rightspeed-leftspeed);
            alpha = timedelta*(rightspeed-leftspeed)/self.wheel_distance;
        # changed here
        dx = radius*sin(alpha);
        dy = radius - radius*cos(alpha);
        # and here
        x, y, phi = self.pos
        x += dx*cos(-phi)+dy*sin(-phi);
        y += -dx*sin(-phi)+dy*cos(-phi);
        phi += alpha
        phi = normalize_angle(phi)
        self.pos = (x, y, phi)
        pass

    def get_odom(self):
        return self.pos

class GnuPlot:
    def __init__(self, x_range=(-10.0, 10.0), y_range=(-10.0, 10.0), title=None):
        self.data = []
        self.title = title
        print "set xrange [%f:%f]"%x_range
        print "set yrange [%f:%f]"%y_range
        print "set term x11"
        print "set grid"
        print "set size square"
        print "set style data vector"
        print "# x y"

    def plot(self, pos):
        if self.title:
            print "plot \"-\" using 1:2:3:4 title \"%s\""%self.title
        else:
            print "plot \"-\" using 1:2:3:4 notitle"
        self.data.append(pos)
        for d in self.data:
            x = d[0]
            y = d[1]
            dx = cos(d[2])*0.06
            dy = sin(d[2])*0.06
            print "%f %f %f %f"%(x, y, dx, dy)
        print "e"

class Simulator:
    def __init__(self, max_sim_iter=10, timedelta=0.1, speedup=-1, printevery=-1):
        self.max_iter = max_sim_iter;
        self.timedelta = timedelta
        self.speedup = speedup
        self.printevery = printevery
        self.robot = Robot(self.timedelta)
        self.model = ModelFollow(self.robot.wheel_distance)
        self.curve = Curve([(0,0), (1,1), (2,2), (3,3), (4,4), (5,5)])
        max_plot = 0.1
        self.plot = GnuPlot((-max_plot, max_plot), (-max_plot, max_plot), "goal position in robot frame")

    def run(self):
        u = 0.0
        for i in xrange(self.max_iter):
            # according to the chosen model and giving the robot position in
            # the goal frame, calculate velocity and angle to take
            u, omega = self.model.calc_next_velocity(self.robot.get_odom(), u, self.curve)

            sys.stderr.write("%f %f\n"%(u, omega))

            self.robot.drive(u, omega, self.timedelta)

            if self.printevery == -1 or i%self.printevery == 0:
                self.plot.plot(self.robot.get_odom())

            # abort if robot drives completely off
            if abs(self.robot.get_odom()[0]) > 40 or abs(self.robot.get_odom()[1]) > 40:
                sys.stderr.write("drove off\n")
                exit(1)

            if self.speedup != -1:
                sleep(self.timedelta/self.speedup)

        if i == self.max_iter-1:
            sys.stderr.write("reached max iter\n")
            exit(1)

class Curve():
    def __init__(self, path):
        self.path = path
        self.cur = 0
        self.A = self.path[self.cur+1][1]-self.path[self.cur][1];
        self.B = self.path[self.cur][0]-self.path[self.cur+1][0];
        self.C = -self.A*self.path[self.cur][0]-self.B*self.path[self.cur][1];

    def getNext(self):
        if cur >= len(self.path)-1:
            return False

        self.cur += 1
        self.A = self.path[self.cur+1][1]-self.path[self.cur][1];
        self.B = self.path[self.cur][0]-self.path[self.cur+1][0];
        self.C = -self.A*self.path[self.cur][0]-self.B*self.path[self.cur][1];
        return True

    def evaluate(self, x, y):
        return self.A*x+self.B*y+self.C

    def getAng(self):
        return atan2(self.path[self.cur+1][1]-self.path[self.cur][1],
                     self.path[self.cur+1][0]-self.path[self.cur][0])

    def pointInn(self, x, y):
        a1 = self.path[self.cur+1][0]-self.path[self.cur][0]
        b1 = self.path[self.cur+1][1]-self.path[self.cur][1]
        c1 = -a1*self.path[self.cur][0] - b1*self.path[self.cur][1]
        c2 = -a1*self.path[self.cur+1][0]-b1*self.path[self.cur+1][1]
        c3 = -a1*x-b1*y;

        if c2>=c1:
            if c3>=c1:
                return 1
            else:
                return 0
        else:
            if c2<=c1:
                if c3>=c2:
                    return 1
                else:
                    return 0
            else:
                return 0

    def getDistance(self, x, y):
        return abs(self.A*x+self.B*y+self.C)/sqrt(self.A**2+self.B**2)

    def getDistanceToEnd(self, x, y):
        t1 = self.getDistance(x,y);
        t2 = ((self.path[self.cur+1][0]-x)*(self.path[self.cur+1][0]-x)
             +(self.path[self.cur+1][0]-y)*(self.path[self.cur+1][0]-y))
        return sqrt(t2-t1**2);

class ModelFollow():
    def __init__(self, axis_length):
        self.d_phic = 0.0034
        self.alpha = 1.2
        self.kr_max = 2/axis_length
        self.d_y = 0.001

    def calc_next_velocity(self, pos, u, curve):
        loop_exit = 1
        exit = 0
        x, y, phi = pos

        while loop_exit != 0 and not exit:
            if curve.pointInn(x, y) and curve.getDistanceToEnd(x, y) > 0.2:
                exit = 1
                continue
            loop_exit = curve.getNext()

        if not exit:
            return (0, 0)

        l = curve.getDistance(x, y)
        if curve.evaluate(x, y) > 0.00005:
            l = -l

        phic = pi/2 - phi - curve.getAng()
        phic = normalize_angle(phic)

        def H_case_1(y, phic, u):
            tmp = phic*phic*(1+2*self.alpha)*(1+2*self.alpha)
            res = (self.kr_max**2)/tmp
            return res, 2*self.alpha*u*sqrt(res)

        def H_case_2(y, phic, u):
            tmp = -(phic*phic)/(y*y);
            tmp2 = (4*self.kr_max**2)/(y*y);
            tmp2 /= (1+2*self.alpha)*(1+2*self.alpha);
            tmp2 += tmp*tmp;
            res = 0.5*(tmp+sqrt(tmp2))
            return res, 2*self.alpha*u*sqrt(res)

        if y==0.0:
            if abs(phic) >= self.d_phic:
                h, gamma = H_case_1(y, phic, u);
            else:
                h, gamma = H_case_1(y, self.d_phic, u);
        else:
            if abs(y) >= self.d_y:
                h, gamma = H_case_2(y, self.d_phic, u);
            else:
                if abs(phic) < self.d_phic:
                    h, gamma = H_case_2(self.d_y, self.d_phic, u);
                else:
                    h, gamma = H_case_2(self.d_y, phic, u);

        if abs(phic) >= self.d_phic:
            t1 = sin(phic)/phic;
            t2 = h*u*y*t1;
            res = -t2-gamma*phic;
        else:
            if abs(phic) <= self.d_phic and abs(phic) >= self.d_phic_0:
               res = -h*u*y-gamma*phic;
            else:
               res = -h*u*y;

        w = res

        return (10.0, w)

if __name__ == "__main__":
    sim = Simulator(speedup=0.01, timedelta=0.01)
    sim.run()