{
 "cells": [
  {
   "cell_type": "markdown",
   "id": "89f8bc99",
   "metadata": {},
   "source": [
    "# Config-3: Importing, editing & manipulating them\n",
    "* This scripts loads a mini scene defined in 'mini.g'.\n",
    "* The 'watchFile' command allows you to edit the file in some other editor, while it is being redisplayed whenever you save.\n",
    "* 'set/getJointState' shows how to access degrees-of-freedom of the scene\n",
    "* 'eval' shows how to access other features, including its Jacobian"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "0b373788",
   "metadata": {},
   "source": [
    "## Importing robot models\n",
    "Let's first have a look at robot models that are already installed with rai. (They are defined in the `rai-robotModels` github repo, which is partly copied into the `.../site-packages/robotic/rai-robotModels` install path.)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "c2135256",
   "metadata": {},
   "outputs": [],
   "source": [
    "import robotic as ry"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "294c17f2",
   "metadata": {},
   "outputs": [],
   "source": [
    "print('The path where model files are pre-installed:\\n', ry.raiPath(''))\n",
    "# you could overwrite this with:\n",
    "# ry.setRaiPath('your_path/rai-robotModels/')"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "90cefb18",
   "metadata": {},
   "outputs": [],
   "source": [
    "C = ry.Config()\n",
    "C.addFile(ry.raiPath('panda/panda.g')) .setPosition([1.,.0,.0]) .setQuaternion([1,0,0,1])\n",
    "C.addFile(ry.raiPath('panda/panda.g'), 'r_') .setPosition([1.5,.0,.0]) .setQuaternion([1,0,0,1])\n",
    "C.addFile(ry.raiPath('robotiq/robotiq.g')) .setParent(C.getFrame('panda_joint7')) .setRelativePosition([0., 0., .15])\n",
    "C.addFile(ry.raiPath('g1/g1.g')) .setPosition([.5, 0., .8]) .setQuaternion([1,0,0,1])\n",
    "C.addFile(ry.raiPath('pr2/pr2.g')) .setPosition([-.5,.0,.0])\n",
    "C.addFile(ry.raiPath('ur10/ur10.g')) .setPosition([-1.5,.0,.5])\n",
    "C.view()"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "37c8573c",
   "metadata": {},
   "source": [
    "The `addFile` returns the *first* frame defined in the file, which typically is the base frame of the whole robot. Therefore, by setting it's position, you can move the whole loaded robot.\n",
    "\n",
    "Sometimes you want to add a model twice, but avoid duplicated frame names. With `addFile` you can specify a `prefix` string (here `r_`) which add that prefix to all frame names of the added robot. This is exemplified with the second panda added here.\n",
    "\n",
    "The Robotiq example also shows that this base frame can be made a child of other frames -- attaching the robotiq to the panda arm. (Here in addition to the existing gripper, which would first have to be removed using `C.delFrame`.)\n",
    "\n",
    "The following is a simple helper to articulare all dofs in the display (press `q` to interrupt):"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "58cdcbf3",
   "metadata": {},
   "outputs": [],
   "source": [
    "#C.animate()\n",
    "C.animateSpline(5)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "a8b99d2e",
   "metadata": {},
   "outputs": [],
   "source": [
    "del C"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "d7badd49",
   "metadata": {},
   "source": [
    "## Creating a grid of duplicated configurations\n",
    "It's become populat to setup simulations to include a grid of copies of the scene. Here how to do this:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "2910dcf4",
   "metadata": {},
   "outputs": [],
   "source": [
    "import robotic as ry"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "0afbc019",
   "metadata": {},
   "outputs": [],
   "source": [
    "C = ry.Config()\n",
    "table = C.addFrame('table') .setShape(ry.ST.ssBox,[.5, .5, .1, .01]) .setPosition([0,0,1]) .setColor([.2])\n",
    "panda = C.addFile(ry.raiPath('panda/panda.g'))\n",
    "panda.setParent(table) .setRelativePosition([0,0,.05])\n",
    "C.view()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "c590dfbf",
   "metadata": {},
   "outputs": [],
   "source": [
    "Cgrid = ry.Config()\n",
    "d = 6\n",
    "for x in range(d):\n",
    "    for y in range(d):\n",
    "        base = Cgrid.addConfigurationCopy(C, f'x{x}_y{y}_') #the string adds a prefix to all frame names\n",
    "        base.setPosition([x-(d-1)/2,y-(d-1)/2,1])\n",
    "print('#frame in grid:', Cgrid.getFrameDimension())\n",
    "print('1st frame\\'s name:', Cgrid.getFrames()[0].name)\n",
    "Cgrid.view()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "7ba346e5",
   "metadata": {},
   "outputs": [],
   "source": [
    "Cgrid.animateSpline()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "d41d10a4",
   "metadata": {},
   "outputs": [],
   "source": [
    "del Cgrid"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "7dd18584",
   "metadata": {},
   "source": [
    "## Conversion from URDF\n",
    "\n",
    "The python package should install a script `urdf2rai.py` in `.local/bin`. That converts a urdf-file to a .g-file. For instance, place a `ur10.urdf` and the `ur_description` folder into the same folder and run:\n",
    "```\n",
    "urdf2rai.py ur10.urdf > ur10_org.g\n",
    "```\n",
    "That should generate a `ur10_org.g` file that can be displayed with `kinEdit ur10_org.g`. Note, by default that **excludes** exporting collision shapes (as I typically replace them, e.g. by the convex hull of the visual shapes). The `-coll 1` option of the script should include also the collision shapes. The `rai-robotModels` repository README provides more info on conversion from URDF, as well as specific examples for those models included in the package."
   ]
  },