tutorials/1-basics.ipynb

{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Tutorial 1\n",
    "\n",
    "other doc sources:\n",
    "https://github.com/MarcToussaint/rai-maintenance/tree/master/help"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 1,
   "metadata": {},
   "outputs": [],
   "source": [
    "import sys\n",
    "sys.path.append('../build')\n",
    "import numpy as np\n",
    "import libry as ry"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Setting up a basic Config\n",
    "\n",
    "The starting point is to create a `Configuration`."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2,
   "metadata": {},
   "outputs": [],
   "source": [
    "C = ry.Config()\n",
    "D = C.view()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "This shows an empty configuration. Tip: Make the view window appear \"Always On Top\" (right click on the window bar)\n",
    "\n",
    "You can add things (objects, scene models, robots) to a configuration."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 3,
   "metadata": {},
   "outputs": [],
   "source": [
    "C.clear()\n",
    "C.addFile('../rai-robotModels/pr2/pr2.g')\n",
    "C.addFile('../rai-robotModels/objects/kitchen.g')"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Note that the view was updated automatically. (Internally, the view 'subscribes' to updates of the shared memory of Config)."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 4,
   "metadata": {},
   "outputs": [],
   "source": [
    "ball = C.addFrame(name=\"ball\")\n",
    "ball.setShape(ry.ST.sphere, [.1])\n",
    "ball.setPosition([.8,.8,1.5])\n",
    "ball.setColor([1,1,0])"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "One can also add convex meshes (just passing the vertex array), or use sphere-swept convex meshes (ssBox, capsule, sphere, etc)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 5,
   "metadata": {},
   "outputs": [],
   "source": [
    "hand = C.addFrame(name=\"hand\", parent=\"pr2L\")\n",
    "hand.setShape(ry.ST.ssBox, size=[.2,.2,.1,.02]),\n",
    "hand.setRelativePosition([0,0,-.1])\n",
    "hand.setColor([1,1,0])"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "In this last example, the new object has another frame (pr2L) as *parent*. This means that it is permanently attached to this parent. pos and quat/rot are interpreted relative to the parent."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Joint and Frame State\n",
    "\n",
    "A configuration is a tree of n frames. Every frame has a pose (position & quaternion), which is represented as a 7D vector (x,y,z, qw,qx,qy,qz). The frame state is the $n\\times 7$ matrix, where the i-th row is the pose of the i-th frame.\n",
    "\n",
    "A configuration also defines joints, which means that the relative transfromation from a parent to a child frame is parameterized by degrees-of-freedoms (DOFs). If the configuration has in total n DOFs, the joint state is a n-dimensional vector.\n",
    "\n",
    "Setting the joint state implies computing all relative transformations, and then forward chaining all transformations to compute all frame poses. So setting the joint state also sets the frame state.\n",
    "     \n",
    "Setting the frame state allows you to set frame poses that are inconsistent/impossible w.r.t. the joints! Setting the frame state implies computing all relative transformations from the frame poses, and then assigning the joint state to the *projection* onto the actual DOFs"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 6,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "joint names:  ['worldTranslationRotation:0', 'worldTranslationRotation:1', 'worldTranslationRotation:2', 'torso_lift_joint', 'head_pan_joint', 'laser_tilt_mount_joint', 'r_shoulder_pan_joint', 'l_shoulder_pan_joint', 'head_tilt_joint', 'r_shoulder_lift_joint', 'l_shoulder_lift_joint', 'r_upper_arm_roll_joint', 'l_upper_arm_roll_joint', 'r_elbow_flex_joint', 'l_elbow_flex_joint', 'r_forearm_roll_joint', 'l_forearm_roll_joint', 'r_wrist_flex_joint', 'l_wrist_flex_joint', 'r_wrist_roll_joint', 'l_wrist_roll_joint', 'r_gripper_l_finger_joint', 'l_gripper_l_finger_joint', 'r_gripper_joint', 'l_gripper_joint']\n",
      "joint state:  [ 0.          0.          1.57079633  0.1         0.          0.         -1.\n",
      "  1.          0.4         0.5         0.5        -1.          1.         -2.\n",
      " -2.         -1.5         1.5        -0.5        -0.5        -0.5         0.5\n",
      "  0.1         0.1         0.01        0.01      ]\n"
     ]
    }
   ],
   "source": [
    "q = C.getJointState()\n",
    "print('joint names: ', C.getJointNames() )\n",
    "print('joint state: ', q)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Let's move the configuration by adding to the joint configuration"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 7,
   "metadata": {},
   "outputs": [],
   "source": [
    "q[2] = q[2] + 1.\n",
    "C.setJointState(q)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "The *frame state* is a $n\\times 7$ matrix, which contains for all of $n$ frames the 7D pose. A pose is stored as [p_x, p_y, p_z, q_w, q_x, q_y, q_z], with position p and quaternion q."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 8,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "frame state:  [[ 0.          0.          0.         ...,  0.          0.          0.        ]\n",
      " [ 0.          0.          0.         ...,  0.          0.          0.95954963]\n",
      " [ 0.04207349 -0.02701512  0.89067506 ...,  0.67850426 -0.19907857\n",
      "   0.67850426]\n",
      " ..., \n",
      " [ 0.5         2.          0.4        ...,  0.          0.          0.        ]\n",
      " [ 0.8         0.8         1.5        ...,  0.          0.          0.        ]\n",
      " [-0.5577233   0.17550615  0.96712056 ...,  0.10678927 -0.76993463\n",
      "   0.62796173]]\n"
     ]
    }
   ],
   "source": [
    "X0 = C.getFrameState()\n",
    "print('frame state: ', X0)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Let's do a questionable thing: adding .1 to all numbers in the frame matrix!"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 9,
   "metadata": {},
   "outputs": [],
   "source": [
    "X = X0 + .1\n",
    "C.setFrameState(X)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "The rows of X have non-normalized quaternions! These are normalized when setting the frame state.\n",
    "\n",
    "Also, the frame poses are now *inconsistent* to the joint constraints! We can read out the projected joint state, set the joint state, and get a consistent state again:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 10,
   "metadata": {},
   "outputs": [],
   "source": [
    "C.setJointState( C.getJointState() )"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Let's bring the configuration back into the state before the harsh *setFrame*"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 11,
   "metadata": {},
   "outputs": [],
   "source": [
    "C.setFrameState(X0)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    " ## Selecting joints\n",
    "\n",
    "Often one would like to choose which joints are actually active, that is, which joints are referred to in q. This allows one to sub-select joints and work only with projections of the full configuration state. This changes the joint state dimensionality, including ordering of entries in q.\n",
    "\n",
    "The frame state is not affected by such a selection of active joints."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 12,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "joint state:  [ 0.          0.          2.57079633  1.00001     0.50001     1.00001\n",
      " -2.00001     1.50001    -0.50001     0.50001   ]\n",
      "joint names:  ['worldTranslationRotation:0', 'worldTranslationRotation:1', 'worldTranslationRotation:2', 'l_shoulder_pan_joint', 'l_shoulder_lift_joint', 'l_upper_arm_roll_joint', 'l_elbow_flex_joint', 'l_forearm_roll_joint', 'l_wrist_flex_joint', 'l_wrist_roll_joint']\n"
     ]
    }
   ],
   "source": [
    "C.selectJointsByTag([\"armL\",\"base\"])\n",
    "q = C.getJointState()\n",
    "print('joint state: ', q)\n",
    "print('joint names: ', C.getJointNames() )"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Features & Jacobians\n",
    "\n",
    "A core part of rai defines features over configurations. A feature is a differentiable mapping from a configuration (or set of configurations) to a vector. Starndard features are \"position-of-endeffector-X\" or \"distance/penetration-between-convex-shapes-A-and-B\", etc. But there are many, many more features defined in rai, like error of Newton-Euler-equations for an object, total energy of the system, etc. Defining differentiable features is the core of many functionalities in the rai code.\n",
    "\n",
    "Let's define a basic feature over C: the 3D (world coordinate) position of pr2L (left hand)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 13,
   "metadata": {},
   "outputs": [],
   "source": [
    "F = C.feature(ry.FS.position, [\"pr2L\"])"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "We can now evaluate the feature, and also get the Jacobian:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 14,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Default-0-pos-pr2L-WORLD\n",
      "hand position = [-0.55020428  0.07799091  0.9462799 ]\n",
      "Jacobian = [[ 1.          0.         -0.22810087 -0.26435811  0.1113553   0.04779716\n",
      "  -0.48205648 -0.3349538  -0.02817083 -0.07290414]\n",
      " [ 0.          1.         -0.7156067  -0.49224813  0.16348229  0.12079695\n",
      "   0.004211   -0.02652025 -0.04521884 -0.0025984 ]\n",
      " [ 0.          0.         -0.07364761 -0.00287158 -0.0955967  -0.3185536\n",
      "  -0.16469427 -0.06076412  0.20142233 -0.01414623]]\n"
     ]
    }
   ],
   "source": [
    "print(F.description(C))\n",
    "\n",
    "[y,J] = F.eval(C)\n",
    "print('hand position =', y)\n",
    "print('Jacobian =', J)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Another example"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 15,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "PairCollision-hand-ball\n"
     ]
    }
   ],
   "source": [
    "F2 = C.feature(ry.FS.distance, [\"hand\", \"ball\"])\n",
    "print(F2.description(C))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 16,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "(array([-1.35583907]),\n",
       " array([[ 0.85716505,  0.40723341, -0.58443063, -0.49809416,  0.13408342,\n",
       "         -0.00325706, -0.54509974, -0.36514203,  0.00672857, -0.06356977]]))"
      ]
     },
     "execution_count": 16,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "F2.eval(C)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "When you call a feature on a *tuple* of configurations, by default it computes the difference, acceleration, etc, w.r.t. these configurations"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 17,
   "metadata": {},
   "outputs": [],
   "source": [
    "C2 = ry.Config()\n",
    "C2.copy(C)  #this replicates the whole structure\n",
    "V2 = C2.view()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 18,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "array([ -1.11022302e-15,   2.77555756e-16,   0.00000000e+00])"
      ]
     },
     "execution_count": 18,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "F.eval((C,C2))[0]"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "This should be zero. To see a difference, let's move the 2nd configuration:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 19,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "hand difference (y(C2) - y(K)) = [ 0.0031262  -0.02791085  0.04772   ]\n"
     ]
    }
   ],
   "source": [
    "# just to see a difference between the two:\n",
    "q = C2.getJointState()\n",
    "q = q - .1\n",
    "C2.setJointState(q)\n",
    "y = F.eval((C,C2))[0]\n",
    "print('hand difference (y(C2) - y(K)) =', y)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "An acceleration example:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 20,
   "metadata": {},
   "outputs": [],
   "source": [
    "C3 = ry.Config()\n",
    "C3.copy(C);\n",
    "C3.setJointState(q + .2);"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 21,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "hand acceleration =  [ 0.01748784  0.05117527 -0.1607353 ]\n",
      "shape of Jacobian = (3, 30)\n"
     ]
    }
   ],
   "source": [
    "(y,J) = F.eval((C, C2, C3))\n",
    "print('hand acceleration = ', y)\n",
    "print('shape of Jacobian =', J.shape)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Note that the Jacobian is now w.r.t. all three configurations! It is of size 3x3xdim(q). Let's retrieve the Jacobian w.r.t. C3 only:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 22,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "shape of Jacobian = (3, 3, 10)\n"
     ]
    },
    {
     "data": {
      "text/plain": [
       "array([[ -2.00000000e+00,   0.00000000e+00,   3.00160118e-01,\n",
       "          6.56852434e-01,   2.01244715e-01,  -2.30246355e-01,\n",
       "          9.77095656e-01,   2.29441230e-01,   9.08141752e-02,\n",
       "          5.33684535e-08],\n",
       "       [  0.00000000e+00,  -2.00000000e+00,   8.94156151e-01,\n",
       "          7.38763203e-01,   4.69532253e-02,  -3.97204130e-01,\n",
       "         -2.05991963e-01,  -7.03152016e-02,   5.14246272e-02,\n",
       "          2.72098254e-07],\n",
       "       [  0.00000000e+00,   0.00000000e+00,   0.00000000e+00,\n",
       "          7.29252696e-17,   3.70196347e-01,   7.43779799e-01,\n",
       "          1.84265090e-02,   4.11573563e-02,  -4.18381665e-01,\n",
       "         -1.57242171e-06]])"
      ]
     },
     "execution_count": 22,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "J = J.reshape((3,3,q.size))\n",
    "print('shape of Jacobian =', J.shape)\n",
    "J[:,1,:]"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Another example, when the dimensions of C and C2 are different:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 23,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "shape of Jacobian = (3, 17)\n",
      "dimensions of configurations = (10, 7)\n"
     ]
    }
   ],
   "source": [
    "C2.selectJointsByTag(['armL'])\n",
    "(y,J) = F.eval((C,C2))\n",
    "print('shape of Jacobian =', J.shape)\n",
    "print('dimensions of configurations =', (C.getJointDimension(), C2.getJointDimension()))"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Finally, we can linearly transform features by setting 'scale' and 'target':"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 24,
   "metadata": {},
   "outputs": [],
   "source": [
    "#F.scale = 10.\n",
    "#F.target = [0., 0., 1.];\n",
    "#  y = F(C);\n",
    "#  //.. now y = F.scale * (f(C) - F.target), which means y is zero if\n",
    "#  //the feature f(C) is equal to the target (here, if the hand is in world\n",
    "#  //position (0,0,1) )\n",
    "#\n",
    "#  //F.scale can also be a matrix, which can transform the feature also to lower dimensionality\n",
    "#  F.scale = arr(1,3,{0., 0., 1.}); //defines the 1-times-3 matrix (0 0 1)\n",
    "#  y = F(C);\n",
    "#  //.. now y is 1-dimensional and captures only the z-position "
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Camera views (needs more testing)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "We can also add a frame, attached to the head, which has no shape associated to it, but create a view is associated with that frame:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 25,
   "metadata": {},
   "outputs": [],
   "source": [
    "C.addFrame(name='camera', parent='head_tilt_link', args='Q:<d(-90 1 0 0) d(180 0 0 1)> focalLength:.3')\n",
    "V = C.cameraView()\n",
    "IV = V.imageViewer()\n",
    "V.addSensor(name='camera', frameAttached='camera', width=600, height=400)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 26,
   "metadata": {},
   "outputs": [],
   "source": [
    "[image,depth] = V.computeImageAndDepth()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "When we move the robot, that view moves with it:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 27,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "(array([[[255, 255, 255],\n",
       "         [255, 255, 255],\n",
       "         [255, 255, 255],\n",
       "         ..., \n",
       "         [255, 255, 255],\n",
       "         [255, 255, 255],\n",
       "         [255, 255, 255]],\n",
       " \n",
       "        [[255, 255, 255],\n",
       "         [255, 255, 255],\n",
       "         [255, 255, 255],\n",
       "         ..., \n",
       "         [255, 255, 255],\n",
       "         [255, 255, 255],\n",
       "         [255, 255, 255]],\n",
       " \n",
       "        [[255, 255, 255],\n",
       "         [255, 255, 255],\n",
       "         [255, 255, 255],\n",
       "         ..., \n",
       "         [255, 255, 255],\n",
       "         [255, 255, 255],\n",
       "         [255, 255, 255]],\n",
       " \n",
       "        ..., \n",
       "        [[158, 158, 158],\n",
       "         [158, 158, 158],\n",
       "         [158, 158, 158],\n",
       "         ..., \n",
       "         [161, 138, 110],\n",
       "         [161, 138, 110],\n",
       "         [161, 138, 110]],\n",
       " \n",
       "        [[158, 158, 158],\n",
       "         [158, 158, 158],\n",
       "         [158, 158, 158],\n",
       "         ..., \n",
       "         [111, 125, 139],\n",
       "         [111, 125, 139],\n",
       "         [111, 125, 139]],\n",
       " \n",
       "        [[158, 158, 158],\n",
       "         [158, 158, 158],\n",
       "         [158, 158, 158],\n",
       "         ..., \n",
       "         [111, 125, 139],\n",
       "         [111, 125, 139],\n",
       "         [111, 125, 139]]], dtype=uint8),\n",
       " array([[-1.        , -1.        , -1.        , ..., -1.        ,\n",
       "         -1.        , -1.        ],\n",
       "        [-1.        , -1.        , -1.        , ..., -1.        ,\n",
       "         -1.        , -1.        ],\n",
       "        [-1.        , -1.        , -1.        , ..., -1.        ,\n",
       "         -1.        , -1.        ],\n",
       "        ..., \n",
       "        [ 1.93585241,  1.93708158,  1.93832362, ...,  2.20169616,\n",
       "          2.20169616,  2.20169616],\n",
       "        [ 1.93660092,  1.93784225,  1.93908525, ...,  2.19807625,\n",
       "          2.19807625,  2.19807625],\n",
       "        [ 1.93736124,  1.93859231,  1.93983626, ...,  2.19445419,\n",
       "          2.19445419,  2.19445419]], dtype=float32))"
      ]
     },
     "execution_count": 27,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "C.setJointState(q=np.asarray([0.5]), joints=['head_pan_joint'])\n",
    "V.updateConfig(C)\n",
    "V.computeImageAndDepth()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "To close a view (or destroy a handle to a computational module), we reassign it to zero. We can also remove a frame from the configuration."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 28,
   "metadata": {},
   "outputs": [],
   "source": [
    "IV = 0\n",
    "V = 0\n",
    "C.delFrame('camera')"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "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.6.9"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 2
}