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.resetDebugVisualizerCamera(cameraDistance=1.0, cameraYaw=-90, cameraPitch=-45, cameraTargetPosition=[-0.1,-0.0,0.65])
|
||||
|
||||
p.resetSimulation()
|
||||
|
||||
|
@ -113,12 +114,19 @@ def run_sim():
|
|||
y_history=[]
|
||||
|
||||
time.sleep(0.5)
|
||||
input("Press Enter to continue...")
|
||||
while (1):
|
||||
|
||||
state = get_state(car)
|
||||
x_history.append(state[0])
|
||||
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
|
||||
p.addUserDebugLine([state[0],state[1],0],[state[0],state[1],0.5],[1,0,0])
|
||||
|
||||
|
|
|
@ -32,7 +32,7 @@
|
|||
"\n",
|
||||
"The inputs of the model are:\n",
|
||||
"\n",
|
||||
"* $v$ linear velocity of the robot\n",
|
||||
"* $a$ acceleration of the robot\n",
|
||||
"* $\\delta$ angular velocity of the robot\n",
|
||||
"\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[1,:]=0.1\n",
|
||||
"\n",
|
||||
" \n",
|
||||
"K=road_curve(x0,track)\n",
|
||||
"\n",
|
||||
"# dynamics starting state w.r.t vehicle frame\n",
|
||||
|
@ -729,7 +728,6 @@
|
|||
" xt_plus_one = np.squeeze(np.dot(A,xt)+np.dot(B,ut)+C)\n",
|
||||
" x_bar[:,t]= xt_plus_one\n",
|
||||
"\n",
|
||||
"\n",
|
||||
"x = cp.Variable((N, T+1))\n",
|
||||
"u = cp.Variable((M, T))\n",
|
||||
"\n",
|
||||
|
|
|
@ -350,7 +350,7 @@
|
|||
},
|
||||
{
|
||||
"cell_type": "code",
|
||||
"execution_count": 43,
|
||||
"execution_count": 11,
|
||||
"metadata": {},
|
||||
"outputs": [
|
||||
{
|
||||
|
@ -409,7 +409,7 @@
|
|||
},
|
||||
{
|
||||
"cell_type": "code",
|
||||
"execution_count": 44,
|
||||
"execution_count": 12,
|
||||
"metadata": {},
|
||||
"outputs": [
|
||||
{
|
||||
|
@ -419,7 +419,7 @@
|
|||
" -0.58083993])"
|
||||
]
|
||||
},
|
||||
"execution_count": 44,
|
||||
"execution_count": 12,
|
||||
"metadata": {},
|
||||
"output_type": "execute_result"
|
||||
}
|
||||
|
@ -430,7 +430,7 @@
|
|||
},
|
||||
{
|
||||
"cell_type": "code",
|
||||
"execution_count": 40,
|
||||
"execution_count": 13,
|
||||
"metadata": {},
|
||||
"outputs": [
|
||||
{
|
||||
|
@ -475,12 +475,12 @@
|
|||
"cell_type": "markdown",
|
||||
"metadata": {},
|
||||
"source": [
|
||||
"## With BSLINES"
|
||||
"## With SPLINES"
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "code",
|
||||
"execution_count": 18,
|
||||
"execution_count": 17,
|
||||
"metadata": {},
|
||||
"outputs": [
|
||||
{
|
||||
|
@ -523,10 +523,9 @@
|
|||
"\n",
|
||||
"#fit poly\n",
|
||||
"import scipy\n",
|
||||
"from scipy.interpolate import BSpline\n",
|
||||
"from scipy.interpolate import CubicSpline\n",
|
||||
"from scipy.interpolate import PPoly,splrep\n",
|
||||
"spl=splrep(wp_vehicle_frame[0,:], wp_vehicle_frame[1,:])\n",
|
||||
"coeff\n",
|
||||
"print( spl)\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",
|
||||
|
@ -536,6 +535,30 @@
|
|||
"\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",
|
||||
"metadata": {},
|
||||
|
@ -577,7 +600,7 @@
|
|||
},
|
||||
{
|
||||
"cell_type": "code",
|
||||
"execution_count": 45,
|
||||
"execution_count": 15,
|
||||
"metadata": {},
|
||||
"outputs": [
|
||||
{
|
||||
|
|
Loading…
Reference in New Issue