What is wrong with my Implementation of 4th Order runge kutta in python for nonholonomic constraints?

Question

I am trying to implement 4th order Runge Kutta for nonholonomic motion for car-like robots. I don't know what I am doing wrong,essentially I am passing +-Pi/4 to calculate hard left and right turns to get different trajectories. But no matter if I pass +pi/4 or -pi/4 to it, I get the same answer. I cannot figure out what I am doing wrong. The constraint equations that I am using are:

thetadot = (s/L)*tan(phi)
xdot = s*cos(theta)
ydot = s*sin(theta)

Where s is the speed and L is the length of the car like robot.

#! /usr/bin/env python
import sys, random, math, pygame
from pygame.locals import *
from math import sqrt,cos,sin,atan2,tan

import numpy as np
import matplotlib.pyplot as plt

XDIM = 640
YDIM = 480
WINSIZE = [XDIM, YDIM]
PHI = 45
s = 0.5
white = 255, 240, 200
black = 20, 20, 40
red = 255, 0, 0
green = 0, 255, 0
blue = 0, 0, 255
cyan = 0,255,255

pygame.init()
screen = pygame.display.set_mode(WINSIZE)


X = XDIM/2
Y = YDIM/2
THETA = 45
def main():
    nodes = []
    nodes.append(Node(XDIM/2.0,YDIM/2.0,0.0))
    plt.plot(runge_kutta(nodes[0], (3.14/4)))  #Hard Left turn
    plt.plot(runge_kutta(nodes[0], 0))         #Straight ahead
    plt.plot(runge_kutta(nodes[0], -(3.14/4))) #Hard Right turn
    plt.show()

class Node:
    x = 0
    y = 0
    theta = 0
    distance=0  
    parent=None
    def __init__(self,xcoord, ycoord, theta):
         self.x = xcoord
         self.y = ycoord
         self.theta = theta

def rk4(f, x, y, n):
    x0 = y0 =  0
    vx = [0]*(n + 1)
    vy = [0]*(n + 1)
    h = 0.8
    vx[0] = x = x0
    vy[0] = y = y0
    for i in range(1, n + 1):
        k1 = h*f(x, y)
        k2 = h*f(x + 0.5*h, y + 0.5*k1)
        k3 = h*f(x + 0.5*h, y + 0.5*k2)
        k4 = h*f(x + h, y + k3)
        vx[i] = x = x0 + i*h
        vy[i] = y = y + (k1 + k2 + k2 + k3 + k3 + k4)/6
    print "1"
    print vy
    return vy


def fun1(x,y):
    x = (0.5/2)*tan(y)
    print "2"
    print x
    return x

def fun2(x,y):
    x = 0.5*cos(y)
    print "3"
    print x
    return x

def fun3(x,y):
    x = 0.5*sin(y)
    print "4"
    print x
    return x

def runge_kutta(p, phi):
    x1 = p.x
    y1 = p.y
    theta1 = p.theta
    fi = phi
    for i in range(0,5):
        x2 = rk4(fun2, x1, theta1, 5)
        y2 = rk4(fun3, y1, theta1, 5)
        theta2 = rk4(fun1, theta1 ,fi, 5)
        theta1 = theta2
    print "5"
    print zip(x2,y2)
    return zip(x2,y2)





# if python says run, then we should run
if __name__ == '__main__':
    main()
    running = True
    while running:
       for event in pygame.event.get():
        if event.type == pygame.QUIT:
               running = False

Answer 1

I can't really say much about the algorithm, but the way you set up our rk4 function, the x, and y arguments will never have any effect:

def rk4(f, x, y, n):
    x0 = y0 =  0          # x0 and y0 will both be 0 after this
    vx = [0]*(n + 1)
    vy = [0]*(n + 1)
    h = 0.8
    vx[0] = x = x0        # now x will be 0
    vy[0] = y = y0        # and y will be 0 too
    ...

The rest of the function will use x=0 and y=0 in any case.

Also, I don't know if that's intentional, but the other functions fun1, fun2 and fun3 don't ever use the parameter passed as x, they only use y. They change x locally, but that won't reflect outside the function.