{
 "cells": [
  {
   "cell_type": "markdown",
   "id": "94bf0959",
   "metadata": {},
   "source": [
    "# LGP-1: First Mini Interface\n",
    "\n",
    "**Disclaimer:** The algorithm interfaced here is absolutely not made or tuned for performance. The purpose of this first interface less to provide the algorithm, but rather to think about:\n",
    "1. what are interfaces to *specify* a TAMP problem (see TAMP_Provider, and Logic2KOMO_Translator below), and\n",
    "2. what are basic computational components that an underlying kinematics/optimization engine should provide, so that a good algorithm could be build on top.\n",
    "\n",
    "In the current version, the LGP tool needs three things as input:\n",
    "* a configuration,\n",
    "* a TAMP_Provider, which provides a method 'getNewPlan', which returns a next possible sequence of logical actions (action=string tuple),\n",
    "* a Logic2KOMO_Translator, which provides a method 'add_action_constraints', which adds KOMO constraints for any action.\n",
    "\n",
    "Eventually, both should be specified by the user in python. For the TAMP_Provider, the user should use plain PDDL and a planner to provide more and more plans. For the Logic2KOMO_Translater, the user can define any translation of logic predicates to geometric constraints - beyond just pick'n'place or push. But for now, for development, I'm using intransparent C++ implementations of these classes to provide defaults."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "a13d9bd0",
   "metadata": {},
   "outputs": [],
   "source": [
    "import robotic as ry\n",
    "ry.compiled()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "caf624e0",
   "metadata": {},
   "outputs": [],
   "source": [
    "problem = 'data/lgp_single_pnp'\n",
    "#problem = 'data/lgp_bimanual_pnp'\n",
    "\n",
    "C = ry.Config()\n",
    "C.addFile(problem+'.g') # change the config geometry (including size of trays) to check if solver still works\n",
    "#C.view() #or C.watchFile('data/lgp_single_pnp.g') #to edit"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "36e43d13-d1d5-4df5-8238-cf187474a6ff",
   "metadata": {},
   "outputs": [],
   "source": [
    "trans = ry.default_Actions2KOMO_Translator()\n",
    "tamp = ry.default_TAMP_Provider(C, problem+'.lgp') # change the logic goal ('terminal') to check is solver still works\n",
    "lgp = ry.LGP_Tool(C, tamp, trans)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "98949c6c-698d-44bc-b948-9e8a69007bf6",
   "metadata": {},
   "outputs": [],
   "source": [
    "for k in range(5):\n",
    "    lgp.solve(0)\n",
    "    plan = lgp.getSolvedPlan()\n",
    "    print(f'** solution {k}: {plan}', flush=True)\n",
    "    lgp.view_solved(pause=False) #change to inspect each plan\n",
    "    \n",
    "    for k in range(len(plan)):\n",
    "        piece = lgp.get_piecewiseMotionProblem(k, True)\n",
    "        ret = ry.NLP_Solver(piece.nlp(), verbose=0 ) .solve()\n",
    "        piece.set_viewer(C.get_viewer()) # to prevent too many windows popping up\n",
    "        piece.view_play(False, f'PIECE solution {k} {plan[k]}')\n",
    "        \n",
    "    path = lgp.get_fullMotionProblem(True)\n",
    "    ret = ry.NLP_Solver(path.nlp(), verbose=0 ) .solve()\n",
    "    path.set_viewer(C.get_viewer()) # to prevent too many windows popping up\n",
    "    path.view_play(False, f'FULL PATH solution {plan}')\n",
    "\n",
    "lgp.view_close()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "3f67eeb3",
   "metadata": {},
   "outputs": [],
   "source": [
    "del C"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "0b4156f4-f6a0-4c2b-87cf-3c817e44ce36",
   "metadata": {},
   "outputs": [],
   "source": []
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "venv",
   "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.12.3"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 5
}