{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# UD04 · Notebook 4 — Cinemática de un manipulador\n",
    "\n",
    "**Requisitos**\n",
    "\n",
    "```bash\n",
    "pip install roboticstoolbox-python spatialmath-python numpy matplotlib\n",
    "```\n",
    "\n",
    "En el aula se podrá usar además un simulador 3D (Webots o CoppeliaSim), según lo que decida el\n",
    "profesor y los equipos disponibles.\n",
    "\n",
    "**Objetivo**: cargar un robot real (Panda de Franka o Puma 560), calcular su cinemática directa e\n",
    "inversa, generar una trayectoria y detectar singularidades.\n",
    "\n",
    "Es una **entrega evaluable**: se corrige con la rúbrica de su tarea en Moodle."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Requisitos\n",
    "\n",
    "La siguiente celda instala lo necesario. En Colab tarda un par de minutos la primera vez."
   ]
  },
  {
   "cell_type": "code",
   "metadata": {},
   "execution_count": null,
   "outputs": [],
   "source": [
    "%pip install roboticstoolbox-python spatialmath-python numpy matplotlib"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Fase 1 — Carga el robot\n",
    "\n",
    "Anota en el informe: número de articulaciones, tipo de cada una y el nombre del modelo."
   ]
  },
  {
   "cell_type": "code",
   "metadata": {},
   "execution_count": null,
   "outputs": [],
   "source": [
    "import numpy as np\n",
    "import roboticstoolbox as rtb\n",
    "import spatialmath.base as smb\n",
    "\n",
    "# TU CÓDIGO: carga el robot Panda e imprimelo\n",
    "robot = ...\n",
    "print(robot)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "**✏️ Respuesta**: ¿cuántas articulaciones tiene? ¿De qué tipo es cada una?\n",
    "\n",
    "*(escribe aquí)*"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Fase 2 — Cinemática directa (FK)\n",
    "\n",
    "Verifica que la posición del efector coincide con la que predice el modelo."
   ]
  },
  {
   "cell_type": "code",
   "metadata": {},
   "execution_count": null,
   "outputs": [],
   "source": [
    "q = np.array([0, -0.8, 0.8, 0, 0.8, 0, 0])\n",
    "\n",
    "# TU CÓDIGO: calcula la pose con fkine y muestra posicion y orientacion\n",
    "pose = ...\n",
    "print(\"Posición del efector:\", ...)\n",
    "print(\"Orientación (Euler, grados):\", ...)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "**✏️ Respuesta**: ¿cuántas soluciones tiene la cinemática **directa**? ¿Por qué?\n",
    "\n",
    "*(escribe aquí)*"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Fase 3 — Cinemática inversa (IK)\n",
    "\n",
    "Comprueba que al aplicar FK a la solución IK se recupera la pose original."
   ]
  },
  {
   "cell_type": "code",
   "metadata": {},
   "execution_count": null,
   "outputs": [],
   "source": [
    "# TU CÓDIGO: resuelve la IK y verifica el resultado\n",
    "sol = ...\n",
    "print(\"Solución IK:\", ...)\n",
    "print(\"¿Converge?\", ...)\n",
    "print(\"Pose reconstruida:\", ...)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "**✏️ Respuesta**: la posición reconstruida coincide, pero **los ángulos no tienen por qué ser\n",
    "los de partida**. Explica por qué.\n",
    "\n",
    "*(escribe aquí)*"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Fase 4 — Genera una trayectoria"
   ]
  },
  {
   "cell_type": "code",
   "metadata": {},
   "execution_count": null,
   "outputs": [],
   "source": [
    "from roboticstoolbox import jtraj\n",
    "\n",
    "q0 = robot.qr\n",
    "q1 = np.array([0, -0.4, 1.2, 0, 0.8, 0, 0])\n",
    "\n",
    "# TU CÓDIGO: genera la trayectoria y calcula las poses\n",
    "traj = ...\n",
    "poses = ...\n",
    "print(poses[:3])"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "**✏️ Respuesta**: `jtraj` interpola en el **espacio articular** con un polinomio de 5.º grado.\n",
    "¿Qué habría cambiado usando `ctraj`? ¿En qué caso real preferirías cada uno?\n",
    "\n",
    "*(escribe aquí)*"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Fase 5 — Detecta singularidades\n",
    "\n",
    "¿En algún punto el rango baja de 6? Si es así, anota cuál y por qué ocurre."
   ]
  },
  {
   "cell_type": "code",
   "metadata": {},
   "execution_count": null,
   "outputs": [],
   "source": [
    "# TU CÓDIGO: calcula el rango del jacobiano cada 10 pasos\n",
    "for i in range(0, 50, 10):\n",
    "    J = ...\n",
    "    rango = ...\n",
    "    print(f\"Paso {i:2d}: rango del jacobiano = {rango}\")"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "**✏️ Respuesta**: ¿baja el rango de 6 en algún punto? Si baja, ¿en cuál y por qué? Y si no\n",
    "baja, ¿qué habría que cambiar en la trayectoria para provocar una singularidad?\n",
    "\n",
    "*(escribe aquí)*"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Fase 6 — Visualiza la trayectoria"
   ]
  },
  {
   "cell_type": "code",
   "metadata": {},
   "execution_count": null,
   "outputs": [],
   "source": [
    "import matplotlib.pyplot as plt\n",
    "\n",
    "# TU CÓDIGO: extrae x, y, z de las poses y dibujalas en 3D\n",
    "x, y, z = ..., ..., ...\n",
    "\n",
    "ax = plt.figure().add_subplot(projection='3d')\n",
    "ax.plot(x, y, z, label='Trayectoria')\n",
    "ax.scatter(x[0], y[0], z[0], c='g', label='Inicio')\n",
    "ax.scatter(x[-1], y[-1], z[-1], c='r', label='Fin')\n",
    "ax.legend()\n",
    "plt.show()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Cierre\n",
    "\n",
    "**✏️ Respuesta**: en dos o tres frases, ¿qué relación hay entre lo que has hecho aquí y las\n",
    "técnicas de IA de las unidades anteriores (percepción, razonamiento, acción)?\n",
    "\n",
    "*(escribe aquí)*"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Entrega\n",
    "\n",
    "Sube el **notebook ejecutado**. Se valora que **expliques lo que ocurre**, no solo que la celda dé\n",
    "un resultado: una salida sin interpretar no demuestra que hayas entendido nada.\n",
    "\n",
    "| Fase | Evidencia mínima |\n",
    "|---|---|\n",
    "| 1 | El robot cargado, con el número y el tipo de sus articulaciones |\n",
    "| 2 | Posición y orientación del efector, y por qué la FK tiene una única solución |\n",
    "| 3 | La solución de IK **verificada** con FK, y por qué los ángulos no coinciden con los de partida |\n",
    "| 4 | La trayectoria generada y la diferencia entre `jtraj` y `ctraj` |\n",
    "| 5 | El rango del jacobiano a lo largo de la trayectoria, y qué habría que cambiar para provocar una singularidad |\n",
    "| 6 | El recorrido del efector dibujado en 3D, con inicio y fin marcados |\n",
    "\n",
    "**Soluciones**\n",
    "\n",
    "Las soluciones no se publican: se corrigen y comentan en clase."
   ]
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "name": "python",
   "version": "3.12"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 5
}