I was trying defining a trajectory using velocity and curvature by time; i.e. v(t)
and k(t)
.
To get x and y position at time t, I used scipy.integrate.quad
but it gives a wrong value.
Below is what I've tried.
import numpy as np
from scipy.integrate import quad
# Define v(t) and k(t)
def v(t):
return 10.0
def k(t):
return 0.0
def dtheta(t): # heading rate.
return v(t) * k(t);
# Compute theta(t) by integrating dtheta
def theta(t):
result, _ = quad(dtheta, 0, t, epsabs=1e-4, limit=100) # initial heading 0
print(f"heading {result}")
return result
# return 0
# Define the integrands for x(t) and y(t)
def dx(t):
ret = v(t) * np.cos(theta(t))
print(f"dx {ret}")
return ret
def dy(t):
return v(t) * np.sin(theta(t))
# Integrate to find x(t) and y(t)
def x(t):
result, _ = quad(dx, 0, t, epsabs=1e-4, limit=100) # initial x 0
return result
def y(t):
result, _ = quad(dy, 0, t, epsabs=1e-4, limit=100) # initial y 0
return result
print(x(0.5))
As you can see, to test the code, I just put really simple values; constant 10 to velocity and constant 0 to curvature. Therefore, the heading should be constantly 0 and the expected x(0.5)
is 5.0
. However, it gives a strange number 4.235577740302726e-06
. I checked that theta(t)
always outputs 0
and dx(t)
always outputs 10.0
,
heading 0.0
dx 10.0
heading 0.0
dx 10.0
heading 0.0
dx 10.0
heading 0.0
dx 10.0
heading 0.0
dx 10.0
heading 0.0
dx 10.0
heading 0.0
dx 10.0
heading 0.0
dx 10.0
heading 0.0
dx 10.0
heading 0.0
dx 10.0
heading 0.0
dx 10.0
heading 0.0
dx 10.0
heading 0.0
dx 10.0
heading 0.0
dx 10.0
heading 0.0
dx 10.0
4.235577740302726e-06
It gives me a correct value (i.e. 5.0
) if I manually return 0
for theta(t)
but I don't like to do this cause it harms its applicability. What am I doing wrong here?
You can use solve_ivp()
, which is designed for solving initial value problems for ODEs:
import numpy as np
from scipy.integrate import solve_ivp
def system(t, y, v, k):
x, y_pos, theta = y
dxdt = v(t) * np.cos(theta)
dydt = v(t) * np.sin(theta)
dthetadt = v(t) * k(t)
return [dxdt, dydt, dthetadt]
def v(t):
return 10.0 * np.exp(-t)
def k(t):
return 0.1 * np.sin(2 * np.pi * t)
ts, te = (0, 0.5), np.linspace(0, 0.5, 100)
IVP = solve_ivp(lambda t, y: system(t, y, v, k), ts, [0, 0, 0], t_eval=te)
x1, y1 = IVP.y[0, -1], IVP.y[1, -1]
print(f"x(0.5) = {x1}, y(0.5) = {y1}")
x(0.5) = 3.89226601538334, y(0.5) = 0.4613683813905819