trabalho-01.ipynb

{
 "cells": [
  {
   "cell_type": "markdown",
   "id": "7d18d9b6-dca6-4f2a-9d9c-a7d2db812f14",
   "metadata": {},
   "source": [
    "# Trabalho de Programação Linear aplicada ao problema de Cinemática Inversa de Braços Robóticos\n",
    "\n",
    "Para rodar a simulação no Google Colab execute os comandos abaixo:"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "c43eafe1-68ea-4c4e-96e7-a32c33838128",
   "metadata": {},
   "source": [
    "!pip install roboticstoolbox-python\n",
    "\n",
    "!pip install ortools       "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "7026cf65-8fa2-452d-bdc5-dbbbefa898d6",
   "metadata": {},
   "outputs": [],
   "source": [
    "!pip install cvxpy"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "9dd23aaf-d4d7-439a-a53e-2faaf28810e7",
   "metadata": {},
   "source": [
    "https://github.com/petercorke/robotics-toolbox-python\n",
    "\n",
    "https://petercorke.github.io/robotics-toolbox-python/index.html"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "9f6f1407-dc5f-4051-9874-c22f442987dd",
   "metadata": {},
   "outputs": [],
   "source": [
    "%matplotlib widget\n",
    "import matplotlib.pyplot as plt"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "4933e962-14f3-4c97-9b45-b6e8ff31cb90",
   "metadata": {},
   "outputs": [],
   "source": [
    "import roboticstoolbox as rtb\n",
    "import spatialmath as sm\n",
    "import numpy as np\n",
    "from roboticstoolbox.backends.PyPlot import PyPlot"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "3a0ce26b-6366-489e-8562-074a1ab68975",
   "metadata": {},
   "outputs": [],
   "source": [
    "# Implemente o modelo de PO dentro da função solve\n",
    "import cvxpy as cvx\n",
    "\n",
    "def solve(jacobe, velocity, qd_t_1):\n",
    "    J = cvx.Parameter((6, 7), value=jacobe)\n",
    "    v = cvx.Parameter(6, value=velocity)\n",
    "    qd = cvx.Variable(7)\n",
    "    delta = cvx.Variable(7)\n",
    "    gamma = cvx.Variable(7)\n",
    "    a = b = np.ones(7)\n",
    "\n",
    "    prob = cvx.Problem(cvx.Minimize(a @ delta + b @ gamma),\n",
    "    [J @ qd == v, \n",
    "    -delta <= qd, qd <= delta, delta >= 0,\n",
    "    -gamma <= qd - qd_t_1, qd - qd_t_1 <= gamma, gamma >=0\n",
    "    ])\n",
    "\n",
    "    prob.solve()\n",
    "\n",
    "    return qd.value"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "ba4ab95d-428e-4f81-b7fc-df3f38913576",
   "metadata": {},
   "outputs": [],
   "source": [
    "# Carrega um modelo do robô Panda e define seus ângulos de junta para a configuração de prontidão\n",
    "panda = rtb.models.DH.Panda()\n",
    "panda.q = panda.qr\n",
    "\n",
    "previous_joint_velocity = np.zeros(7)\n",
    "\n",
    "current_effector_pose = panda.fkine(panda.q)\n",
    "\n",
    "# Define uma pose desejada para a garra em função da pose atual da garra\n",
    "target_effector_pose = current_effector_pose * sm.SE3.Tx(0.2) * sm.SE3.Ty(0.2) * sm.SE3.Tz(0.45)\n",
    "\n",
    "panda.plot(panda.q)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "1f0813de-c4b2-4a05-9284-5ae32d6cfada",
   "metadata": {},
   "outputs": [],
   "source": [
    "panda.q"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "5719c574-2613-45e4-884d-c46ff7b88b52",
   "metadata": {},
   "outputs": [],
   "source": [
    "env = PyPlot()\n",
    "env.launch()\n",
    "env.add(panda)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "0c089859-a7cd-4a11-9489-f4b2e5fe9c53",
   "metadata": {},
   "outputs": [],
   "source": [
    "# Simula o robô enquanto ele não alcança o objetivo\n",
    "effector_arrived = False\n",
    "while not effector_arrived:\n",
    "    current_effector_pose = panda.fkine(panda.q)\n",
    "    \n",
    "    # Calcula a velocidade necessária da garra para ir em direção ao objetivo\n",
    "    effector_velocity, effector_arrived = rtb.p_servo(current_effector_pose, target_effector_pose, 1)\n",
    "    \n",
    "    J = panda.jacobe(panda.q)\n",
    "\n",
    "    # Calcula as velocidades das juntas do Panda conforme Eq. 8.3 do livro do Peter Corke (pag. 234)\n",
    "    # current_joint_velocity = np.linalg.pinv(J) @ effector_velocity\n",
    "    \n",
    "    current_joint_velocity = solve(J, effector_velocity, previous_joint_velocity)\n",
    "    \n",
    "    previous_joint_velocity = current_joint_velocity\n",
    "    \n",
    "    # Defina as velocidades das juntas do Panda\n",
    "    panda.qd = current_joint_velocity\n",
    "    \n",
    "    env.step(0.05)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "0731299d-df34-42cc-ac99-65ed41df51a4",
   "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.9.4"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 5
}