非ホロノミックな制約のためにPythonで4th runge kuttaを実装したのはどうですか？

Question

私は車のようなロボットのための非ホロノミックな動きのための4次のRunge Kuttaを実装しようとしています。 私は何が間違っているのかわからない、本質的に私は+ -Pi/4を通過して、異なる軌道を得るために左と右のターンを計算する。 しかし、+ pi/4または-pi/4を渡しても、私は同じ答えを得ます。 私が間違っていることを理解できません。私が使用しています 制約式は、次のとおりです。■は速度であり、Lは、ロボットのような車の長さである

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

。

#! /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

私は本当にアルゴリズムについて多くを語ることはできませんが、私たちのrk4機能、x、およびy引数を設定する方法は何の影響も持っていることは決してありません：

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 
    ... 

の残りの部分をいずれの場合も、関数はx=0とy=0を使用します。これまでに、彼らは唯一のyを使用し、xとして渡されたパラメータを使用しないことが意図的かどう

また、私は知らないが、他の機能fun1、fun2とfun3。彼らはxをローカルに変更しますが、それはその機能の外には反映されません。