CPG for two odrives(left and right legs).ipynb

{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# 1 Design gait and basic functions"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "import numpy as np\n",
    "\n",
    "import matplotlib as mpl\n",
    "\n",
    "import matplotlib.pyplot as plt\n",
    "plt.ion()\n",
    "\n",
    "# for the symbolic manipulation of jacobian\n",
    "import sympy as sp\n",
    "# from sympy import symbols\n",
    "# from sympy import sin, cos, asin, acos, pi, atan2, sqrt\n",
    "from sympy.utilities.lambdify import lambdify\n",
    "# from sympy import Matrix\n",
    "\n",
    "from scipy.optimize import minimize\n",
    "from scipy.optimize import fsolve\n",
    "from scipy import interpolate"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "## Motor constants\n",
    "K = 0.0285;                 # Nm / A\n",
    "peak_amp = 30;              # A\n",
    "peak_torque = K * peak_amp; # Nm\n",
    "m_motor = 0.2;              # kg\n",
    "m_assembly = 0.2;           # kg\n",
    "\n",
    "m_total = 2*m_motor + m_assembly; # kg\n",
    "\n",
    "gravity = 9.8;              # m/s^2\n",
    "\n",
    "weight = m_total * gravity  # N\n",
    "\n",
    "l1 = 0.09;                  # m \n",
    "l2 = 0.16;                  # m\n",
    "w = 0.07;                   # m\n",
    "\n",
    "T = 0.25;                   # s\n",
    "d = 0.5;\n",
    "N = 100;"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# solve jacobian of constraint equation\n",
    "(thetaL_sym, \n",
    " thetaR_sym, \n",
    " link1_sym, \n",
    " link2_sym, \n",
    " width_sym) = sp.symbols(\"\"\"thetaL_sym \n",
    "                            thetaR_sym \n",
    "                            link1_sym \n",
    "                            link2_sym \n",
    "                            width_sym\"\"\", real = True)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "def T(theta, x, y):\n",
    "    return sp.Matrix([[sp.cos(theta), -sp.sin(theta), x], \n",
    "                      [sp.sin(theta), sp.cos(theta), y],\n",
    "                      [0, 0, 1]])\n",
    "\n",
    "def sym_to_np(T):\n",
    "    return np.array(T).astype(np.float64)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## 1.1 FK Through transformation matrices"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "x_r = width_sym/2 + link1_sym*sp.cos(thetaR_sym)\n",
    "x_l = -width_sym/2 + link1_sym*sp.cos(thetaL_sym)\n",
    "\n",
    "y_r = link1_sym*sp.sin(thetaR_sym)\n",
    "y_l = link1_sym*sp.sin(thetaL_sym)\n",
    "\n",
    "theta3_sym = sp.atan2(y_r - y_l, x_r - x_l)\n",
    "L = sp.sqrt((x_l - x_r)**2 + (y_l - y_r)**2)\n",
    "\n",
    "FK = T(thetaL_sym, -width_sym/2, 0)@T(-(thetaL_sym - theta3_sym), link1_sym, 0)@sp.Matrix([L/2, sp.sqrt(link2_sym**2 - (L/2)**2), 1])\n",
    "FK = FK[:2,:]\n",
    "FK.simplify()\n",
    "FK_fast = FK.subs([(link1_sym, l1), (link2_sym, l2), (width_sym, w)]).evalf()\n",
    "# lambdify the forward kinematics (makes it faster)\n",
    "FK_lambda = lambdify((thetaR_sym, thetaL_sym), FK_fast)\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## 1.2 IK Through Fsolve method"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "def IK_constraint_eqn(x, x_pt, y_pt, l1, l2, w):\n",
    "    theta_R = x[0]\n",
    "    theta_L = x[1]\n",
    "    \n",
    "    return (l1**2 - l2**2 + (x_pt - w/2)**2 + y_pt**2 - 2*l1 * (y_pt * np.sin(theta_R) + (x_pt - w/2)*np.cos(theta_R)),\n",
    "            l1**2 - l2**2 + (x_pt + w/2)**2 + y_pt**2 - 2*l1 * (y_pt * np.sin(theta_L) + (x_pt + w/2)*np.cos(theta_L)))"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## 1.3 define a function to transport gait to angle versus time"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "def add_arrow(line, position=None, direction='right', size=15, color=None):\n",
    "    \"\"\"\n",
    "    add an arrow to a line.\n",
    "\n",
    "    line:       Line2D object\n",
    "    position:   x-position of the arrow. If None, mean of xdata is taken\n",
    "    direction:  'left' or 'right'\n",
    "    size:       size of the arrow in fontsize points\n",
    "    color:      if None, line color is taken.\n",
    "    \"\"\"\n",
    "    if color is None:\n",
    "        color = line.get_color()\n",
    "\n",
    "    xdata = line.get_xdata()\n",
    "    ydata = line.get_ydata()\n",
    "\n",
    "    if position is None:\n",
    "        position = xdata.mean()\n",
    "    # find closest index\n",
    "    start_ind = np.argmin(np.absolute(xdata - position))\n",
    "    if direction == 'right':\n",
    "        end_ind = start_ind + 1\n",
    "    else:\n",
    "        end_ind = start_ind - 1\n",
    "\n",
    "    line.axes.annotate('',\n",
    "        xytext=(xdata[start_ind], ydata[start_ind]),\n",
    "        xy=(xdata[end_ind], ydata[end_ind]),\n",
    "        arrowprops=dict(arrowstyle=\"->\", color=color),\n",
    "        size=size\n",
    "    )\n",
    "\n",
    "def gait2angle(x_on, y_on, x_off, y_off, x_sw, y_sw, T, d, N, leg_position):    \n",
    "    if leg_position == 1:\n",
    "        x_on = -x_on\n",
    "        x_off = -x_off\n",
    "        x_sw = -x_sw\n",
    "    \n",
    "    N_st = np.int64(N*d)  \n",
    "    N_sw = np.int64(N*(1-d))\n",
    "\n",
    "    # generate line of stance\n",
    "    x = np.array([x_on,x_off])\n",
    "    y = np.array([y_on,y_off])\n",
    "    z = np.polyfit(x, y, deg = 1)\n",
    "    poly_st = np.poly1d(z)\n",
    "    x_st = np.linspace(x_on, x_off, N_st+1)\n",
    "    \n",
    "    # generate poly of swing\n",
    "    x = np.array([x_off,x_sw,x_on])\n",
    "    y = np.array([y_off,y_sw,y_on])\n",
    "    z = np.polyfit(x, y, deg = 2)\n",
    "    poly_sw = np.poly1d(z)\n",
    "    x_sw = np.linspace(x_off, x_on, N_sw+1)\n",
    "\n",
    "    # plot gait\n",
    "    plt.figure()\n",
    "    st_line = plt.plot(x_st, poly_st(x_st),'b', label=\"Stance phase\")[0]\n",
    "    add_arrow(st_line)\n",
    "    sw_curve = plt.plot(x_sw, poly_sw(x_sw),'r', label=\"Swing phase\")[0]\n",
    "    add_arrow(sw_curve)\n",
    "    plt.plot(x_st[0], poly_st(x_st[0]), 's', label=\"ON position\")\n",
    "    plt.plot(x_st[-1], poly_st(x_st[-1]), 'o', label=\"OFF position\")\n",
    "    if leg_position == 1:\n",
    "        plt.plot(x_st[-1], poly_st(x_st[-1]), 'o', markersize=12, label=\"START position\")\n",
    "    else:\n",
    "        plt.plot(x_st[0], poly_st(x_st[0]), 'o', markersize=12, label=\"START position\")\n",
    "    plt.title('Gait trajectory')\n",
    "    plt.xlabel('x (m)')\n",
    "    plt.ylabel('y (m)')\n",
    "    plt.legend()\n",
    "    \n",
    "    y_st = poly_st(x_st)\n",
    "    y_sw = poly_sw(x_sw)\n",
    "    \n",
    "    all_x = np.append(x_st, x_sw)\n",
    "    all_y = np.append(y_st, y_sw)\n",
    "    \n",
    "    print('gait origin is ', np.mean(all_x), np.mean(all_y))\n",
    "    \n",
    "    # stance gait to angle\n",
    "    theta_st = np.zeros(shape=(N_st+1,2))\n",
    "    i = 0\n",
    "    for j in x_st:\n",
    "        theta_st[i] = fsolve(lambda x: IK_constraint_eqn(x, j, poly_st(j), l1, l2,w), (0,np.pi))\n",
    "        i = i+1\n",
    "    \n",
    "    # swing gait to angle\n",
    "    theta_sw = np.zeros(shape=(N_sw+1,2))\n",
    "    i = 0\n",
    "    for j in x_sw:\n",
    "        theta_sw[i] = fsolve(lambda x: IK_constraint_eqn(x, j, poly_sw(j), l1, l2,w), (0,np.pi))\n",
    "        i = i+1\n",
    "    \n",
    "    # plot angle versus time\n",
    "    plt.figure()\n",
    "    if leg_position == 0:\n",
    "        theta_sw = np.delete(theta_sw, 0, 0)\n",
    "        theta = np.append(theta_st, theta_sw, axis = 0)\n",
    "    elif leg_position == 1:\n",
    "        theta_st = np.delete(theta_st, 0, 0)\n",
    "        theta = np.append(theta_sw, theta_st, axis = 0)\n",
    "         \n",
    "    t = np.linspace(0, T, N+1)\n",
    "    plt.plot(t, theta[:, 0],'r')\n",
    "    plt.plot(t, theta[:, 1],'b')\n",
    "    plt.title('Angle versus time')\n",
    "    plt.xlabel('t (s)')\n",
    "    plt.ylabel('θ (rad)')\n",
    "    plt.legend(['${θ_R}$','${θ_L}$'])\n",
    "        \n",
    "    return (theta, np.mean(all_x), np.mean(all_y))"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## 1.4 plot the gait(top) & angle versus time(bottom)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "theta1, origin_x, origin_y = gait2angle(0.11,0.19,-0.11,0.19,0,0.12,0.25,0.5,100,0)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "theta2, origin_x, origin_y = gait2angle(0.11,0.19,-0.11,0.19,0,0.12,0.25,0.5,100,1)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Gait design and selection"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Left leg"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "x_on = 0.0271340208767868\n",
    "y_on = 0.150504090025698\n",
    "x_off = -0.0622642119510020\n",
    "y_off = 0.180845634937132\n",
    "\n",
    "x_sw = -0.0210965760128508\n",
    "y_sw = 0.0874034639992732\n",
    "\n",
    "LEFT_LEG = 0\n",
    "RIGHT_LEG = 1\n",
    "\n",
    "T            = 0.25        # seconds\n",
    "dutyfactor   = 0.5         # duty factor (higher means more )\n",
    "N            = 100         # no. of trajectory points\n",
    "leg_position = LEFT_LEG    # which leg?\n",
    "\n",
    "theta1, origin_x, origin_y = gait2angle(x_on, y_on, x_off, y_off, x_sw, y_sw, \\\n",
    "                    T, dutyfactor, N, leg_position)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "leg_position = RIGHT_LEG    # which leg?\n",
    "\n",
    "theta2, origin_x, origin_y = gait2angle(x_on, y_on, x_off, y_off, x_sw, y_sw, \\\n",
    "                    T, dutyfactor, N, leg_position)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# 2 Precompute and interpolate the transform funtions"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## 2.1 define trajectory parameters"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# y_shift = 0.02\n",
    "# x_pos = 0.07\n",
    "# x_on = 0.03\n",
    "# y_on = 0.2-y_shift\n",
    "# x_off = -0.07\n",
    "# y_off = 0.2-y_shift\n",
    "# x_sw = 0\n",
    "# y_sw = 0.08\n",
    "\n",
    "# y_origin = (y_off + y_sw)/2\n",
    "# x_origin = (x_on + x_off)/2\n",
    "\n",
    "trajectories = [\n",
    "    {'x_on': 0.0271340208767868,\n",
    "     'y_on': 0.150504090025698,\n",
    "     'x_off': -0.0622642119510020,\n",
    "     'y_off': 0.180845634937132,\n",
    "     'x_sw': -0.0210965760128508,\n",
    "     'y_sw': 0.0874034639992732,\n",
    "    },\n",
    "    {\n",
    "     'x_on': 0.0371340208767868,\n",
    "     'y_on': 0.150504090025698,\n",
    "     'x_off': -0.0622642119510020,\n",
    "     'y_off': 0.170845634937132,\n",
    "     'x_sw': -0.0210965760128508,\n",
    "     'y_sw': 0.0874034639992732,\n",
    "    }]\n",
    "\n",
    "trajectory = \"2\"\n",
    "traj = trajectories[int(trajectory)-1]\n",
    "# load trajectory parameters (on, off and swing points)\n",
    "x_on = traj['x_on']\n",
    "y_on = traj['y_on']\n",
    "x_off = traj['x_off']\n",
    "y_off = traj['y_off']\n",
    "x_sw = traj['x_sw']\n",
    "y_sw = traj['y_sw']\n",
    "\n",
    "T          = 0.25 # seconds\n",
    "dutyfactor = 0.5 # dutyfactor\n",
    "N          = 100 # number of trajectory points\n",
    "\n",
    "# calculate the gait/trajectory's origin (using left leg always!)\n",
    "_, origin_x, origin_y = gait2angle(x_on, y_on, x_off, y_off, x_sw, y_sw, \\\n",
    "                    T, dutyfactor, N, LEFT_LEG)\n",
    "\n",
    "_, a, b = gait2angle(x_on, y_on, x_off, y_off, x_sw, y_sw, \\\n",
    "                    T, dutyfactor, N, RIGHT_LEG)\n",
    "\n",
    "\n",
    "x_origin = origin_x\n",
    "y_origin = origin_y\n",
    "\n",
    "psi_left = sp.atan2((y_off-y_origin),(x_off-x_origin))\n",
    "psi_right = sp.atan2((y_on-y_origin),(x_on-x_origin))\n",
    "\n",
    "x = np.array([x_on-x_origin,x_off-x_origin])\n",
    "y = np.array([y_on-y_origin,y_off-y_origin])\n",
    "z_st = np.polyfit(x, y, deg = 1)\n",
    "poly_st = np.poly1d(z_st)\n",
    "y_st = z_st[1]\n",
    "\n",
    "x = np.array([x_off-x_origin,x_sw-x_origin,x_on-x_origin])\n",
    "y = np.array([y_off-y_origin,y_sw-y_origin,y_on-y_origin])\n",
    "z_sw = np.polyfit(x, y, deg = 2)\n",
    "poly_sw = np.poly1d(z_sw)\n",
    "y_sw = z_sw"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## 2.2 define ψ to motor angle function & get current ψ function"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# ψ to motor angle function for left odrive\n",
    "def psi2angleL(psi,x_on=x_on,y_on=y_on,x_off=x_off,y_off=y_off,x_sw=x_sw,y_sw=y_sw,x_origin=x_origin,y_origin=y_origin):\n",
    "\n",
    "    #whether psi matches the swing or stance\n",
    "    if psi_right<psi<psi_left:\n",
    "        #calculate the x,y position related to psi\n",
    "        if psi == sp.pi/2 or psi == np.pi/2:\n",
    "            cal_x = 0\n",
    "        else:\n",
    "            x = sp.symbols('x')\n",
    "            cal_x = y_st/sp.tan(psi)\n",
    "        cal_y = y_st\n",
    "    else:\n",
    "        x = sp.symbols('x')\n",
    "        cal_x = sp.solve(sp.tan(psi) - poly_sw(x)/x,x)\n",
    "        if psi == sp.pi/2*3 or psi == np.pi/2*3:\n",
    "            cal_x = 0 \n",
    "        elif sp.pi/2<psi<sp.pi/2*3:\n",
    "            if cal_x[0]<0:\n",
    "                cal_x = cal_x[0]\n",
    "            else:\n",
    "                cal_x = cal_x[1]\n",
    "        elif psi>sp.pi/2*3 or psi<sp.pi/2:\n",
    "            if cal_x[0]>=0:\n",
    "                cal_x = cal_x[0]\n",
    "            else:\n",
    "                cal_x = cal_x[1]\n",
    "        cal_y = poly_sw(cal_x)\n",
    "    \n",
    "    cal_x = cal_x + x_origin\n",
    "    cal_y = cal_y + y_origin\n",
    "        \n",
    "    theta_motor = fsolve(lambda x: IK_constraint_eqn(x, cal_x, cal_y, l1, l2,w), (0,np.pi))    \n",
    "    \n",
    "    return(theta_motor)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# ψ to motor angle function for right odrive\n",
    "def psi2angleR(psi,x_on=x_on,y_on=y_on,x_off=x_off,y_off=y_off,x_sw=x_sw,y_sw=y_sw,x_origin=x_origin,y_origin=y_origin):\n",
    "\n",
    "    #whether psi matches the swing or stance\n",
    "    if psi_right<psi<psi_left:\n",
    "        #calculate the x,y position related to psi\n",
    "        if psi == sp.pi/2 or psi == np.pi/2:\n",
    "            cal_x = 0\n",
    "        else:\n",
    "            x = sp.symbols('x')\n",
    "            cal_x = sp.solve(sp.tan(psi) - poly_st(x)/x,x)[0]\n",
    "        cal_y = poly_st(cal_x)\n",
    "    else:\n",
    "        x = sp.symbols('x')\n",
    "        cal_x = sp.solve(sp.tan(psi) - poly_sw(x)/x,x)\n",
    "        if psi == sp.pi/2*3 or psi == np.pi/2*3:\n",
    "            cal_x = 0 \n",
    "        elif sp.pi/2<psi<sp.pi/2*3:\n",
    "            if cal_x[0]<0:\n",
    "                cal_x = cal_x[0]\n",
    "            else:\n",
    "                cal_x = cal_x[1]\n",
    "        elif psi>sp.pi/2*3 or psi<sp.pi/2:\n",
    "            if cal_x[0]>=0:\n",
    "                cal_x = cal_x[0]\n",
    "            else:\n",
    "                cal_x = cal_x[1]\n",
    "        else:\n",
    "            cal_x = 0 \n",
    "        cal_y = poly_sw(cal_x)\n",
    "    \n",
    "    cal_x = -(cal_x + x_origin)\n",
    "    cal_y = cal_y + y_origin\n",
    "        \n",
    "    theta_motor = fsolve(lambda x: IK_constraint_eqn(x, cal_x, cal_y, l1, l2,w), (0,np.pi))    \n",
    "    \n",
    "    return(theta_motor)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## 2.3 precompute the function (input is motor_θ and output are x and y)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "N_int = 100\n",
    "thetaL_int, thetaR_int = np.mgrid[0:2*np.pi+2*np.pi/N_int:2*np.pi/N_int, -np.pi:np.pi+2*np.pi/N_int:2*np.pi/N_int]"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "#%%time\n",
    "#generate points for the Right leg    \n",
    "x_L = np.zeros((N_int+1,N_int+1))\n",
    "y_L = np.zeros((N_int+1,N_int+1))\n",
    "a = 0\n",
    "\n",
    "for i in np.arange(0,2*np.pi+2*np.pi/N_int,2*np.pi/N_int):\n",
    "    b = 0\n",
    "    for j in np.arange(-np.pi,np.pi+2*np.pi/N_int,2*np.pi/N_int):\n",
    "        # [x,y] = FK_fast.subs([(thetaL_sym, i), (thetaR_sym, j)])\n",
    "        [x,y] = FK_lambda(j, i)\n",
    "        x = x-x_origin\n",
    "        y = y-y_origin\n",
    "        x_L[a,b] = x\n",
    "        y_L[a,b] = y\n",
    "        b = b+1\n",
    "    a = a+1"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "np.save('x_L_traj{}.npy'.format(trajectory), x_L)\n",
    "np.save('y_L_traj{}.npy'.format(trajectory), y_L)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "x_L = np.load('x_L_traj{}.npy'.format(trajectory))\n",
    "y_L = np.load('y_L_traj{}.npy'.format(trajectory))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "#%%time\n",
    "tck_getxL = interpolate.bisplrep(thetaL_int, thetaR_int, x_L,s=0.01)\n",
    "tck_getyL = interpolate.bisplrep(thetaL_int, thetaR_int, y_L,s=0.01)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# %%time\n",
    "# #generate points for the Right leg    \n",
    "x_R = np.zeros((N_int+1,N_int+1))\n",
    "y_R = np.zeros((N_int+1,N_int+1))\n",
    "a = 0\n",
    "for i in np.arange(0,2*np.pi+2*np.pi/N_int,2*np.pi/N_int):\n",
    "    b = 0\n",
    "    for j in np.arange(-np.pi,np.pi+2*np.pi/N_int,2*np.pi/N_int):\n",
    "        #[x,y] = FK_fast.subs([(thetaL_sym, i), (thetaR_sym, j)])\n",
    "        [x,y] = FK_lambda(j,i)\n",
    "        x = -(x-x_origin)\n",
    "        y = y-y_origin\n",
    "        x_R[a,b] = x\n",
    "        y_R[a,b] = y\n",
    "        b = b+1\n",
    "    a = a+1"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "np.save('x_R_traj{}.npy'.format(trajectory),x_R)\n",
    "np.save('y_R_traj{}.npy'.format(trajectory),y_R)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "x_R = np.load('x_R_traj{}.npy'.format(trajectory))\n",
    "y_R = np.load('y_R_traj{}.npy'.format(trajectory))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "#%%time\n",
    "tck_getxR = interpolate.bisplrep(thetaL_int, thetaR_int, x_R,s=0.01)\n",
    "tck_getyR = interpolate.bisplrep(thetaL_int, thetaR_int, y_R,s=0.01)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "def xy2psi(x,y):\n",
    "    if x>0 and y>=0:\n",
    "        real_psi = sp.atan(y/x)\n",
    "    elif x>0 and y<0:\n",
    "        real_psi = sp.atan(y/x) + 2*sp.pi\n",
    "    elif x<0:\n",
    "        real_psi = sp.atan(y/x) + sp.pi\n",
    "    elif x==0 and y>0:\n",
    "        real_psi = sp.pi/2\n",
    "    else:\n",
    "        real_psi = sp.pi/2*3\n",
    "    return(real_psi)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## 2.4 precompute the function (input is ψ and output is motor_θ)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "N_int = 50\n",
    "psi_int = np.arange(0,2*np.pi+np.pi/N_int*2,np.pi/N_int*2)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# %%time\n",
    "# # use interpolation to genetate funcition to match psi and motor thetas\n",
    "\n",
    "#generate points for the Left leg\n",
    "j = 0\n",
    "theta_intL = np.zeros((N_int+1,2))\n",
    "for i in psi_int:\n",
    "    theta_intL[j] = psi2angleL(i)\n",
    "    j = j+1\n",
    "    \n",
    "#generate points for the Right leg    \n",
    "j = 0\n",
    "theta_intR = np.zeros((N_int+1,2))\n",
    "for i in psi_int:\n",
    "    theta_intR[j] = psi2angleR(i)\n",
    "    j = j+1"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "np.save('theta_intL_traj{}.npy'.format(trajectory),theta_intL)\n",
    "np.save('theta_intR_traj{}.npy'.format(trajectory),theta_intR)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "theta_intL = np.load('theta_intL_traj{}.npy'.format(trajectory))\n",
    "theta_intR = np.load('theta_intR_traj{}.npy'.format(trajectory))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "#%%time\n",
    "tck_L0=interpolate.splrep(psi_int,theta_intL[:,0])\n",
    "tck_L1=interpolate.splrep(psi_int,theta_intL[:,1])\n",
    "tck_R0=interpolate.splrep(psi_int,theta_intR[:,0])\n",
    "tck_R1=interpolate.splrep(psi_int,theta_intR[:,1])"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## 2.5 define phase change function for CPG"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# consider right leg moves faster than left leg \n",
    "# psi from psi_right(o,pi/2) to psi_left(pi/2,pi) to psi_right(2*pi,5/2*pi)\n",
    "psi_st = np.float64(np.array([psi_right,psi_left]))\n",
    "delta_st = np.float64(np.array([psi_left-psi_right,sp.pi*2-(psi_left-psi_right)]))\n",
    "z = np.polyfit(psi_st, delta_st, deg = 1)\n",
    "poly_delta_st = np.poly1d(z)\n",
    "\n",
    "psi_sw = np.float64(np.array([psi_left,psi_right+sp.pi*2]))\n",
    "delta_sw = np.float64(np.array([sp.pi*2-(psi_left-psi_right),psi_left-psi_right]))\n",
    "z = np.polyfit(psi_sw, delta_sw, deg = 1)\n",
    "poly_delta_sw = np.poly1d(z)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# 3 Ready to use Odrive"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2,
   "metadata": {},
   "outputs": [],
   "source": [
    "import time\n",
    "\n",
    "import odrive\n",
    "import odrive.utils\n",
    "import odrive.enums"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## 3.1 Connect to ODrive"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Connect to odrv1, calibrate,  dump errors"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 13,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Connected!\n",
      "Odrive serial 53207019500085\n",
      "Motor 0 calibrated: True\n",
      "Motor 1 calibrated: True\n"
     ]
    }
   ],
   "source": [
    "odrv0 = odrive.find_any(serial_number = '3064397F3235', timeout = 20)\n",
    "if odrv0 is not None:\n",
    "    print('Connected!')\n",
    "    print('Odrive serial {}'.format(odrv0.serial_number))\n",
    "    \n",
    "    m0 = odrv0.axis0.motor.is_calibrated\n",
    "    m1 = odrv0.axis1.motor.is_calibrated\n",
    "    \n",
    "    print('Motor 0 calibrated: {}'.format(m0))\n",
    "    print('Motor 1 calibrated: {}'.format(m1))    \n",
    "    \n",
    "else:\n",
    "    print('Not connected')"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 5,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "\t Odrv 0_Motor 0 calibration result: False \r\n",
      " \t Odrv 0_Motor 1 calibration result: True\n",
      "odrv0 errors\n",
      "axis0\n",
      "  axis: \u001b[92;1mno error\u001b[0m\n",
      "  motor: \u001b[92;1mno error\u001b[0m\n",
      "  encoder: \u001b[92;1mno error\u001b[0m\n",
      "  controller: \u001b[92;1mno error\u001b[0m\n",
      "axis1\n",
      "  axis: \u001b[92;1mno error\u001b[0m\n",
      "  motor: \u001b[92;1mno error\u001b[0m\n",
      "  encoder: \u001b[92;1mno error\u001b[0m\n",
      "  controller: \u001b[92;1mno error\u001b[0m\n"
     ]
    }
   ],
   "source": [
    "print('\\t Odrv 0_Motor 0 calibration result: {} \\r\\n'.format(odrv0.axis0.motor.is_calibrated), \n",
    "      '\\t Odrv 0_Motor 1 calibration result: {}'.format(odrv0.axis1.motor.is_calibrated))\n",
    "\n",
    "print('odrv0 errors')\n",
    "odrive.utils.dump_errors(odrv0, True)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "curr_limit_all = 5 # amperes\n",
    "odrv0.axis0.motor.config.current_lim = curr_limit_all\n",
    "odrv0.axis1.motor.config.current_lim = curr_limit_all"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 14,
   "metadata": {},
   "outputs": [],
   "source": [
    "odrv0.axis0.requested_state = odrive.enums.AXIS_STATE_FULL_CALIBRATION_SEQUENCE\n",
    "odrv0.axis1.requested_state = odrive.enums.AXIS_STATE_FULL_CALIBRATION_SEQUENCE"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 16,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "odrv0 errors\n",
      "axis0\n",
      "  axis: \u001b[92;1mno error\u001b[0m\n",
      "  motor: \u001b[92;1mno error\u001b[0m\n",
      "  encoder: \u001b[92;1mno error\u001b[0m\n",
      "  controller: \u001b[92;1mno error\u001b[0m\n",
      "axis1\n",
      "  axis: \u001b[92;1mno error\u001b[0m\n",
      "  motor: \u001b[92;1mno error\u001b[0m\n",
      "  encoder: \u001b[92;1mno error\u001b[0m\n",
      "  controller: \u001b[92;1mno error\u001b[0m\n"
     ]
    }
   ],
   "source": [
    "print('odrv0 errors')\n",
    "odrive.utils.dump_errors(odrv0)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "%qtconsole"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 12,
   "metadata": {},
   "outputs": [],
   "source": [
    "del odrv0"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 23,
   "metadata": {},
   "outputs": [],
   "source": [
    "odrv0.axis0.requested_state = odrive.enums.AXIS_STATE_CLOSED_LOOP_CONTROL\n",
    "odrv0.axis1.requested_state = odrive.enums.AXIS_STATE_CLOSED_LOOP_CONTROL"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 24,
   "metadata": {},
   "outputs": [],
   "source": [
    "odrv0.axis0.requested_state = odrive.enums.AXIS_STATE_IDLE\n",
    "odrv0.axis1.requested_state = odrive.enums.AXIS_STATE_IDLE"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Connect to odrv1, calibrate,  dump errors"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "odrv1 = odrive.find_any(serial_number = '3081396C3235', timeout = 20)\n",
    "if odrv1 is not None:\n",
    "    print('Connected!')\n",
    "    print('Odrive serial {}'.format(odrv1.serial_number))\n",
    "    \n",
    "    m0 = odrv1.axis0.motor.is_calibrated\n",
    "    m1 = odrv1.axis1.motor.is_calibrated\n",
    "    \n",
    "    print('Motor 0 calibrated: {}'.format(m0))\n",
    "    print('Motor 1 calibrated: {}'.format(m1))    \n",
    "    \n",
    "else:\n",
    "    print('Not connected')"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "print('\\t Odrv 1_Motor 0 calibration result: {} \\r\\n'.format(odrv1.axis0.motor.is_calibrated), \n",
    "      '\\t Odrv 1_Motor 1 calibration result: {}'.format(odrv1.axis1.motor.is_calibrated))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "curr_limit_all = 20 # amperes\n",
    "odrv1.axis0.motor.config.current_lim = curr_limit_all\n",
    "odrv1.axis1.motor.config.current_lim = curr_limit_all"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "odrv1.axis0.requested_state = odrive.enums.AXIS_STATE_FULL_CALIBRATION_SEQUENCE\n",
    "odrv1.axis1.requested_state = odrive.enums.AXIS_STATE_FULL_CALIBRATION_SEQUENCE"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "print('odrv1 errors')\n",
    "odrive.utils.dump_errors(odrv1)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Connect to both odrv0 and odrv1, calibrate,  dump errors"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "odrv0 = odrive.find_any(serial_number = '3064397F3235', timeout = 20)\n",
    "odrv1 = odrive.find_any(serial_number = '3081396C3235', timeout = 20)\n",
    "if odrv0 is not None:\n",
    "    print('Connected!')\n",
    "    print('Odrive serial {}'.format(odrv0.serial_number))\n",
    "    \n",
    "    m0 = odrv0.axis0.motor.is_calibrated\n",
    "    m1 = odrv0.axis1.motor.is_calibrated\n",
    "    \n",
    "    print('Motor 0 calibrated: {}'.format(m0))\n",
    "    print('Motor 1 calibrated: {}'.format(m1))    \n",
    "    \n",
    "else:\n",
    "    print('Not connected')\n",
    "\n",
    "if odrv1 is not None:\n",
    "    print('Connected!')\n",
    "    print('Odrive serial {}'.format(odrv1.serial_number))\n",
    "    \n",
    "    m0 = odrv1.axis0.motor.is_calibrated\n",
    "    m1 = odrv1.axis1.motor.is_calibrated\n",
    "    \n",
    "    print('Motor 0 calibrated: {}'.format(m0))\n",
    "    print('Motor 1 calibrated: {}'.format(m1))    \n",
    "    \n",
    "else:\n",
    "    print('Not connected')"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## 3.2 preparation before moving motor"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "odrv0.axis0.requested_state = odrive.enums.AXIS_STATE_IDLE\n",
    "odrv0.axis1.requested_state = odrive.enums.AXIS_STATE_IDLE\n",
    "odrv1.axis0.requested_state = odrive.enums.AXIS_STATE_IDLE\n",
    "odrv1.axis1.requested_state = odrive.enums.AXIS_STATE_IDLE"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "curr_limit_all = 20 # amperes\n",
    "odrv0.axis0.motor.config.current_lim = curr_limit_all\n",
    "odrv0.axis1.motor.config.current_lim = curr_limit_all\n",
    "odrv1.axis0.motor.config.current_lim = curr_limit_all\n",
    "odrv1.axis1.motor.config.current_lim = curr_limit_all"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "odrv0.axis0.controller.config.vel_limit = 2000000 # encoder counts/sec\n",
    "odrv0.axis1.controller.config.vel_limit = 2000000 # encoder counts/sec\n",
    "odrv1.axis0.controller.config.vel_limit = 2000000 # encoder counts/sec\n",
    "odrv1.axis1.controller.config.vel_limit = 2000000 # encoder counts/sec"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Calibrate"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# calibrate odrv0\n",
    "odrv0.axis0.requested_state = odrive.enums.AXIS_STATE_FULL_CALIBRATION_SEQUENCE\n",
    "odrv0.axis1.requested_state = odrive.enums.AXIS_STATE_FULL_CALIBRATION_SEQUENCE\n",
    "\n",
    "# calibrate odrv1\n",
    "odrv1.axis0.requested_state = odrive.enums.AXIS_STATE_FULL_CALIBRATION_SEQUENCE\n",
    "odrv1.axis1.requested_state = odrive.enums.AXIS_STATE_FULL_CALIBRATION_SEQUENCE\n",
    "\n",
    "# wait for calibration to take place and then check if calibrated\n",
    "time.sleep(15)\n",
    "print('\\t Odrv 1_Motor 0 calibration result: {} \\r\\n'.format(odrv1.axis0.motor.is_calibrated), \n",
    "      '\\t Odrv 1_Motor 1 calibration result: {}'.format(odrv1.axis1.motor.is_calibrated))\n",
    "\n",
    "print('\\t Odrv 0_Motor 0 calibration result: {} \\r\\n'.format(odrv0.axis0.motor.is_calibrated), \n",
    "      '\\t Odrv 0_Motor 1 calibration result: {}'.format(odrv0.axis1.motor.is_calibrated))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "odrv0.axis0.controller.error=0\n",
    "odrv0.axis0.motor.error=0\n",
    "odrv0.axis0.encoder.error=0\n",
    "odrv0.axis0.error=0\n",
    "\n",
    "odrv0.axis1.controller.error=0\n",
    "odrv0.axis1.motor.error=0\n",
    "odrv0.axis1.encoder.error=0\n",
    "odrv0.axis1.error=0\n",
    "\n",
    "odrv1.axis0.controller.error=0\n",
    "odrv1.axis0.motor.error=0\n",
    "odrv1.axis0.encoder.error=0\n",
    "odrv1.axis0.error=0\n",
    "\n",
    "odrv1.axis1.controller.error=0\n",
    "odrv1.axis1.motor.error=0\n",
    "odrv1.axis1.encoder.error=0\n",
    "odrv1.axis1.error=0"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "print('odrv0 errors')\n",
    "odrive.utils.dump_errors(odrv0)\n",
    "print('\\nodrv1 errors')\n",
    "odrive.utils.dump_errors(odrv1)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Put the legs on NOW! (before running more code below)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "odrv0.axis0.requested_state = odrive.enums.AXIS_STATE_CLOSED_LOOP_CONTROL\n",
    "odrv0.axis1.requested_state = odrive.enums.AXIS_STATE_CLOSED_LOOP_CONTROL"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "odrv1.axis0.requested_state = odrive.enums.AXIS_STATE_CLOSED_LOOP_CONTROL\n",
    "odrv1.axis1.requested_state = odrive.enums.AXIS_STATE_CLOSED_LOOP_CONTROL"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### define angle to position function & position to angle function"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "motor_cpr0 = (odrv0.axis0.encoder.config.cpr,odrv0.axis1.encoder.config.cpr)\n",
    "pos_zero0 = ([odrv0.axis0.encoder.pos_estimate,odrv0.axis1.encoder.pos_estimate])"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# get motor parameters\n",
    "motor_cpr1 = (odrv1.axis0.encoder.config.cpr,odrv1.axis1.encoder.config.cpr)\n",
    "pos_zero1 = ([odrv1.axis0.encoder.pos_estimate,odrv1.axis1.encoder.pos_estimate])"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "def angle2posL(theta, pos_zero = pos_zero0, motor_cpr = motor_cpr0, zero_theta = (0,np.pi)):\n",
    "    pos = np.zeros(shape=(1,2))\n",
    "    pos[0,0] = -(theta[0] - zero_theta[0])/(2*np.pi)*motor_cpr[0] + pos_zero[0]\n",
    "    pos[0,1] = -(theta[1] - zero_theta[1])/(2*np.pi)*motor_cpr[1] + pos_zero[1]    \n",
    "    return (pos)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# angle to position for 2 odrives\n",
    "# pos = np.zeros(shape=(1,2))\n",
    "def angle2posR(theta, pos_zero = pos_zero1, motor_cpr = motor_cpr1, zero_theta = (0,np.pi)):\n",
    "    pos = np.zeros(shape=(1,2))\n",
    "    pos[0,0] = -(theta[0] - zero_theta[0])/(2*np.pi)*motor_cpr[0] + pos_zero[0]\n",
    "    pos[0,1] = -(theta[1] - zero_theta[1])/(2*np.pi)*motor_cpr[1] + pos_zero[1]    \n",
    "    return (pos)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "def pos2angleL(pos, motor_cpr = motor_cpr0, pos_zero = pos_zero0, zero_theta = (0,np.pi)):\n",
    "    angle = np.zeros(shape=(1,2))\n",
    "    angle[0,0] = -2*np.pi*(pos[0] - pos_zero[0])/motor_cpr[0]+zero_theta[0]\n",
    "    angle[0,1] = -2*np.pi*(pos[1] - pos_zero[1])/motor_cpr[1]+zero_theta[1]\n",
    "    return (angle)\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# position to angle for 2 odrives\n",
    "# angle = np.zeros(shape=(1,2))\n",
    "\n",
    "def pos2angleR(pos, motor_cpr = motor_cpr1, pos_zero = pos_zero1, zero_theta = (0,np.pi)):\n",
    "    angle = np.zeros(shape=(1,2))\n",
    "    angle[0,0] = -2*np.pi*(pos[0] - pos_zero[0])/motor_cpr[0]+zero_theta[0]\n",
    "    angle[0,1] = -2*np.pi*(pos[1] - pos_zero[1])/motor_cpr[1]+zero_theta[1]\n",
    "    return (angle)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## 3.3 Get legs on and start moving motors"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### move legs to start points"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "odrv0.axis0.requested_state = odrive.enums.AXIS_STATE_CLOSED_LOOP_CONTROL\n",
    "odrv0.axis1.requested_state = odrive.enums.AXIS_STATE_CLOSED_LOOP_CONTROL\n",
    "odrv1.axis0.requested_state = odrive.enums.AXIS_STATE_CLOSED_LOOP_CONTROL\n",
    "odrv1.axis1.requested_state = odrive.enums.AXIS_STATE_CLOSED_LOOP_CONTROL"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "posgain = 100\n",
    "odrv0.axis0.controller.config.pos_gain = posgain\n",
    "odrv0.axis1.controller.config.pos_gain = posgain\n",
    "odrv1.axis0.controller.config.pos_gain = posgain\n",
    "odrv1.axis1.controller.config.pos_gain = posgain"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# move left leg to on_point; ritht leg to off_point\n",
    "theta_startL = fsolve(lambda x: IK_constraint_eqn(x, x_on, y_on, l1, l2,w), (0,np.pi))\n",
    "theta_startR = fsolve(lambda x: IK_constraint_eqn(x, -x_off, y_off, l1, l2,w), (0,np.pi))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "#theta_startR = fsolve(lambda x: IK_constraint_eqn(x, -x_on, y_on, l1, l2,w), (0,np.pi))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "odrv0.axis0.controller.pos_setpoint = angle2posL(theta_startL)[0,0]\n",
    "odrv0.axis1.controller.pos_setpoint = angle2posL(theta_startL)[0,1]"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "odrv1.axis0.controller.pos_setpoint = angle2posR(theta_startR)[0,0]\n",
    "odrv1.axis1.controller.pos_setpoint = angle2posR(theta_startR)[0,1]"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# odrv1.axis0.controller.pos_setpoint = angle2posR(theta_startR)[0,0]\n",
    "# odrv1.axis1.controller.pos_setpoint = angle2posR(theta_startR)[0,1]"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### move motors use CPG"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# aL, aR represent to the angles related to the supposed coordinate system\n",
    "w_speed = 25\n",
    "A_L = 0\n",
    "A_R = 0\n",
    "delta_t = 0.005\n",
    "\n",
    "start = time.time()\n",
    "t_curr = 0\n",
    "\n",
    "#get the initial point\n",
    "real_pos_L = (odrv0.axis0.encoder.pos_estimate,odrv0.axis1.encoder.pos_estimate)\n",
    "real_pos_R = (odrv1.axis0.encoder.pos_estimate,odrv1.axis1.encoder.pos_estimate)\n",
    "    \n",
    "real_angle_L = pos2angleL(real_pos_L)\n",
    "real_angle_R = pos2angleR(real_pos_R)\n",
    "\n",
    "xL = interpolate.bisplev(real_angle_L[0,1],real_angle_L[0,0], tck_getxL)\n",
    "yL = interpolate.bisplev(real_angle_L[0,1],real_angle_L[0,0], tck_getyL)\n",
    "xR = interpolate.bisplev(real_angle_R[0,1],real_angle_R[0,0], tck_getxR)\n",
    "yR = interpolate.bisplev(real_angle_R[0,1],real_angle_R[0,0], tck_getyR)\n",
    "\n",
    "aL0 = xy2psi(xL,yL)\n",
    "aR0 = xy2psi(xR,yR)\n",
    "\n",
    "#start moving\n",
    "while(t_curr < 2.5):\n",
    "    t_curr = (time.time() - start)\n",
    "    \n",
    "    real_pos_L = (odrv0.axis0.encoder.pos_estimate,odrv0.axis1.encoder.pos_estimate)\n",
    "    real_pos_R = (odrv1.axis0.encoder.pos_estimate,odrv1.axis1.encoder.pos_estimate)\n",
    "    \n",
    "    real_angle_L = pos2angleL(real_pos_L)\n",
    "    real_angle_R = pos2angleR(real_pos_R)\n",
    "    \n",
    "    xL = interpolate.bisplev(real_angle_L[0,1],real_angle_L[0,0], tck_getxL)\n",
    "    yL = interpolate.bisplev(real_angle_L[0,1],real_angle_L[0,0], tck_getyL)\n",
    "    xR = interpolate.bisplev(real_angle_R[0,1],real_angle_R[0,0], tck_getxR)\n",
    "    yR = interpolate.bisplev(real_angle_R[0,1],real_angle_R[0,0], tck_getyR)\n",
    "#     print('xL, yL is ({},{})'.format(xL, yL))\n",
    "#     print('xR, yR is ({},{})'.format(xR, yR))\n",
    "    \n",
    "    aL = xy2psi(xL,yL)\n",
    "    aR = xy2psi(xR,yR)\n",
    "    \n",
    "    if psi_right<=aL<=psi_left:\n",
    "        delta_psi = poly_delta_st(aL)\n",
    "    elif aL>psi_left:\n",
    "        delta_psi = poly_delta_sw(aL)\n",
    "    else:\n",
    "        delta_psi = poly_delta_sw(aL+2*np.pi)\n",
    "    \n",
    "#     if aL>aR:\n",
    "#         aR = aR + sp.pi*2\n",
    "#     print('aL is {}, aR is {}'.format(np.float(aL), np.float(aR)))\n",
    "\n",
    "    aL0 = aL0 + delta_t*(w_speed+A_L*sp.sin(((aR-aL)-delta_psi)))\n",
    "    aR0 = aR0 + delta_t*(w_speed+A_R*sp.sin((delta_psi-(aR-aL))))\n",
    "    \n",
    "    aL1 = aL0 % (sp.pi*2)\n",
    "    aR1 = aR0 % (sp.pi*2)\n",
    "    \n",
    "    motor_theta_L = (interpolate.splev(np.float(aL1),tck_L0),interpolate.splev(np.float(aL1),tck_L1))\n",
    "    motor_theta_R = (interpolate.splev(np.float(aR1),tck_R0),interpolate.splev(np.float(aR1),tck_R1))\n",
    "    \n",
    "    odrv0.axis0.controller.pos_setpoint = angle2posL(motor_theta_L)[0,0]\n",
    "    odrv0.axis1.controller.pos_setpoint = angle2posL(motor_theta_L)[0,1]\n",
    "    odrv1.axis0.controller.pos_setpoint = angle2posR(motor_theta_R)[0,0]\n",
    "    odrv1.axis1.controller.pos_setpoint = angle2posR(motor_theta_R)[0,1]\n",
    "\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": []
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "odrv0.axis0.requested_state = odrive.enums.AXIS_STATE_IDLE\n",
    "odrv0.axis1.requested_state = odrive.enums.AXIS_STATE_IDLE\n",
    "odrv1.axis0.requested_state = odrive.enums.AXIS_STATE_IDLE\n",
    "odrv1.axis1.requested_state = odrive.enums.AXIS_STATE_IDLE"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "odrv0.axis0.controller.error=0\n",
    "odrv0.axis0.motor.error=0\n",
    "odrv0.axis0.encoder.error=0\n",
    "odrv0.axis0.error=0\n",
    "\n",
    "odrv0.axis1.controller.error=0\n",
    "odrv0.axis1.motor.error=0\n",
    "odrv0.axis1.encoder.error=0\n",
    "odrv0.axis1.error=0\n",
    "\n",
    "odrv1.axis0.controller.error=0\n",
    "odrv1.axis0.motor.error=0\n",
    "odrv1.axis0.encoder.error=0\n",
    "odrv1.axis0.error=0\n",
    "\n",
    "odrv1.axis1.controller.error=0\n",
    "odrv1.axis1.motor.error=0\n",
    "odrv1.axis1.encoder.error=0\n",
    "odrv1.axis1.error=0"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "%qtconsole"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "real_pos_L = (odrv0.axis0.encoder.pos_estimate,odrv0.axis1.encoder.pos_estimate)\n",
    "real_pos_R = (odrv1.axis0.encoder.pos_estimate,odrv1.axis1.encoder.pos_estimate)\n",
    "\n",
    "real_angle_L = pos2angleL(real_pos_L)\n",
    "real_angle_R = pos2angleR(real_pos_R)\n",
    "\n",
    "xL = interpolate.bisplev(real_angle_L[0,1],real_angle_L[0,0], tck_getxL)\n",
    "yL = interpolate.bisplev(real_angle_L[0,1],real_angle_L[0,0], tck_getyL)\n",
    "xR = interpolate.bisplev(real_angle_R[0,1],real_angle_R[0,0], tck_getxR)\n",
    "yR = interpolate.bisplev(real_angle_R[0,1],real_angle_R[0,0], tck_getyR)\n",
    "print('xL, yL is ({},{})'.format(xL, yL))\n",
    "print('xR, yR is ({},{})'.format(xR, yR))\n",
    "\n",
    "aL = xy2psi(xL,yL)\n",
    "aR = xy2psi(xR,yR)\n",
    "\n",
    "print('aL is {}, aR is {}'.format(np.float(aL), np.float(aR)))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "real_pos_L = (odrv0.axis0.encoder.pos_estimate,odrv0.axis1.encoder.pos_estimate)\n",
    "\n",
    "real_angle_L = pos2angleL(real_pos_L)\n",
    "print(real_pos_L)\n",
    "print('real_angle_L', real_angle_L)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# [x,y] = FK_fast.subs([(thetaL_sym, 2.6419104), (thetaR_sym, -0.09320132)])\n",
    "[x,y] = FK_lambda(-0.09320132, 2.6419104)\n",
    "print('xon', x, y)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "real_pos_L = (odrv0.axis0.encoder.pos_estimate,odrv0.axis1.encoder.pos_estimate)\n",
    "\n",
    "real_angle_L = pos2angleL(real_pos_L)\n",
    "print(real_pos_L)\n",
    "print('real_angle_L', real_angle_L)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# [x,y] = FK_fast.subs([(thetaL_sym, 3.23632795), (thetaR_sym, 0.98825514)])\n",
    "[x,y] = FK_lambda(0.98825514, 3.23632795)\n",
    "print('xoff', x, y)\n",
    "\n",
    "# xon = 0.0271340208767868\n",
    "# yon = 0.118504090025698\n",
    "# xoff = -0.0622642119510020\n",
    "# yoff = 0.138845634937132\n",
    "# xswing = -0.0239762803121994\n",
    "# yswing = 0.101106600363156"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "real_pos_L = (odrv0.axis0.encoder.pos_estimate,odrv0.axis1.encoder.pos_estimate)\n",
    "\n",
    "real_angle_L = pos2angleL(real_pos_L)\n",
    "print(real_pos_L)\n",
    "print('real_angle_L', real_angle_L)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# [x,y] = FK_fast.subs([(thetaL_sym, 3.43458298), (thetaR_sym, 0.34673958)])\n",
    "[x,y] = FK_lambda(0.34673958, 3.43458298)\n",
    "print('xswing', x, y)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "real_pos_L = (odrv0.axis0.encoder.pos_estimate,odrv0.axis1.encoder.pos_estimate)\n",
    "\n",
    "real_angle_L = pos2angleL(real_pos_L)\n",
    "print(real_pos_L)\n",
    "print('real_angle_L', real_angle_L)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# [x,y] = FK_fast.subs([(thetaL_sym, 3.63592995), (thetaR_sym, 0.20325245)])\n",
    "[x,y] = FK_lambda(0.20325245, 3.63592995)\n",
    "print('xswing', x, y)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": []
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3",
   "language": "python",
   "name": "python3"
  },
  "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.7.3"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 4
}