added input delay in sim
parent
f3a985940e
commit
49e2a00a10
Binary file not shown.
After Width: | Height: | Size: 499 KiB |
|
@ -56,6 +56,7 @@ def run_sim():
|
||||||
"""
|
"""
|
||||||
"""
|
"""
|
||||||
p.connect(p.GUI)
|
p.connect(p.GUI)
|
||||||
|
p.resetDebugVisualizerCamera(cameraDistance=1.0, cameraYaw=-90, cameraPitch=-45, cameraTargetPosition=[-0.1,-0.0,0.65])
|
||||||
|
|
||||||
p.resetSimulation()
|
p.resetSimulation()
|
||||||
|
|
||||||
|
@ -113,12 +114,19 @@ def run_sim():
|
||||||
y_history=[]
|
y_history=[]
|
||||||
|
|
||||||
time.sleep(0.5)
|
time.sleep(0.5)
|
||||||
|
input("Press Enter to continue...")
|
||||||
while (1):
|
while (1):
|
||||||
|
|
||||||
state = get_state(car)
|
state = get_state(car)
|
||||||
x_history.append(state[0])
|
x_history.append(state[0])
|
||||||
y_history.append(state[1])
|
y_history.append(state[1])
|
||||||
|
|
||||||
|
#add 1 timestep delay to input
|
||||||
|
state[0]=state[0]+state[2]*np.cos(state[3])*P.dt
|
||||||
|
state[1]=state[1]+state[2]*np.sin(state[3])*P.dt
|
||||||
|
state[2]=state[2]+opt_u[0,0]*P.dt
|
||||||
|
state[3]=state[3]+opt_u[0,0]*np.tan(opt_u[1,0])/0.3*P.dt
|
||||||
|
|
||||||
#track path in bullet
|
#track path in bullet
|
||||||
p.addUserDebugLine([state[0],state[1],0],[state[0],state[1],0.5],[1,0,0])
|
p.addUserDebugLine([state[0],state[1],0],[state[0],state[1],0.5],[1,0,0])
|
||||||
|
|
||||||
|
|
|
@ -32,7 +32,7 @@
|
||||||
"\n",
|
"\n",
|
||||||
"The inputs of the model are:\n",
|
"The inputs of the model are:\n",
|
||||||
"\n",
|
"\n",
|
||||||
"* $v$ linear velocity of the robot\n",
|
"* $a$ acceleration of the robot\n",
|
||||||
"* $\\delta$ angular velocity of the robot\n",
|
"* $\\delta$ angular velocity of the robot\n",
|
||||||
"\n",
|
"\n",
|
||||||
"These are the differential equations f(x,u) of the model:\n",
|
"These are the differential equations f(x,u) of the model:\n",
|
||||||
|
@ -715,7 +715,6 @@
|
||||||
"u_bar[0,:]=0.5\n",
|
"u_bar[0,:]=0.5\n",
|
||||||
"u_bar[1,:]=0.1\n",
|
"u_bar[1,:]=0.1\n",
|
||||||
"\n",
|
"\n",
|
||||||
" \n",
|
|
||||||
"K=road_curve(x0,track)\n",
|
"K=road_curve(x0,track)\n",
|
||||||
"\n",
|
"\n",
|
||||||
"# dynamics starting state w.r.t vehicle frame\n",
|
"# dynamics starting state w.r.t vehicle frame\n",
|
||||||
|
@ -728,7 +727,6 @@
|
||||||
" A,B,C=get_linear_model(xt,ut)\n",
|
" A,B,C=get_linear_model(xt,ut)\n",
|
||||||
" xt_plus_one = np.squeeze(np.dot(A,xt)+np.dot(B,ut)+C)\n",
|
" xt_plus_one = np.squeeze(np.dot(A,xt)+np.dot(B,ut)+C)\n",
|
||||||
" x_bar[:,t]= xt_plus_one\n",
|
" x_bar[:,t]= xt_plus_one\n",
|
||||||
" \n",
|
|
||||||
"\n",
|
"\n",
|
||||||
"x = cp.Variable((N, T+1))\n",
|
"x = cp.Variable((N, T+1))\n",
|
||||||
"u = cp.Variable((M, T))\n",
|
"u = cp.Variable((M, T))\n",
|
||||||
|
|
|
@ -350,7 +350,7 @@
|
||||||
},
|
},
|
||||||
{
|
{
|
||||||
"cell_type": "code",
|
"cell_type": "code",
|
||||||
"execution_count": 43,
|
"execution_count": 11,
|
||||||
"metadata": {},
|
"metadata": {},
|
||||||
"outputs": [
|
"outputs": [
|
||||||
{
|
{
|
||||||
|
@ -409,7 +409,7 @@
|
||||||
},
|
},
|
||||||
{
|
{
|
||||||
"cell_type": "code",
|
"cell_type": "code",
|
||||||
"execution_count": 44,
|
"execution_count": 12,
|
||||||
"metadata": {},
|
"metadata": {},
|
||||||
"outputs": [
|
"outputs": [
|
||||||
{
|
{
|
||||||
|
@ -419,7 +419,7 @@
|
||||||
" -0.58083993])"
|
" -0.58083993])"
|
||||||
]
|
]
|
||||||
},
|
},
|
||||||
"execution_count": 44,
|
"execution_count": 12,
|
||||||
"metadata": {},
|
"metadata": {},
|
||||||
"output_type": "execute_result"
|
"output_type": "execute_result"
|
||||||
}
|
}
|
||||||
|
@ -430,7 +430,7 @@
|
||||||
},
|
},
|
||||||
{
|
{
|
||||||
"cell_type": "code",
|
"cell_type": "code",
|
||||||
"execution_count": 40,
|
"execution_count": 13,
|
||||||
"metadata": {},
|
"metadata": {},
|
||||||
"outputs": [
|
"outputs": [
|
||||||
{
|
{
|
||||||
|
@ -475,12 +475,12 @@
|
||||||
"cell_type": "markdown",
|
"cell_type": "markdown",
|
||||||
"metadata": {},
|
"metadata": {},
|
||||||
"source": [
|
"source": [
|
||||||
"## With BSLINES"
|
"## With SPLINES"
|
||||||
]
|
]
|
||||||
},
|
},
|
||||||
{
|
{
|
||||||
"cell_type": "code",
|
"cell_type": "code",
|
||||||
"execution_count": 18,
|
"execution_count": 17,
|
||||||
"metadata": {},
|
"metadata": {},
|
||||||
"outputs": [
|
"outputs": [
|
||||||
{
|
{
|
||||||
|
@ -523,10 +523,9 @@
|
||||||
"\n",
|
"\n",
|
||||||
"#fit poly\n",
|
"#fit poly\n",
|
||||||
"import scipy\n",
|
"import scipy\n",
|
||||||
"from scipy.interpolate import BSpline\n",
|
"from scipy.interpolate import CubicSpline\n",
|
||||||
"from scipy.interpolate import PPoly,splrep\n",
|
"from scipy.interpolate import PPoly,splrep\n",
|
||||||
"spl=splrep(wp_vehicle_frame[0,:], wp_vehicle_frame[1,:])\n",
|
"spl=splrep(wp_vehicle_frame[0,:], wp_vehicle_frame[1,:])\n",
|
||||||
"coeff\n",
|
|
||||||
"print( spl)\n",
|
"print( spl)\n",
|
||||||
"print(PPoly.from_spline(spl).c)\n",
|
"print(PPoly.from_spline(spl).c)\n",
|
||||||
"#coeff=np.polyfit(wp_vehicle_frame[0,:], wp_vehicle_frame[1,:], 5, rcond=None, full=False, w=None, cov=False)\n",
|
"#coeff=np.polyfit(wp_vehicle_frame[0,:], wp_vehicle_frame[1,:], 5, rcond=None, full=False, w=None, cov=False)\n",
|
||||||
|
@ -536,6 +535,30 @@
|
||||||
"\n"
|
"\n"
|
||||||
]
|
]
|
||||||
},
|
},
|
||||||
|
{
|
||||||
|
"cell_type": "code",
|
||||||
|
"execution_count": null,
|
||||||
|
"metadata": {},
|
||||||
|
"outputs": [],
|
||||||
|
"source": [
|
||||||
|
"def spline_planning(qs, qf, ts, tf, dqs=0.0, dqf=0.0, ddqs=0.0, ddqf=0.0):\n",
|
||||||
|
" \n",
|
||||||
|
" bc = np.array([ys, dys, ddys, yf, dyf, ddyf]).T \n",
|
||||||
|
" \n",
|
||||||
|
" C = np.array([[1, xs, xs**2, xs**3, xs**4, xs**5], #f(xs)=ys\n",
|
||||||
|
" [0, 1, 2*xs**1, 3*xs**2, 4*xs**3, 5**xs^4], #df(xs)=dys\n",
|
||||||
|
" [0, 0, 1, 6*xs**1, 12*xs**2, 20**xs^3], #ddf(xs)=ddys\n",
|
||||||
|
" [1, xf, xf**2, xf**3, xf**4, xf**5], #f(xf)=yf\n",
|
||||||
|
" [0, 1, 2*xf**1, 3*xf**2, 4*xf**3, 5**xf^4], #df(xf)=dyf\n",
|
||||||
|
" [0, 0, 1, 6*xf**1, 12*xf**2, 20**xf^3]]) #ddf(xf)=ddyf\n",
|
||||||
|
" \n",
|
||||||
|
" #To compute the polynomial coefficients we solve:\n",
|
||||||
|
" #Ax = B. \n",
|
||||||
|
" #Matrices A and B must have the same number of rows\n",
|
||||||
|
" a = np.linalg.lstsq(C,bc)[0]\n",
|
||||||
|
" return a"
|
||||||
|
]
|
||||||
|
},
|
||||||
{
|
{
|
||||||
"cell_type": "markdown",
|
"cell_type": "markdown",
|
||||||
"metadata": {},
|
"metadata": {},
|
||||||
|
@ -577,7 +600,7 @@
|
||||||
},
|
},
|
||||||
{
|
{
|
||||||
"cell_type": "code",
|
"cell_type": "code",
|
||||||
"execution_count": 45,
|
"execution_count": 15,
|
||||||
"metadata": {},
|
"metadata": {},
|
||||||
"outputs": [
|
"outputs": [
|
||||||
{
|
{
|
||||||
|
|
Loading…
Reference in New Issue