455 lines
115 KiB
Plaintext
455 lines
115 KiB
Plaintext
|
{
|
||
|
|
"cells": [
|
||
|
|
{
|
||
|
|
"cell_type": "markdown",
|
||
|
|
"metadata": {},
|
||
|
|
"source": [
|
||
|
|
"# Iterative Linearization"
|
||
|
|
]
|
||
|
|
},
|
||
|
|
{
|
||
|
|
"cell_type": "markdown",
|
||
|
|
"metadata": {},
|
||
|
|
"source": [
|
||
|
|
"The goal is to have a more accurate linearization of the diff equations. For every time step the optimization is iterativelly repeated using he previous optimization results **u_bar** to approximate the vehicle dynamics, instead of a random starting guess and/or the rsult at time t-1.\n",
|
||
|
|
"\n",
|
||
|
|
"In previous case the results at t-1 wer used to approimate the dynamics art time t!\n",
|
||
|
|
"\n",
|
||
|
|
"This maks the results less correlated but makes the controller slower!"
|
||
|
|
]
|
||
|
|
},
|
||
|
|
{
|
||
|
|
"cell_type": "code",
|
||
|
|
"execution_count": 1,
|
||
|
|
"metadata": {},
|
||
|
|
"outputs": [],
|
||
|
|
"source": [
|
||
|
|
"import numpy as np\n",
|
||
|
|
"from scipy.integrate import odeint\n",
|
||
|
|
"from scipy.interpolate import interp1d\n",
|
||
|
|
"import cvxpy as cp\n",
|
||
|
|
"\n",
|
||
|
|
"import matplotlib.pyplot as plt\n",
|
||
|
|
"plt.style.use(\"ggplot\")\n",
|
||
|
|
"\n",
|
||
|
|
"import time"
|
||
|
|
]
|
||
|
|
},
|
||
|
|
{
|
||
|
|
"cell_type": "code",
|
||
|
|
"execution_count": 2,
|
||
|
|
"metadata": {},
|
||
|
|
"outputs": [],
|
||
|
|
"source": [
|
||
|
|
"\"\"\"\n",
|
||
|
|
"Control problem statement.\n",
|
||
|
|
"\"\"\"\n",
|
||
|
|
"\n",
|
||
|
|
"N = 4 #number of state variables\n",
|
||
|
|
"M = 2 #number of control variables\n",
|
||
|
|
"T = 20 #Prediction Horizon\n",
|
||
|
|
"DT = 0.2 #discretization step\n",
|
||
|
|
"\n",
|
||
|
|
"def get_linear_model(x_bar,u_bar):\n",
|
||
|
|
" \"\"\"\n",
|
||
|
|
" Computes the LTI approximated state space model x' = Ax + Bu + C\n",
|
||
|
|
" \"\"\"\n",
|
||
|
|
" \n",
|
||
|
|
" L=0.3 #vehicle wheelbase\n",
|
||
|
|
" \n",
|
||
|
|
" x = x_bar[0]\n",
|
||
|
|
" y = x_bar[1]\n",
|
||
|
|
" v = x_bar[2]\n",
|
||
|
|
" theta = x_bar[3]\n",
|
||
|
|
" \n",
|
||
|
|
" a = u_bar[0]\n",
|
||
|
|
" delta = u_bar[1]\n",
|
||
|
|
" \n",
|
||
|
|
" A = np.zeros((N,N))\n",
|
||
|
|
" A[0,2]=np.cos(theta)\n",
|
||
|
|
" A[0,3]=-v*np.sin(theta)\n",
|
||
|
|
" A[1,2]=np.sin(theta)\n",
|
||
|
|
" A[1,3]=v*np.cos(theta)\n",
|
||
|
|
" A[3,2]=v*np.tan(delta)/L\n",
|
||
|
|
" A_lin=np.eye(N)+DT*A\n",
|
||
|
|
" \n",
|
||
|
|
" B = np.zeros((N,M))\n",
|
||
|
|
" B[2,0]=1\n",
|
||
|
|
" B[3,1]=v/(L*np.cos(delta)**2)\n",
|
||
|
|
" B_lin=DT*B\n",
|
||
|
|
" \n",
|
||
|
|
" f_xu=np.array([v*np.cos(theta), v*np.sin(theta), a,v*np.tan(delta)/L]).reshape(N,1)\n",
|
||
|
|
" C_lin = DT*(f_xu - np.dot(A,x_bar.reshape(N,1)) - np.dot(B,u_bar.reshape(M,1)))\n",
|
||
|
|
" \n",
|
||
|
|
" return np.round(A_lin,4), np.round(B_lin,4), np.round(C_lin,4)\n",
|
||
|
|
"\n",
|
||
|
|
"\"\"\"\n",
|
||
|
|
"the ODE is used to update the simulation given the mpc results\n",
|
||
|
|
"I use this insted of using the LTI twice\n",
|
||
|
|
"\"\"\"\n",
|
||
|
|
"def kinematics_model(x,t,u):\n",
|
||
|
|
" \"\"\"\n",
|
||
|
|
" Returns the set of ODE of the vehicle model.\n",
|
||
|
|
" \"\"\"\n",
|
||
|
|
" \n",
|
||
|
|
" L=0.3 #vehicle wheelbase\n",
|
||
|
|
" dxdt = x[2]*np.cos(x[3])\n",
|
||
|
|
" dydt = x[2]*np.sin(x[3])\n",
|
||
|
|
" dvdt = u[0]\n",
|
||
|
|
" dthetadt = x[2]*np.tan(u[1])/L\n",
|
||
|
|
"\n",
|
||
|
|
" dqdt = [dxdt,\n",
|
||
|
|
" dydt,\n",
|
||
|
|
" dvdt,\n",
|
||
|
|
" dthetadt]\n",
|
||
|
|
"\n",
|
||
|
|
" return dqdt\n",
|
||
|
|
"\n",
|
||
|
|
"def predict(x0,u):\n",
|
||
|
|
" \"\"\"\n",
|
||
|
|
" \"\"\"\n",
|
||
|
|
" \n",
|
||
|
|
" x_ = np.zeros((N,T+1))\n",
|
||
|
|
" \n",
|
||
|
|
" x_[:,0] = x0\n",
|
||
|
|
" \n",
|
||
|
|
" # solve ODE\n",
|
||
|
|
" for t in range(1,T+1):\n",
|
||
|
|
"\n",
|
||
|
|
" tspan = [0,DT]\n",
|
||
|
|
" x_next = odeint(kinematics_model,\n",
|
||
|
|
" x0,\n",
|
||
|
|
" tspan,\n",
|
||
|
|
" args=(u[:,t-1],))\n",
|
||
|
|
"\n",
|
||
|
|
" x0 = x_next[1]\n",
|
||
|
|
" x_[:,t]=x_next[1]\n",
|
||
|
|
" \n",
|
||
|
|
" return x_\n",
|
||
|
|
"\n",
|
||
|
|
"def compute_path_from_wp(start_xp, start_yp, step = 0.1):\n",
|
||
|
|
" \"\"\"\n",
|
||
|
|
" Computes a reference path given a set of waypoints\n",
|
||
|
|
" \"\"\"\n",
|
||
|
|
" \n",
|
||
|
|
" final_xp=[]\n",
|
||
|
|
" final_yp=[]\n",
|
||
|
|
" delta = step #[m]\n",
|
||
|
|
"\n",
|
||
|
|
" for idx in range(len(start_xp)-1):\n",
|
||
|
|
" section_len = np.sum(np.sqrt(np.power(np.diff(start_xp[idx:idx+2]),2)+np.power(np.diff(start_yp[idx:idx+2]),2)))\n",
|
||
|
|
"\n",
|
||
|
|
" interp_range = np.linspace(0,1,np.floor(section_len/delta).astype(int))\n",
|
||
|
|
" \n",
|
||
|
|
" fx=interp1d(np.linspace(0,1,2),start_xp[idx:idx+2],kind=1)\n",
|
||
|
|
" fy=interp1d(np.linspace(0,1,2),start_yp[idx:idx+2],kind=1)\n",
|
||
|
|
" \n",
|
||
|
|
" final_xp=np.append(final_xp,fx(interp_range))\n",
|
||
|
|
" final_yp=np.append(final_yp,fy(interp_range))\n",
|
||
|
|
" \n",
|
||
|
|
" dx = np.append(0, np.diff(final_xp))\n",
|
||
|
|
" dy = np.append(0, np.diff(final_yp))\n",
|
||
|
|
" theta = np.arctan2(dy, dx)\n",
|
||
|
|
"\n",
|
||
|
|
" return np.vstack((final_xp,final_yp,theta))\n",
|
||
|
|
"\n",
|
||
|
|
"\n",
|
||
|
|
"def get_nn_idx(state,path):\n",
|
||
|
|
" \"\"\"\n",
|
||
|
|
" Computes the index of the waypoint closest to vehicle\n",
|
||
|
|
" \"\"\"\n",
|
||
|
|
"\n",
|
||
|
|
" dx = state[0]-path[0,:]\n",
|
||
|
|
" dy = state[1]-path[1,:]\n",
|
||
|
|
" dist = np.hypot(dx,dy)\n",
|
||
|
|
" nn_idx = np.argmin(dist)\n",
|
||
|
|
"\n",
|
||
|
|
" try:\n",
|
||
|
|
" v = [path[0,nn_idx+1] - path[0,nn_idx],\n",
|
||
|
|
" path[1,nn_idx+1] - path[1,nn_idx]] \n",
|
||
|
|
" v /= np.linalg.norm(v)\n",
|
||
|
|
"\n",
|
||
|
|
" d = [path[0,nn_idx] - state[0],\n",
|
||
|
|
" path[1,nn_idx] - state[1]]\n",
|
||
|
|
"\n",
|
||
|
|
" if np.dot(d,v) > 0:\n",
|
||
|
|
" target_idx = nn_idx\n",
|
||
|
|
" else:\n",
|
||
|
|
" target_idx = nn_idx+1\n",
|
||
|
|
"\n",
|
||
|
|
" except IndexError as e:\n",
|
||
|
|
" target_idx = nn_idx\n",
|
||
|
|
"\n",
|
||
|
|
" return target_idx\n",
|
||
|
|
"\n",
|
||
|
|
"def get_ref_trajectory(state, path, target_v):\n",
|
||
|
|
" \"\"\"\n",
|
||
|
|
" Adapted from pythonrobotics\n",
|
||
|
|
" \"\"\"\n",
|
||
|
|
" xref = np.zeros((N, T + 1))\n",
|
||
|
|
" dref = np.zeros((1, T + 1))\n",
|
||
|
|
" \n",
|
||
|
|
" #sp = np.ones((1,T +1))*target_v #speed profile\n",
|
||
|
|
" \n",
|
||
|
|
" ncourse = path.shape[1]\n",
|
||
|
|
"\n",
|
||
|
|
" ind = get_nn_idx(state, path)\n",
|
||
|
|
"\n",
|
||
|
|
" xref[0, 0] = path[0,ind] #X\n",
|
||
|
|
" xref[1, 0] = path[1,ind] #Y\n",
|
||
|
|
" xref[2, 0] = target_v #sp[ind] #V\n",
|
||
|
|
" xref[3, 0] = path[2,ind] #Theta\n",
|
||
|
|
" dref[0, 0] = 0.0 # steer operational point should be 0\n",
|
||
|
|
" \n",
|
||
|
|
" dl = 0.05 # Waypoints spacing [m]\n",
|
||
|
|
" travel = 0.0\n",
|
||
|
|
"\n",
|
||
|
|
" for i in range(T + 1):\n",
|
||
|
|
" travel += abs(target_v) * DT #current V or target V?\n",
|
||
|
|
" dind = int(round(travel / dl))\n",
|
||
|
|
"\n",
|
||
|
|
" if (ind + dind) < ncourse:\n",
|
||
|
|
" xref[0, i] = path[0,ind + dind]\n",
|
||
|
|
" xref[1, i] = path[1,ind + dind]\n",
|
||
|
|
" xref[2, i] = target_v #sp[ind + dind]\n",
|
||
|
|
" xref[3, i] = path[2,ind + dind]\n",
|
||
|
|
" dref[0, i] = 0.0\n",
|
||
|
|
" else:\n",
|
||
|
|
" xref[0, i] = path[0,ncourse - 1]\n",
|
||
|
|
" xref[1, i] = path[1,ncourse - 1]\n",
|
||
|
|
" xref[2, i] = 0.0 #stop? #sp[ncourse - 1]\n",
|
||
|
|
" xref[3, i] = path[2,ncourse - 1]\n",
|
||
|
|
" dref[0, i] = 0.0\n",
|
||
|
|
"\n",
|
||
|
|
" return xref, dref"
|
||
|
|
]
|
||
|
|
},
|
||
|
|
{
|
||
|
|
"cell_type": "code",
|
||
|
|
"execution_count": 3,
|
||
|
|
"metadata": {},
|
||
|
|
"outputs": [
|
||
|
|
{
|
||
|
|
"name": "stderr",
|
||
|
|
"output_type": "stream",
|
||
|
|
"text": [
|
||
|
|
"<ipython-input-2-e22409964dd8>:127: RuntimeWarning: invalid value encountered in true_divide\n",
|
||
|
|
" v /= np.linalg.norm(v)\n"
|
||
|
|
]
|
||
|
|
},
|
||
|
|
{
|
||
|
|
"name": "stdout",
|
||
|
|
"output_type": "stream",
|
||
|
|
"text": [
|
||
|
|
"CVXPY Optimization Time: Avrg: 0.5979s Max: 0.8275s Min: 0.2939s\n"
|
||
|
|
]
|
||
|
|
}
|
||
|
|
],
|
||
|
|
"source": [
|
||
|
|
"track = compute_path_from_wp([0,3,4,6,10,12,14,6,1,0],\n",
|
||
|
|
" [0,0,2,4,3,3,-2,-6,-2,-2],0.05)\n",
|
||
|
|
"\n",
|
||
|
|
"# track = compute_path_from_wp([0,10,10,0],\n",
|
||
|
|
"# [0,0,1,1],0.05)\n",
|
||
|
|
"\n",
|
||
|
|
"sim_duration = 200 #time steps\n",
|
||
|
|
"opt_time=[]\n",
|
||
|
|
"\n",
|
||
|
|
"x_sim = np.zeros((N,sim_duration))\n",
|
||
|
|
"u_sim = np.zeros((M,sim_duration-1))\n",
|
||
|
|
"\n",
|
||
|
|
"MAX_SPEED = 1.5 #m/s\n",
|
||
|
|
"MAX_ACC = 1.0 #m/ss\n",
|
||
|
|
"MAX_D_ACC = 1.0 #m/sss\n",
|
||
|
|
"MAX_STEER = np.radians(30) #rad\n",
|
||
|
|
"MAX_D_STEER = np.radians(30) #rad/s\n",
|
||
|
|
"\n",
|
||
|
|
"REF_VEL = 1.0 #m/s\n",
|
||
|
|
"\n",
|
||
|
|
"# Starting Condition\n",
|
||
|
|
"x0 = np.zeros(N)\n",
|
||
|
|
"x0[0] = 0 #x\n",
|
||
|
|
"x0[1] = -0.25 #y\n",
|
||
|
|
"x0[2] = 0.0 #v\n",
|
||
|
|
"x0[3] = np.radians(-0) #yaw\n",
|
||
|
|
"\n",
|
||
|
|
"for sim_time in range(sim_duration-1):\n",
|
||
|
|
" \n",
|
||
|
|
" iter_start = time.time()\n",
|
||
|
|
" \n",
|
||
|
|
" #starting guess for ctrl\n",
|
||
|
|
" u_bar = np.zeros((M,T))\n",
|
||
|
|
" u_bar[0,:] = MAX_ACC/2 #a\n",
|
||
|
|
" u_bar[1,:] = 0.0 #delta \n",
|
||
|
|
" \n",
|
||
|
|
" for _ in range(5):\n",
|
||
|
|
" u_prev = u_bar\n",
|
||
|
|
" \n",
|
||
|
|
" # dynamics starting state\n",
|
||
|
|
" x_bar = np.zeros((N,T+1))\n",
|
||
|
|
" x_bar[:,0] = x_sim[:,sim_time]\n",
|
||
|
|
"\n",
|
||
|
|
" #prediction for linearization of costrains\n",
|
||
|
|
" for t in range (1,T+1):\n",
|
||
|
|
" xt = x_bar[:,t-1].reshape(N,1)\n",
|
||
|
|
" ut = u_bar[:,t-1].reshape(M,1)\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",
|
||
|
|
" x_bar[:,t] = xt_plus_one\n",
|
||
|
|
"\n",
|
||
|
|
" #CVXPY Linear MPC problem statement\n",
|
||
|
|
" x = cp.Variable((N, T+1))\n",
|
||
|
|
" u = cp.Variable((M, T))\n",
|
||
|
|
" cost = 0\n",
|
||
|
|
" constr = []\n",
|
||
|
|
"\n",
|
||
|
|
" # Cost Matrices\n",
|
||
|
|
" Q = np.diag([20,20,10,0]) #state error cost\n",
|
||
|
|
" Qf = np.diag([30,30,30,0]) #state final error cost\n",
|
||
|
|
" R = np.diag([10,10]) #input cost\n",
|
||
|
|
" R_ = np.diag([10,10]) #input rate of change cost\n",
|
||
|
|
"\n",
|
||
|
|
" #Get Reference_traj\n",
|
||
|
|
" x_ref, d_ref = get_ref_trajectory(x_bar[:,0] ,track, REF_VEL)\n",
|
||
|
|
"\n",
|
||
|
|
" #Prediction Horizon\n",
|
||
|
|
" for t in range(T):\n",
|
||
|
|
"\n",
|
||
|
|
" # Tracking Error\n",
|
||
|
|
" cost += cp.quad_form(x[:,t] - x_ref[:,t], Q)\n",
|
||
|
|
"\n",
|
||
|
|
" # Actuation effort\n",
|
||
|
|
" cost += cp.quad_form(u[:,t], R)\n",
|
||
|
|
"\n",
|
||
|
|
" # Actuation rate of change\n",
|
||
|
|
" if t < (T - 1):\n",
|
||
|
|
" cost += cp.quad_form(u[:,t+1] - u[:,t], R_)\n",
|
||
|
|
" constr+= [cp.abs(u[0, t + 1] - u[0, t])/DT <= MAX_D_ACC] #max acc rate of change\n",
|
||
|
|
" constr += [cp.abs(u[1, t + 1] - u[1, t])/DT <= MAX_D_STEER] #max steer rate of change\n",
|
||
|
|
"\n",
|
||
|
|
" # Kinrmatics Constrains (Linearized model)\n",
|
||
|
|
" A,B,C = get_linear_model(x_bar[:,t], u_bar[:,t])\n",
|
||
|
|
" constr += [x[:,t+1] == A@x[:,t] + B@u[:,t] + C.flatten()]\n",
|
||
|
|
"\n",
|
||
|
|
" #Final Point tracking\n",
|
||
|
|
" cost += cp.quad_form(x[:, T] - x_ref[:, T], Qf)\n",
|
||
|
|
"\n",
|
||
|
|
" # sums problem objectives and concatenates constraints.\n",
|
||
|
|
" constr += [x[:,0] == x_bar[:,0]] #starting condition\n",
|
||
|
|
" constr += [x[2,:] <= MAX_SPEED] #max speed\n",
|
||
|
|
" constr += [x[2,:] >= 0.0] #min_speed (not really needed)\n",
|
||
|
|
" constr += [cp.abs(u[0,:]) <= MAX_ACC] #max acc\n",
|
||
|
|
" constr += [cp.abs(u[1,:]) <= MAX_STEER] #max steer\n",
|
||
|
|
"\n",
|
||
|
|
" # Solve\n",
|
||
|
|
" prob = cp.Problem(cp.Minimize(cost), constr)\n",
|
||
|
|
" solution = prob.solve(solver=cp.OSQP, verbose=False)\n",
|
||
|
|
"\n",
|
||
|
|
" #retrieved optimized U and assign to u_bar to linearize in next step\n",
|
||
|
|
" u_bar = np.vstack((np.array(u.value[0,:]).flatten(),\n",
|
||
|
|
" (np.array(u.value[1,:]).flatten())))\n",
|
||
|
|
" \n",
|
||
|
|
" #check how this solution differs from previous\n",
|
||
|
|
" #if the solutions are very\n",
|
||
|
|
" delta_u = np.sum(np.sum(np.abs(u_bar - u_prev),axis=0),axis=0)\n",
|
||
|
|
" if delta_u < 0.05:\n",
|
||
|
|
" break\n",
|
||
|
|
" \n",
|
||
|
|
" \n",
|
||
|
|
" # select u from best iteration\n",
|
||
|
|
" u_sim[:,sim_time] = u_bar[:,0]\n",
|
||
|
|
" \n",
|
||
|
|
" \n",
|
||
|
|
" # Measure elpased time to get results from cvxpy\n",
|
||
|
|
" opt_time.append(time.time()-iter_start)\n",
|
||
|
|
" \n",
|
||
|
|
" # move simulation to t+1\n",
|
||
|
|
" tspan = [0,DT]\n",
|
||
|
|
" x_sim[:,sim_time+1] = odeint(kinematics_model,\n",
|
||
|
|
" x_sim[:,sim_time],\n",
|
||
|
|
" tspan,\n",
|
||
|
|
" args=(u_bar[:,0],))[1]\n",
|
||
|
|
" \n",
|
||
|
|
" #reset u_bar? -> this simulates that we don use previous solution!\n",
|
||
|
|
" u_bar[0,:] = MAX_ACC/2 #a\n",
|
||
|
|
" u_bar[1,:] = 0.0 #delta\n",
|
||
|
|
" \n",
|
||
|
|
" \n",
|
||
|
|
"print(\"CVXPY Optimization Time: Avrg: {:.4f}s Max: {:.4f}s Min: {:.4f}s\".format(np.mean(opt_time),\n",
|
||
|
|
" np.max(opt_time),\n",
|
||
|
|
" np.min(opt_time))) "
|
||
|
|
]
|
||
|
|
},
|
||
|
|
{
|
||
|
|
"cell_type": "code",
|
||
|
|
"execution_count": 4,
|
||
|
|
"metadata": {},
|
||
|
|
"outputs": [
|
||
|
|
{
|
||
|
|
"data": {
|
||
|
|
"image/png": "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
|
||
|
|
"text/plain": [
|
||
|
|
"<Figure size 1080x720 with 5 Axes>"
|
||
|
|
]
|
||
|
|
},
|
||
|
|
"metadata": {},
|
||
|
|
"output_type": "display_data"
|
||
|
|
}
|
||
|
|
],
|
||
|
|
"source": [
|
||
|
|
"#plot trajectory\n",
|
||
|
|
"grid = plt.GridSpec(4, 5)\n",
|
||
|
|
"\n",
|
||
|
|
"plt.figure(figsize=(15,10))\n",
|
||
|
|
"\n",
|
||
|
|
"plt.subplot(grid[0:4, 0:4])\n",
|
||
|
|
"plt.plot(track[0,:],track[1,:],\"b+\")\n",
|
||
|
|
"plt.plot(x_sim[0,:],x_sim[1,:])\n",
|
||
|
|
"plt.axis(\"equal\")\n",
|
||
|
|
"plt.ylabel('y')\n",
|
||
|
|
"plt.xlabel('x')\n",
|
||
|
|
"\n",
|
||
|
|
"plt.subplot(grid[0, 4])\n",
|
||
|
|
"plt.plot(u_sim[0,:])\n",
|
||
|
|
"plt.ylabel('a(t) [m/ss]')\n",
|
||
|
|
"\n",
|
||
|
|
"plt.subplot(grid[1, 4])\n",
|
||
|
|
"plt.plot(x_sim[2,:])\n",
|
||
|
|
"plt.ylabel('v(t) [m/s]')\n",
|
||
|
|
"\n",
|
||
|
|
"plt.subplot(grid[2, 4])\n",
|
||
|
|
"plt.plot(np.degrees(u_sim[1,:]))\n",
|
||
|
|
"plt.ylabel('delta(t) [rad]')\n",
|
||
|
|
"\n",
|
||
|
|
"plt.subplot(grid[3, 4])\n",
|
||
|
|
"plt.plot(x_sim[3,:])\n",
|
||
|
|
"plt.ylabel('theta(t) [rad]')\n",
|
||
|
|
"\n",
|
||
|
|
"plt.tight_layout()\n",
|
||
|
|
"plt.show()"
|
||
|
|
]
|
||
|
|
}
|
||
|
|
],
|
||
|
|
"metadata": {
|
||
|
|
"kernelspec": {
|
||
|
|
"display_name": "Python [conda env:.conda-jupyter] *",
|
||
|
|
"language": "python",
|
||
|
|
"name": "conda-env-.conda-jupyter-py"
|
||
|
|
},
|
||
|
|
"language_info": {
|
||
|
|
"codemirror_mode": {
|
||
|
|
"name": "ipython",
|
||
|
|
"version": 3
|
||
|
|
},
|
||
|
|
"file_extension": ".py",
|
||
|
|
"mimetype": "text/x-python",
|
||
|
|
"name": "python",
|
||
|
|
"nbconvert_exporter": "python",
|
||
|
|
"pygments_lexer": "ipython3",
|
||
|
|
"version": "3.8.5"
|
||
|
|
}
|
||
|
|
},
|
||
|
|
"nbformat": 4,
|
||
|
|
"nbformat_minor": 4
|
||
|
|
}
|