Feature/decomposition

python/tests/panda.ipynb

@@ -0,0 +1,151 @@
+{
+ "cells": [
+  {
+   "cell_type": "markdown",
+   "metadata": {},
+   "source": [
+    "# Panda IK\n",
+    "Do Panda IK with shoulder partitioning, using a roadmap IK module for the 3-DOF arm."
+   ]
+  },
+  {
+   "cell_type": "code",
+   "execution_count": null,
+   "metadata": {},
+   "outputs": [],
+   "source": [
+    "from pathlib import Path\n",
+    "import numpy as np\n",
+    "from typing import Optional\n",
+    "\n",
+    "import gtdynamics as gtd\n",
+    "from gtsam import Pose3, Values, Rot3, Point3\n",
+    "\n",
+    "import plotly.express as px\n",
+    "\n",
+    "from prototype.chain import Chain"
+   ]
+  },
+  {
+   "cell_type": "code",
+   "execution_count": null,
+   "metadata": {},
+   "outputs": [],
+   "source": [
+    "model_file = Path(gtd.URDF_PATH) / \"panda\" / \"panda.urdf\"\n",
+    "base_name = \"link0\"\n",
+    "# Crucial to fix base link or FK gives wrong result\n",
+    "robot = gtd.CreateRobotFromFile(str(model_file)).fixLink(base_name)"
+   ]
+  },
+  {
+   "cell_type": "code",
+   "execution_count": null,
+   "metadata": {},
+   "outputs": [],
+   "source": [
+    "# Construct shoulder:\n",
+    "shoulder = Chain.from_robot(robot, base_name, (0, 3))\n"
+   ]
+  },
+  {
+   "cell_type": "code",
+   "execution_count": null,
+   "metadata": {},
+   "outputs": [],
+   "source": [
+    "# Sample a few configurations:\n",
+    "N = 200\n",
+    "theta =  2 * np.pi * (np.random.random_sample((N, 3)) - 0.5)"
+   ]
+  },
+  {
+   "cell_type": "code",
+   "execution_count": null,
+   "metadata": {},
+   "outputs": [],
+   "source": [
+    "# Let's plot end-effector position which is child link of last shoulder joint:\n",
+    "poses = [shoulder.poe(q) for q in theta]\n",
+    "ts = np.array([pose.translation() for pose in poses])\n",
+    "px.scatter_3d(x=ts[:,0], y=ts[:,1], z=ts[:,2])"
+   ]
+  },
+  {
+   "cell_type": "code",
+   "execution_count": null,
+   "metadata": {},
+   "outputs": [],
+   "source": [
+    "# Solve equation for finding the shoulder \"center\":\n",
+    "A = shoulder.axes\n",
+    "JR = A[:3,:]\n",
+    "Jt = A[3:,:]\n",
+    "Skew = -Jt @ np.linalg.pinv(JR)\n",
+    "print(np.round(Skew,3))"
+   ]
+  },
+  {
+   "cell_type": "code",
+   "execution_count": null,
+   "metadata": {},
+   "outputs": [],
+   "source": [
+    "sTc = Pose3(Rot3(), Point3(-Skew[1,2], Skew[0,2], -Skew[0,1]))\n",
+    "np.round(sTc.AdjointMap() @ A, 5)"
+   ]
+  },
+  {
+   "cell_type": "code",
+   "execution_count": null,
+   "metadata": {},
+   "outputs": [],
+   "source": [
+    "# Let's now go to the shoulder position:\n",
+    "cTs = sTc.inverse()\n",
+    "poses = [shoulder.poe(q, cTs) for q in theta]\n",
+    "ts = np.array([pose.translation() for pose in poses])"
+   ]
+  },
+  {
+   "cell_type": "code",
+   "execution_count": null,
+   "metadata": {},
+   "outputs": [],
+   "source": [
+    "np.round(ts[:10,:],3)"
+   ]
+  },
+  {
+   "cell_type": "code",
+   "execution_count": null,
+   "metadata": {},
+   "outputs": [],
+   "source": []
+  }
+ ],
+ "metadata": {
+  "interpreter": {
+   "hash": "c6e4e9f98eb68ad3b7c296f83d20e6de614cb42e90992a65aa266555a3137d0d"
+  },
+  "kernelspec": {
+   "display_name": "Python 3.9.6 64-bit ('base': conda)",
+   "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"
+  },
+  "orig_nbformat": 4
+ },
+ "nbformat": 4,
+ "nbformat_minor": 2
+}

python/tests/prototype/chain.md

@@ -12,11 +12,11 @@ Below I use tensor notation with superscripts and subscripts, and make sure matr
 ## Kinematics
 
 We know that the forward kinematics of a chain can be given by a **product of exponential maps** or POE, e.g., in the 3DOF case we have
-$$T^b_e(\theta) = M^b_e \exp[A^e_1\theta^1]\exp[A^e_2\theta^2]\exp [A^e_3\theta^3] \doteq M^b_e \exp[A^e_j\theta^j]$$
-where the $6\times3$ matrix $A^e_j$ collects the joint screw axes expressed in the end-effector frame. Note I defined $\exp[A^e_j\theta^j]$ as shorthand for the POE of all axes in the matrix $A^e_j$.
+$$T^b_e(\theta^j) = M^b_e \exp[A^e_1\theta^1]\exp[A^e_2\theta^2]\exp [A^e_3\theta^3] \doteq M^b_e \exp[A^e_j,\theta^j]$$
+where the $6\times3$ matrix $A^e_j$ collects the joint screw axes expressed in the end-effector frame. Note I defined $\exp[A^e_j,\theta^j]$ as shorthand for the POE of all axes in the matrix $A^e_j$.
 
 In fact, $A^e_j$ is just the **manipulator Jacobian** $J^e_j(\theta^j)$ at rest, and more generally we have
-$$T^b_e(\theta^j+\delta \theta^j) = T^b_e(\theta^j) \exp[J^e_j(\theta^j)\delta \theta^j]$$
+$$T^b_e(\theta^j+\delta \theta^j) = T^b_e(\theta^j) \exp[J^e_j(\theta^j),\delta \theta^j]$$
 with $A^e_j=J^e_j(0^j)$.
 
 ## Monoid Math
@@ -49,6 +49,7 @@ Note, above we treat $\tau_j$ and $\mathcal{F_e}$ as *row* vectors, as indicated
 
 Of course, we are typically more interested in applying a wrench $\mathcal{F_b}$ on the body $B$, by delivering torques $\tau_j$ ate the joints, so we need to adjoint from the body:
 $$\tau_j = \mathcal{F_b} Ad^b_e J^e_j(\theta)$$
+$$\tau_j = \mathcal{F_b} (Ad^b_e J^e_j(\theta)) = \mathcal{F_b} J^b_j(\theta)$$
 with
 $$Ad^b_e\doteq Ad_{T^b_e}.$$
 

python/tests/prototype/chain.py

@@ -21,7 +21,7 @@ def compose(aSbj: Tuple[Pose3, np.ndarray], bSck: Tuple[Pose3, np.ndarray]):
     aTb, bAj = aSbj
     bTc, cAk = bSck
     assert bAj.shape[0] == 6 and cAk.shape[0] == 6,\
-        f"Jaocobains should have 6 rows, sapes are {bAj.shape} and {cAk.shape}"
+        f"Jaocobians should have 6 rows, shapes are {bAj.shape}, {cAk.shape}"
 
     # Compose poses:
     aTc = aTb.compose(bTc)