
  {
   "cell_type": "markdown",
   "id": "a2ff2eaf",
   "metadata": {},
   "source": [
    "## Configuration file (.g-file) syntax\n",
    "\n",
    "The [Appendix:Yaml-Graph Files](.../yaml-graph-files) provides a more detailed description of the underlying syntax of the .g-files. While the syntax should be yaml compatible, the fact that node names use `(<parents>)` to define the parent(s) of a node, filenames should be strings `<filename>`, and the additional `Include`, `Edit`, and `Delete` tags extend the semantics of what can be define in such a file.\n",
    "\n",
    "In essence, to describe a robot/world configuration, every node describes a\n",
    "frame, and is described by three things: `<frame-name> ( <parent> ) { <attributes> }`, where ``<parent>`` needs to be a previously defined frame, or omitted, if\n",
    "the frame is a root frame. The attributes define properties of the\n",
    "frame, such as its pose (`X`), relative transformation (`Q`), shape, joint, and inertia properties. The main interpreted attributes are the following. Internally, a few more attributes are interpreted in experimental code (also your python code can retrieve attributes via `f.info()`).\n",
    "\n",
    "Frame: `X Q`\n",
    "\n",
    "Shape: `shape size contact mesh meshscale core sdf color texture`\n",
    "\n",
    "Joint: `joint joint_scale q Q limits ctrl_limits ctrl_H sampleUniform joint_active mimic`\n",
    "\n",
    "Inertia: `mass inertia`"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "569fbbb1",
   "metadata": {},
   "source": [
    "## Interactive Editing\n",
    "\n",
    "When developing your own model (robot or scene), you could of course just use the python commands `addFrame` etc. But that might be cumbersome and not interactive enough. The .g-file format is fairly easy to edit. To help doing this more interactively, there is the `watchFile` method:\n",
    "\n",
    "Open the file you want to edit (here `mini.g`) in any editor. At the same time, open it from within python and display it using `watchFile`. The key benefit is that `watchFile` reloads and re-displays the file whenever it is externally modified (it watches the file's inode). That enables interactive editing. It also allows you to return information on objects you point on (their name and attributes), which helps a lot to inspect models. It also reports on collisions in the configuration.\n",
    "\n",
    "A `ry-view` command line script is shipped with the python package, which simply calls `watchFile`. Editing from command line with this script makes it easier to see the info and error messages, e.g. when you get the syntax wrong. Jupyter is problematic, as it holds info messages back -- still, here an example directly from within python:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "0c2bd1ab",
   "metadata": {},
   "outputs": [],
   "source": [
    "import robotic as ry"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "32f64861",
   "metadata": {},
   "outputs": [],
   "source": [
    "C = ry.Config()\n",
    "C.watchFile('mini.g')"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "d9a162b7",
   "metadata": {},
   "outputs": [],
   "source": [
    "del C"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "a2eaed4e",
   "metadata": {},
   "source": [
    "## How to attach frames - faking grasps\n",
    "Note, this is not real grasping. Just editing the kinematic tree in your configuration"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "4e64cc63",
   "metadata": {},
   "outputs": [],
   "source": [
    "import robotic as ry\n",
    "import numpy as np\n",
    "import time"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "023feac6",
   "metadata": {},
   "outputs": [],
   "source": [
    "C = ry.Config()\n",
    "C.addFile(ry.raiPath('scenarios/pandasTable.g'))\n",
    "C.view()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "9dfde73a",
   "metadata": {},
   "outputs": [],
   "source": [
    "C.attach(\"l_gripper\", \"r_gripper\")"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "b7da474c",
   "metadata": {},
   "outputs": [],
   "source": [
    "#move a bit around\n",
    "\n",
    "q = C.getJointState()\n",
    "\n",
    "for t in range(30):\n",
    "    q[0] = np.sin(t/10)\n",
    "    \n",
    "    C.setJointState(q)\n",
    "    C.view()\n",
    "    time.sleep(0.1)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "2f8d26f3",
   "metadata": {},
   "outputs": [],
   "source": [
    "del C"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "67072a61",
   "metadata": {},
   "source": [
    "## Advanced: YAML and dict representations\n",
    "\n",
    "The following shows that configuration files are essentially YAML files with some special semantics. We can load the configuration description as a dict using yaml as follows:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "6fa89f59",
   "metadata": {},
   "outputs": [],
   "source": [
    "import yaml\n",
    "\n",
    "with open(ry.raiPath('panda/panda_arm_hand_conv.yml'), 'r', encoding='utf-8') as fil:\n",
    "    model = yaml.safe_load(fil)\n",
    "\n",
    "print(model)"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "8a8c6899",
   "metadata": {},
   "source": [
    "This dict contains all information about the configuration."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "899dfb5e",
   "metadata": {},
   "outputs": [],
   "source": []
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3 (ipykernel)",
   "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
}