jupyter-notebooks/aula07-locomocao-modelos-cinematicos.ipynb

{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": 18,
   "metadata": {},
   "outputs": [],
   "source": [
    "try:\n",
    "    import sim\n",
    "except:\n",
    "    print ('--------------------------------------------------------------')\n",
    "    print ('\"sim.py\" could not be imported. This means very probably that')\n",
    "    print ('either \"sim.py\" or the remoteApi library could not be found.')\n",
    "    print ('Make sure both are in the same folder as this file,')\n",
    "    print ('or appropriately adjust the file \"sim.py\"')\n",
    "    print ('--------------------------------------------------------------')\n",
    "    print ('')\n",
    "  \n",
    "\n",
    "import time\n",
    "import numpy as np\n",
    "\n",
    "def Rz(theta):\n",
    "  \n",
    "    return np.array([[ np.cos(theta), -np.sin(theta), 0 ],\n",
    "                      [ np.sin(theta), np.cos(theta) , 0 ],\n",
    "                      [ 0            , 0             , 1 ]])"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Holonômico (robotino)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 24,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Program started\n",
      "Connected to remote API server\n",
      "==>  15.000776290893555 [ 2.88009644e-05 -9.99863434e-01] 0.014843940734821792\n",
      "Pos:  [0.1772177666425705, -2.0062546730041504, 0.038924433290958405]\n",
      "Ori:  [-9.93319898e-05  1.48095944e-02 -5.26440352e-01]\n",
      "Program ended\n"
     ]
    }
   ],
   "source": [
    "####################################################################################\n",
    "#                                                                                  #\n",
    "#                 LEMBRE-SE QUE A SIMULAÇÃO DEVE ESTAR EM EXECUÇÃO!                #\n",
    "#                                                                                  #\n",
    "####################################################################################\n",
    "\n",
    "print ('Program started')\n",
    "sim.simxFinish(-1) # just in case, close all opened connections\n",
    "clientID=sim.simxStart('127.0.0.1',19999,True,True,5000,5) # Connect to CoppeliaSim\n",
    "\n",
    "if clientID!=-1:\n",
    "    print ('Connected to remote API server')\n",
    "\n",
    "    robotname = 'robotino'\n",
    "    returnCode, robotHandle = sim.simxGetObjectHandle(clientID, robotname, sim.simx_opmode_oneshot_wait)     \n",
    "                 \n",
    "    returnCode, wheel1 = sim.simxGetObjectHandle(clientID, 'wheel0_joint', sim.simx_opmode_oneshot_wait)\n",
    "    returnCode, wheel2 = sim.simxGetObjectHandle(clientID, 'wheel1_joint', sim.simx_opmode_oneshot_wait)\n",
    "    returnCode, wheel3 = sim.simxGetObjectHandle(clientID, 'wheel2_joint', sim.simx_opmode_oneshot_wait)\n",
    "           \n",
    "    # Robotino\n",
    "    L = 0.135   # Metros\n",
    "    r = 0.040   # Metros\n",
    "               \n",
    "    # Cinemática Direta\n",
    "    Mdir = np.array([[-r/np.sqrt(3), 0, r/np.sqrt(3)], [r/3, (-2*r)/3, r/3], [r/(3*L), r/(3*L), r/(3*L)]])\n",
    "                       \n",
    "    q = np.array([0, 0, 0])\n",
    "    \n",
    "    # Lembrar de habilitar o 'Real-time mode'\n",
    "    t = 0\n",
    "    lastTime = time.time()\n",
    "    while t <= 15:\n",
    "        \n",
    "        now = time.time()\n",
    "        dt = now - lastTime\n",
    "        \n",
    "        # Cinemática Direta\n",
    "        # u = [np.deg2rad(45), np.deg2rad(0), np.deg2rad(-45)]\n",
    "        \n",
    "        # Velocidade desejada (x, y, w)    \n",
    "        if t <= 5:\n",
    "            qdot = np.array([.2, 0, np.deg2rad(10)])\n",
    "        elif t <= 10:    \n",
    "            qdot = np.array([0, -.2, np.deg2rad(-10)])\n",
    "        else:\n",
    "            qdot = np.array([-.2, 0, 0])\n",
    "        \n",
    "        #qdot = np.array([0, .3, 0])\n",
    "        \n",
    "        # Cinemática Inversa\n",
    "        # w1, w2, w3\n",
    "        Minv = np.linalg.inv(Rz(q[2]) @ Mdir)\n",
    "        u = Minv @ qdot\n",
    "\n",
    "        # Enviando velocidades\n",
    "        sim.simxSetJointTargetVelocity(clientID, wheel1, u[0], sim.simx_opmode_streaming)\n",
    "        sim.simxSetJointTargetVelocity(clientID, wheel2, u[1], sim.simx_opmode_streaming)\n",
    "        sim.simxSetJointTargetVelocity(clientID, wheel3, u[2], sim.simx_opmode_streaming)      \n",
    "        \n",
    "        q = q + (Rz(q[2]) @ Mdir @ u)*dt\n",
    "        \n",
    "        t = t + dt        \n",
    "        lastTime = now        \n",
    "        \n",
    "    print('==> ', t, q[:2], np.rad2deg(q[2]))\n",
    "    \n",
    "    returnCode, pos = sim.simxGetObjectPosition(clientID, robotHandle, -1, sim.simx_opmode_oneshot_wait)        \n",
    "    print('Pos: ', pos)\n",
    "\n",
    "    returnCode, ori = sim.simxGetObjectOrientation(clientID, robotHandle, -1, sim.simx_opmode_oneshot_wait)\n",
    "    print('Ori: ', np.rad2deg(ori))\n",
    "        \n",
    "    sim.simxSetJointTargetVelocity(clientID, wheel1, 0, sim.simx_opmode_oneshot_wait)\n",
    "    sim.simxSetJointTargetVelocity(clientID, wheel2, 0, sim.simx_opmode_oneshot_wait)\n",
    "    sim.simxSetJointTargetVelocity(clientID, wheel3, 0, sim.simx_opmode_oneshot_wait)\n",
    "        \n",
    "    # Now close the connection to CoppeliaSim:\n",
    "    sim.simxFinish(clientID)\n",
    "    \n",
    "else:\n",
    "    print ('Failed connecting to remote API server')\n",
    "    \n",
    "print ('Program ended')"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Não-holonômico (Pioneer - diferencial)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 27,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Program started\n",
      "Connected to remote API server\n",
      "==>  10.000176191329956 [-4.00007048  0.        ] 0.0\n",
      "Pos:  [-0.08509187400341034, -1.576438546180725, 0.13864625990390778]\n",
      "Ori:  [ 0.09061561 -0.07323384 45.73659959]\n",
      "Program ended\n"
     ]
    }
   ],
   "source": [
    "####################################################################################\n",
    "#                                                                                  #\n",
    "#                 LEMBRE-SE QUE A SIMULAÇÃO DEVE ESTAR EM EXECUÇÃO!                #\n",
    "#                                                                                  #\n",
    "####################################################################################\n",
    "\n",
    "print ('Program started')\n",
    "sim.simxFinish(-1) # just in case, close all opened connections\n",
    "clientID=sim.simxStart('127.0.0.1',19999,True,True,5000,5) # Connect to CoppeliaSim\n",
    "\n",
    "if clientID!=-1:\n",
    "    print ('Connected to remote API server')\n",
    "\n",
    "    robotname = 'Pioneer_p3dx'\n",
    "    returnCode, robotHandle = sim.simxGetObjectHandle(clientID, robotname, sim.simx_opmode_oneshot_wait)     \n",
    "        \n",
    "    returnCode, l_wheel = sim.simxGetObjectHandle(clientID, robotname + '_leftMotor', sim.simx_opmode_oneshot_wait)\n",
    "    returnCode, r_wheel = sim.simxGetObjectHandle(clientID, robotname + '_rightMotor', sim.simx_opmode_oneshot_wait)    \n",
    "           \n",
    "    # Específico do robô\n",
    "    # https://www.generationrobots.com/media/Pioneer3DX-P3DX-RevA.pdf\n",
    "    # L = 0.381   # Metros\n",
    "    # r = 0.0975  # Metros\n",
    "    \n",
    "    L = 0.331\n",
    "    r = 0.09751\n",
    "    \n",
    "    # Velocidade desejada (linear, angular)\n",
    "    v = .3\n",
    "    w = np.deg2rad(10)\n",
    "\n",
    "    # Cinemática Inversa\n",
    "    wr = ((2.0*v) + (w*L))/(2.0*r)\n",
    "    wl = ((2.0*v) - (w*L))/(2.0*r)    \n",
    "    u = np.array([wr, wl])\n",
    "\n",
    "    # Enviando velocidades\n",
    "    sim.simxSetJointTargetVelocity(clientID, l_wheel, wl, sim.simx_opmode_oneshot_wait)\n",
    "    sim.simxSetJointTargetVelocity(clientID, r_wheel, wr, sim.simx_opmode_oneshot_wait)\n",
    "    \n",
    "    q = np.array([0, 0, 0])\n",
    "    \n",
    "    t = 0\n",
    "    # Lembrar de habilitar o 'Real-time mode'\n",
    "    startTime=time.time()\n",
    "    lastTime = startTime\n",
    "    while t < 10:\n",
    "        \n",
    "        now = time.time()\n",
    "        dt = now - lastTime\n",
    "\n",
    "        # Cinemática Direta\n",
    "        Mdir = np.array([[r*np.cos(q[2])/2, r*np.cos(q[2])/2], [r*np.sin(q[2])/2, r*np.sin(q[2])/2], [r/L, -r/L]])\n",
    "        q = q + (Mdir @ u)*dt\n",
    "        \n",
    "        t = t + dt        \n",
    "        lastTime = now\n",
    "    \n",
    "    print('==> ', t, q[:2], np.rad2deg(q[2]))\n",
    "    \n",
    "    returnCode, pos = sim.simxGetObjectPosition(clientID, robotHandle, -1, sim.simx_opmode_oneshot_wait)        \n",
    "    print('Pos: ', pos)\n",
    "\n",
    "    returnCode, ori = sim.simxGetObjectOrientation(clientID, robotHandle, -1, sim.simx_opmode_oneshot_wait)\n",
    "    print('Ori: ', np.rad2deg(ori))    \n",
    "    \n",
    "    sim.simxSetJointTargetVelocity(clientID, r_wheel, 0, sim.simx_opmode_oneshot_wait)\n",
    "    sim.simxSetJointTargetVelocity(clientID, l_wheel, 0, sim.simx_opmode_oneshot_wait)\n",
    "        \n",
    "    # Now close the connection to CoppeliaSim:\n",
    "    sim.simxFinish(clientID)\n",
    "    \n",
    "else:\n",
    "    print ('Failed connecting to remote API server')\n",
    "    \n",
    "print ('Program ended')"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Para praticar\n",
    "\n",
    "- Tentar utilizar o modelo do youBot (Kuka) e fazer uma navegação/controle simples\n",
    "- Tentar utilizar um modelo do tipo Ackermann (e.g., Manta) e fazer uma navegação/controle simples"
   ]
  }
 ],
 "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.8.5"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 4
}