capture diff ik issue

drake_stuff/diff_ik_fixed_point_issue/.gitignore

@@ -0,0 +1 @@
+/venv/

drake_stuff/diff_ik_fixed_point_issue/README.md

@@ -0,0 +1,17 @@
+# Diff IK Fixed Point
+
+## Setup
+
+You can just run this:
+
+```sh
+./setup.sh
+```
+
+This will set up a small `virtualenv` with Drake and JupyterLab
+
+## Running
+
+```sh
+./setup.sh jupyter lab ./diff_ik_fixed_point_repro.ipynb
+```

drake_stuff/diff_ik_fixed_point_issue/diff_ik_fixed_point_repro.ipynb

@@ -0,0 +1,278 @@
+{
+ "cells": [
+  {
+   "cell_type": "code",
+   "execution_count": 1,
+   "metadata": {},
+   "outputs": [
+    {
+     "name": "stdout",
+     "output_type": "stream",
+     "text": [
+      "20210312074643 9155f7b806992aada841cb61671c3c5ef71c0634"
+     ]
+    }
+   ],
+   "source": [
+    "!cat ./venv/share/doc/drake/VERSION.TXT"
+   ]
+  },
+  {
+   "cell_type": "code",
+   "execution_count": 2,
+   "metadata": {},
+   "outputs": [],
+   "source": [
+    "import numpy as np\n",
+    "\n",
+    "from pydrake.common import FindResourceOrThrow\n",
+    "from pydrake.multibody.parsing import Parser\n",
+    "from pydrake.multibody.plant import MultibodyPlant\n",
+    "from pydrake.multibody.tree import JacobianWrtVariable\n",
+    "from pydrake.manipulation.planner import (\n",
+    "    DifferentialInverseKinematicsParameters,\n",
+    "    DoDifferentialInverseKinematics,\n",
+    ")"
+   ]
+  },
+  {
+   "cell_type": "code",
+   "execution_count": 3,
+   "metadata": {},
+   "outputs": [],
+   "source": [
+    "def print_q_with_limits(plant, q, *, warn_threshold=0.1):\n",
+    "    q_lower = plant.GetPositionLowerLimits()\n",
+    "    q_upper = plant.GetPositionUpperLimits()\n",
+    "    assert len(q_lower) == len(q)\n",
+    "    name_width = 4\n",
+    "    precision = 5\n",
+    "    fmt = f\"< 10.3f\"\n",
+    "    for i, q_i in enumerate(q):\n",
+    "        name = f\"q[{i}]:\"\n",
+    "        q_i_lower = q_lower[i]\n",
+    "        q_i_upper = q_upper[i]\n",
+    "        dist = min(np.abs(q_i - q_i_lower), np.abs(q_i - q_i_upper))\n",
+    "        line = f\"{name:<{name_width}} \"\n",
+    "        line += f\"{q_i_lower:{fmt}} <= {q_i:{fmt}} <= {q_i_upper:{fmt}}\"\n",
+    "        line += f\"   |   dist: {dist:{fmt}}\"\n",
+    "        if dist < warn_threshold:\n",
+    "            line += \" !!!\"\n",
+    "        print(line)"
+   ]
+  },
+  {
+   "cell_type": "code",
+   "execution_count": 4,
+   "metadata": {},
+   "outputs": [],
+   "source": [
+    "def calc_desired_joint_velocities(\n",
+    "    plant, context, frame_E, *, dt, max_v, V_WEdes=None,\n",
+    "):\n",
+    "    ndof = plant.num_positions()\n",
+    "    assert plant.num_velocities() == ndof\n",
+    "    parameters = DifferentialInverseKinematicsParameters(\n",
+    "        num_positions=ndof, num_velocities=ndof\n",
+    "    )\n",
+    "    parameters.set_unconstrained_degrees_of_freedom_velocity_limit(0.0)\n",
+    "    parameters.set_timestep(dt)\n",
+    "    parameters.set_joint_velocity_limits((-max_v, max_v))\n",
+    "    result = DoDifferentialInverseKinematics(\n",
+    "        robot=plant,\n",
+    "        context=context,\n",
+    "        V_WE_desired=V_WEdes,\n",
+    "        frame_E=frame_E,\n",
+    "        parameters=parameters,\n",
+    "    )\n",
+    "    assert result.status == result.status.kSolutionFound, result.status\n",
+    "    return result.joint_velocities"
+   ]
+  },
+  {
+   "cell_type": "code",
+   "execution_count": 5,
+   "metadata": {},
+   "outputs": [],
+   "source": [
+    "plant = MultibodyPlant(time_step=0.01)\n",
+    "model_file = FindResourceOrThrow(\"drake/manipulation/models/franka_description/urdf/panda_arm.urdf\")\n",
+    "arm = Parser(plant).AddModelFromFile(model_file)\n",
+    "frame_W = plant.world_frame()\n",
+    "frame_B = plant.GetFrameByName(\"panda_link0\")\n",
+    "frame_G = plant.GetFrameByName(\"panda_link8\")\n",
+    "\n",
+    "plant.WeldFrames(frame_W, frame_B)\n",
+    "plant.Finalize()\n",
+    "\n",
+    "context = plant.CreateDefaultContext()\n",
+    "\n",
+    "# Functions w/ nasty globals.\n",
+    "\n",
+    "def calc_Jv_WG():\n",
+    "    global plant, context\n",
+    "    return plant.CalcJacobianSpatialVelocity(\n",
+    "        context,\n",
+    "        with_respect_to=JacobianWrtVariable.kV,\n",
+    "        frame_B=frame_G,\n",
+    "        p_BP=[0, 0, 0],\n",
+    "        frame_A=frame_W,\n",
+    "        frame_E=frame_W,\n",
+    "    )\n",
+    "\n",
+    "def calc_v_with_diff_ik(V_WGdes):\n",
+    "    # This does not produce a fixed point for certain combos of (q, v)...\n",
+    "    global plant, context, frame_G\n",
+    "    max_v = np.ones(7) * 0.5\n",
+    "    v = calc_desired_joint_velocities(\n",
+    "        plant,\n",
+    "        context,\n",
+    "        frame_E=frame_G,\n",
+    "        dt=0.1,\n",
+    "        max_v=max_v,\n",
+    "        V_WEdes=V_WGdes,\n",
+    "    )\n",
+    "    return v\n",
+    "\n",
+    "def calc_v_simple(V_WGdes):\n",
+    "    # This can produce a fixed point.\n",
+    "    Jv_WG = calc_Jv_WG()\n",
+    "    v = np.linalg.lstsq(Jv_WG, V_WGdes)[0]\n",
+    "    return v\n",
+    "\n",
+    "def attempt_fixed_point(v, recalc_v):\n",
+    "    global plant, context\n",
+    "    Jv_WG = calc_Jv_WG()\n",
+    "    V_WGdes = Jv_WG @ v\n",
+    "    v = recalc_v(V_WGdes)\n",
+    "    return v, V_WGdes"
+   ]
+  },
+  {
+   "cell_type": "code",
+   "execution_count": 6,
+   "metadata": {},
+   "outputs": [
+    {
+     "name": "stdout",
+     "output_type": "stream",
+     "text": [
+      "q[0]: -2.897     <=  0.041     <=  2.897       |   dist:  2.856    \n",
+      "q[1]: -1.763     <= -0.715     <=  1.763       |   dist:  1.048    \n",
+      "q[2]: -2.897     <= -0.092     <=  2.897       |   dist:  2.805    \n",
+      "q[3]: -3.072     <= -2.790     <= -0.070       |   dist:  0.282    \n",
+      "q[4]: -2.897     <=  0.011     <=  2.897       |   dist:  2.886    \n",
+      "q[5]: -0.018     <=  2.900     <=  3.752       |   dist:  0.853    \n",
+      "q[6]: -2.897     <= -0.800     <=  2.897       |   dist:  2.097    \n",
+      "v[0]: [0 0 0 0 0 0 0]\n",
+      "\n",
+      "[ Use diff IK ]\n",
+      "V_WGdes[0]: [0 0 0 0 0 0]\n",
+      "v[1]:       [-2.59e-23 -1.7e-22 6.29e-23 -2.57e-23 1.07e-23 -2e-22 4.96e-23]\n",
+      "Δv[1]:      [-2.59e-23 -1.7e-22 6.29e-23 -2.57e-23 1.07e-23 -2e-22 4.96e-23]\n",
+      "---\n",
+      "V_WGdes[1]: [1.9e-23 5.45e-23 -1.77e-24 -3.35e-23 1.6e-23 2.33e-23]\n",
+      "v[2]:       [-0.00188 -0.0141 0.00486 -0.00213 0.000218 -0.0166 0.00459]\n",
+      "Δv[2]:      [-0.00188 -0.0141 0.00486 -0.00213 0.000218 -0.0166 0.00459]\n",
+      "---\n",
+      "V_WGdes[2]: [0.00157 0.00452 -0.000147 -0.00278 0.00133 0.00193]\n",
+      "v[3]:       [-0.00376 -0.0282 0.00972 -0.00426 0.000436 -0.0331 0.00918]\n",
+      "Δv[3]:      [-0.00188 -0.0141 0.00486 -0.00213 0.000218 -0.0165 0.00459]\n",
+      "---\n",
+      "V_WGdes[3]: [0.00315 0.00904 -0.000294 -0.00555 0.00266 0.00386]\n",
+      "v[4]:       [-0.00564 -0.0423 0.0146 -0.00639 0.000654 -0.0496 0.0138]\n",
+      "Δv[4]:      [-0.00188 -0.0141 0.00485 -0.00213 0.000218 -0.0165 0.00458]\n",
+      "---\n",
+      "V_WGdes[4]: [0.00472 0.0135 -0.00044 -0.00832 0.00399 0.00578]\n",
+      "v[5]:       [-0.00751 -0.0563 0.0194 -0.00851 0.000872 -0.0661 0.0183]\n",
+      "Δv[5]:      [-0.00187 -0.014 0.00484 -0.00212 0.000217 -0.0165 0.00457]\n",
+      "---\n",
+      "\n",
+      "[ Use lstsq ]\n",
+      "V_WGdes[1]: [0 0 0 0 0 0]\n",
+      "v[2]:       [0 0 0 0 0 0 0]\n",
+      "Δv[2]:      [0 0 0 0 0 0 0]\n",
+      "---\n",
+      "V_WGdes[2]: [0 0 0 0 0 0]\n",
+      "v[3]:       [0 0 0 0 0 0 0]\n",
+      "Δv[3]:      [0 0 0 0 0 0 0]\n",
+      "---\n",
+      "V_WGdes[3]: [0 0 0 0 0 0]\n",
+      "v[4]:       [0 0 0 0 0 0 0]\n",
+      "Δv[4]:      [0 0 0 0 0 0 0]\n",
+      "---\n",
+      "V_WGdes[4]: [0 0 0 0 0 0]\n",
+      "v[5]:       [0 0 0 0 0 0 0]\n",
+      "Δv[5]:      [0 0 0 0 0 0 0]\n",
+      "---\n",
+      "\n"
+     ]
+    }
+   ],
+   "source": [
+    "np.set_printoptions(formatter={\"float_kind\": lambda x: f\"{x:,.3g}\"})\n",
+    "\n",
+    "q = np.array([0.041, -0.715, -0.0918, -2.79, 0.0112, 2.90, -0.80])\n",
+    "plant.SetPositions(context, arm, q)\n",
+    "v0 = np.zeros(7)\n",
+    "\n",
+    "print_q_with_limits(plant, q)\n",
+    "print(f\"v[0]: {v0}\")\n",
+    "\n",
+    "num_repeat = 5\n",
+    "\n",
+    "print()\n",
+    "print(\"[ Use diff IK ]\")\n",
+    "v = v0\n",
+    "for i in range(num_repeat):\n",
+    "    v_prev = v\n",
+    "    v, V_WGdes = attempt_fixed_point(v, calc_v_with_diff_ik)\n",
+    "    print(f\"V_WGdes[{i}]: {V_WGdes}\")\n",
+    "    print(f\"v[{i+1}]:       {v}\")\n",
+    "    print(f\"Δv[{i+1}]:      {v - v_prev}\")\n",
+    "    print(\"---\")\n",
+    "\n",
+    "print()\n",
+    "print(\"[ Use lstsq ]\")\n",
+    "v = v0\n",
+    "for i in range(1, num_repeat):\n",
+    "    v_prev = v\n",
+    "    v, V_WGdes = attempt_fixed_point(v, calc_v_simple)\n",
+    "    print(f\"V_WGdes[{i}]: {V_WGdes}\")\n",
+    "    print(f\"v[{i+1}]:       {v}\")\n",
+    "    print(f\"Δv[{i+1}]:      {v - v_prev}\")\n",
+    "    print(\"---\")\n",
+    "\n",
+    "print()"
+   ]
+  },
+  {
+   "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": 4
+}