embeddef.ipynb

{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": 1,
   "id": "4e99238d-59fa-48e9-8bdc-5abf063ec942",
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Function:\n"
     ]
    },
    {
     "data": {
      "text/latex": [
       "$\\displaystyle Rot\\left({R_z}\\right)$"
      ],
      "text/plain": [
       "<SymE3.core.TotalFunction at 0x7f12c25fdbd0>"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    },
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "<class 'SymE3.core.TotalFunction'>\n",
      "Expanded:\n"
     ]
    },
    {
     "data": {
      "text/latex": [
       "$\\displaystyle \\left[\\begin{matrix}{{R_z}_{00}} {{R_z}_{01}} + {{R_z}_{10}} {{R_z}_{11}} + {{R_z}_{20}} {{R_z}_{21}}\\\\{{R_z}_{00}} {{R_z}_{02}} + {{R_z}_{10}} {{R_z}_{12}} + {{R_z}_{20}} {{R_z}_{22}}\\\\{{R_z}_{01}} {{R_z}_{02}} + {{R_z}_{11}} {{R_z}_{12}} + {{R_z}_{21}} {{R_z}_{22}}\\\\{{R_z}_{00}}^{2} + {{R_z}_{10}}^{2} + {{R_z}_{20}}^{2} - 1\\\\{{R_z}_{01}}^{2} + {{R_z}_{11}}^{2} + {{R_z}_{21}}^{2} - 1\\\\{{R_z}_{02}}^{2} + {{R_z}_{12}}^{2} + {{R_z}_{22}}^{2} - 1\\end{matrix}\\right]$"
      ],
      "text/plain": [
       "[[{{R_z}_{00}}*{{R_z}_{01}} + {{R_z}_{10}}*{{R_z}_{11}} + {{R_z}_{20}}*{{R_z}_{21}}], [{{R_z}_{00}}*{{R_z}_{02}} + {{R_z}_{10}}*{{R_z}_{12}} + {{R_z}_{20}}*{{R_z}_{22}}], [{{R_z}_{01}}*{{R_z}_{02}} + {{R_z}_{11}}*{{R_z}_{12}} + {{R_z}_{21}}*{{R_z}_{22}}], [{{R_z}_{00}}**2 + {{R_z}_{10}}**2 + {{R_z}_{20}}**2 - 1], [{{R_z}_{01}}**2 + {{R_z}_{11}}**2 + {{R_z}_{21}}**2 - 1], [{{R_z}_{02}}**2 + {{R_z}_{12}}**2 + {{R_z}_{22}}**2 - 1]]"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    },
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "(6, 1)\n",
      "<class 'SymE3.detail._Explicit'>\n",
      "Derivative:\n"
     ]
    },
    {
     "data": {
      "text/latex": [
       "$\\displaystyle \\left[\\begin{array}{cccccccccccc}{{R_z}_{01}} & {{R_z}_{00}} & 0 & {{R_z}_{11}} & {{R_z}_{10}} & 0 & {{R_z}_{21}} & {{R_z}_{20}} & 0 & 0 & 0 & 0\\\\{{R_z}_{02}} & 0 & {{R_z}_{00}} & {{R_z}_{12}} & 0 & {{R_z}_{10}} & {{R_z}_{22}} & 0 & {{R_z}_{20}} & 0 & 0 & 0\\\\0 & {{R_z}_{02}} & {{R_z}_{01}} & 0 & {{R_z}_{12}} & {{R_z}_{11}} & 0 & {{R_z}_{22}} & {{R_z}_{21}} & 0 & 0 & 0\\\\2 {{R_z}_{00}} & 0 & 0 & 2 {{R_z}_{10}} & 0 & 0 & 2 {{R_z}_{20}} & 0 & 0 & 0 & 0 & 0\\\\0 & 2 {{R_z}_{01}} & 0 & 0 & 2 {{R_z}_{11}} & 0 & 0 & 2 {{R_z}_{21}} & 0 & 0 & 0 & 0\\\\0 & 0 & 2 {{R_z}_{02}} & 0 & 0 & 2 {{R_z}_{12}} & 0 & 0 & 2 {{R_z}_{22}} & 0 & 0 & 0\\end{array}\\right]$"
      ],
      "text/plain": [
       "Matrix([\n",
       "[  {{R_z}_{01}},   {{R_z}_{00}},              0,   {{R_z}_{11}},   {{R_z}_{10}},              0,   {{R_z}_{21}},   {{R_z}_{20}},              0, 0, 0, 0],\n",
       "[  {{R_z}_{02}},              0,   {{R_z}_{00}},   {{R_z}_{12}},              0,   {{R_z}_{10}},   {{R_z}_{22}},              0,   {{R_z}_{20}}, 0, 0, 0],\n",
       "[             0,   {{R_z}_{02}},   {{R_z}_{01}},              0,   {{R_z}_{12}},   {{R_z}_{11}},              0,   {{R_z}_{22}},   {{R_z}_{21}}, 0, 0, 0],\n",
       "[2*{{R_z}_{00}},              0,              0, 2*{{R_z}_{10}},              0,              0, 2*{{R_z}_{20}},              0,              0, 0, 0, 0],\n",
       "[             0, 2*{{R_z}_{01}},              0,              0, 2*{{R_z}_{11}},              0,              0, 2*{{R_z}_{21}},              0, 0, 0, 0],\n",
       "[             0,              0, 2*{{R_z}_{02}},              0,              0, 2*{{R_z}_{12}},              0,              0, 2*{{R_z}_{22}}, 0, 0, 0]])"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    },
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "(6, 12)\n",
      "<class 'sympy.matrices.dense.MutableDenseMatrix'>\n",
      "Function:\n"
     ]
    },
    {
     "data": {
      "text/latex": [
       "$\\displaystyle {R_z} \\left({g_n} - {g_z}\\right) - {g_n} + {g_z} - {t_n} + {t_z}$"
      ],
      "text/plain": [
       "<SymE3.core.TotalFunction at 0x7f12c2632050>"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    },
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "<class 'SymE3.core.TotalFunction'>\n",
      "Expanded:\n"
     ]
    },
    {
     "data": {
      "text/latex": [
       "$\\displaystyle \\left[\\begin{matrix}{{R_z}_{00}} \\left({{g_n}_{0}} - {{g_z}_{0}}\\right) + {{R_z}_{01}} \\left({{g_n}_{1}} - {{g_z}_{1}}\\right) + {{R_z}_{02}} \\left({{g_n}_{2}} - {{g_z}_{2}}\\right) - {{g_n}_{0}} + {{g_z}_{0}} - {{t_n}_{0}} + {{t_z}_{0}}\\\\{{R_z}_{10}} \\left({{g_n}_{0}} - {{g_z}_{0}}\\right) + {{R_z}_{11}} \\left({{g_n}_{1}} - {{g_z}_{1}}\\right) + {{R_z}_{12}} \\left({{g_n}_{2}} - {{g_z}_{2}}\\right) - {{g_n}_{1}} + {{g_z}_{1}} - {{t_n}_{1}} + {{t_z}_{1}}\\\\{{R_z}_{20}} \\left({{g_n}_{0}} - {{g_z}_{0}}\\right) + {{R_z}_{21}} \\left({{g_n}_{1}} - {{g_z}_{1}}\\right) + {{R_z}_{22}} \\left({{g_n}_{2}} - {{g_z}_{2}}\\right) - {{g_n}_{2}} + {{g_z}_{2}} - {{t_n}_{2}} + {{t_z}_{2}}\\end{matrix}\\right]$"
      ],
      "text/plain": [
       "[[{{R_z}_{00}}*({{g_n}_{0}} - {{g_z}_{0}}) + {{R_z}_{01}}*({{g_n}_{1}} - {{g_z}_{1}}) + {{R_z}_{02}}*({{g_n}_{2}} - {{g_z}_{2}}) - {{g_n}_{0}} + {{g_z}_{0}} - {{t_n}_{0}} + {{t_z}_{0}}], [{{R_z}_{10}}*({{g_n}_{0}} - {{g_z}_{0}}) + {{R_z}_{11}}*({{g_n}_{1}} - {{g_z}_{1}}) + {{R_z}_{12}}*({{g_n}_{2}} - {{g_z}_{2}}) - {{g_n}_{1}} + {{g_z}_{1}} - {{t_n}_{1}} + {{t_z}_{1}}], [{{R_z}_{20}}*({{g_n}_{0}} - {{g_z}_{0}}) + {{R_z}_{21}}*({{g_n}_{1}} - {{g_z}_{1}}) + {{R_z}_{22}}*({{g_n}_{2}} - {{g_z}_{2}}) - {{g_n}_{2}} + {{g_z}_{2}} - {{t_n}_{2}} + {{t_z}_{2}}]]"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    },
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "(3, 1)\n",
      "<class 'SymE3.detail._Explicit'>\n",
      "Derivative:\n"
     ]
    },
    {
     "data": {
      "text/latex": [
       "$\\displaystyle \\left[\\begin{array}{ccccccccccccccc}{{g_n}_{0}} - {{g_z}_{0}} & {{g_n}_{1}} - {{g_z}_{1}} & {{g_n}_{2}} - {{g_z}_{2}} & 0 & 0 & 0 & 0 & 0 & 0 & 1 & 0 & 0 & -1 & 0 & 0\\\\0 & 0 & 0 & {{g_n}_{0}} - {{g_z}_{0}} & {{g_n}_{1}} - {{g_z}_{1}} & {{g_n}_{2}} - {{g_z}_{2}} & 0 & 0 & 0 & 0 & 1 & 0 & 0 & -1 & 0\\\\0 & 0 & 0 & 0 & 0 & 0 & {{g_n}_{0}} - {{g_z}_{0}} & {{g_n}_{1}} - {{g_z}_{1}} & {{g_n}_{2}} - {{g_z}_{2}} & 0 & 0 & 1 & 0 & 0 & -1\\end{array}\\right]$"
      ],
      "text/plain": [
       "Matrix([\n",
       "[{{g_n}_{0}} - {{g_z}_{0}}, {{g_n}_{1}} - {{g_z}_{1}}, {{g_n}_{2}} - {{g_z}_{2}},                         0,                         0,                         0,                         0,                         0,                         0, 1, 0, 0, -1,  0,  0],\n",
       "[                        0,                         0,                         0, {{g_n}_{0}} - {{g_z}_{0}}, {{g_n}_{1}} - {{g_z}_{1}}, {{g_n}_{2}} - {{g_z}_{2}},                         0,                         0,                         0, 0, 1, 0,  0, -1,  0],\n",
       "[                        0,                         0,                         0,                         0,                         0,                         0, {{g_n}_{0}} - {{g_z}_{0}}, {{g_n}_{1}} - {{g_z}_{1}}, {{g_n}_{2}} - {{g_z}_{2}}, 0, 0, 1,  0,  0, -1]])"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    },
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "(3, 15)\n",
      "<class 'sympy.matrices.dense.MutableDenseMatrix'>\n",
      "Function:\n"
     ]
    },
    {
     "data": {
      "text/latex": [
       "$\\displaystyle {w_{n_{(v_s)}}} \\left({R_n} \\left(- {g_n} + {v_s}\\right) + {g_n} + {t_n}\\right) - {q_s}$"
      ],
      "text/plain": [
       "<SymE3.core.TotalFunction at 0x7f12c21845e0>"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    },
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "<class 'SymE3.core.TotalFunction'>\n",
      "Expanded:\n"
     ]
    },
    {
     "data": {
      "text/latex": [
       "$\\displaystyle \\left[\\begin{matrix}{w_{n_{(v_s)}}} \\left({{R_n}_{00}} \\left(- {{g_n}_{0}} + {{v_s}_{0}}\\right) + {{R_n}_{01}} \\left(- {{g_n}_{1}} + {{v_s}_{1}}\\right) + {{R_n}_{02}} \\left(- {{g_n}_{2}} + {{v_s}_{2}}\\right) + {{g_n}_{0}} + {{t_n}_{0}}\\right) - {{q_s}_{0}}\\\\{w_{n_{(v_s)}}} \\left({{R_n}_{10}} \\left(- {{g_n}_{0}} + {{v_s}_{0}}\\right) + {{R_n}_{11}} \\left(- {{g_n}_{1}} + {{v_s}_{1}}\\right) + {{R_n}_{12}} \\left(- {{g_n}_{2}} + {{v_s}_{2}}\\right) + {{g_n}_{1}} + {{t_n}_{1}}\\right) - {{q_s}_{1}}\\\\{w_{n_{(v_s)}}} \\left({{R_n}_{20}} \\left(- {{g_n}_{0}} + {{v_s}_{0}}\\right) + {{R_n}_{21}} \\left(- {{g_n}_{1}} + {{v_s}_{1}}\\right) + {{R_n}_{22}} \\left(- {{g_n}_{2}} + {{v_s}_{2}}\\right) + {{g_n}_{2}} + {{t_n}_{2}}\\right) - {{q_s}_{2}}\\end{matrix}\\right]$"
      ],
      "text/plain": [
       "[[{w_{n_{(v_s)}}}*({{R_n}_{00}}*(-{{g_n}_{0}} + {{v_s}_{0}}) + {{R_n}_{01}}*(-{{g_n}_{1}} + {{v_s}_{1}}) + {{R_n}_{02}}*(-{{g_n}_{2}} + {{v_s}_{2}}) + {{g_n}_{0}} + {{t_n}_{0}}) - {{q_s}_{0}}], [{w_{n_{(v_s)}}}*({{R_n}_{10}}*(-{{g_n}_{0}} + {{v_s}_{0}}) + {{R_n}_{11}}*(-{{g_n}_{1}} + {{v_s}_{1}}) + {{R_n}_{12}}*(-{{g_n}_{2}} + {{v_s}_{2}}) + {{g_n}_{1}} + {{t_n}_{1}}) - {{q_s}_{1}}], [{w_{n_{(v_s)}}}*({{R_n}_{20}}*(-{{g_n}_{0}} + {{v_s}_{0}}) + {{R_n}_{21}}*(-{{g_n}_{1}} + {{v_s}_{1}}) + {{R_n}_{22}}*(-{{g_n}_{2}} + {{v_s}_{2}}) + {{g_n}_{2}} + {{t_n}_{2}}) - {{q_s}_{2}}]]"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    },
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "(3, 1)\n",
      "<class 'SymE3.detail._Explicit'>\n",
      "Derivative:\n"
     ]
    },
    {
     "data": {
      "text/latex": [
       "$\\displaystyle \\left[\\begin{array}{cccccccccccc}{w_{n_{(v_s)}}} \\left(- {{g_n}_{0}} + {{v_s}_{0}}\\right) & {w_{n_{(v_s)}}} \\left(- {{g_n}_{1}} + {{v_s}_{1}}\\right) & {w_{n_{(v_s)}}} \\left(- {{g_n}_{2}} + {{v_s}_{2}}\\right) & 0 & 0 & 0 & 0 & 0 & 0 & {w_{n_{(v_s)}}} & 0 & 0\\\\0 & 0 & 0 & {w_{n_{(v_s)}}} \\left(- {{g_n}_{0}} + {{v_s}_{0}}\\right) & {w_{n_{(v_s)}}} \\left(- {{g_n}_{1}} + {{v_s}_{1}}\\right) & {w_{n_{(v_s)}}} \\left(- {{g_n}_{2}} + {{v_s}_{2}}\\right) & 0 & 0 & 0 & 0 & {w_{n_{(v_s)}}} & 0\\\\0 & 0 & 0 & 0 & 0 & 0 & {w_{n_{(v_s)}}} \\left(- {{g_n}_{0}} + {{v_s}_{0}}\\right) & {w_{n_{(v_s)}}} \\left(- {{g_n}_{1}} + {{v_s}_{1}}\\right) & {w_{n_{(v_s)}}} \\left(- {{g_n}_{2}} + {{v_s}_{2}}\\right) & 0 & 0 & {w_{n_{(v_s)}}}\\end{array}\\right]$"
      ],
      "text/plain": [
       "Matrix([\n",
       "[{w_{n_{(v_s)}}}*(-{{g_n}_{0}} + {{v_s}_{0}}), {w_{n_{(v_s)}}}*(-{{g_n}_{1}} + {{v_s}_{1}}), {w_{n_{(v_s)}}}*(-{{g_n}_{2}} + {{v_s}_{2}}),                                            0,                                            0,                                            0,                                            0,                                            0,                                            0, {w_{n_{(v_s)}}},               0,               0],\n",
       "[                                           0,                                            0,                                            0, {w_{n_{(v_s)}}}*(-{{g_n}_{0}} + {{v_s}_{0}}), {w_{n_{(v_s)}}}*(-{{g_n}_{1}} + {{v_s}_{1}}), {w_{n_{(v_s)}}}*(-{{g_n}_{2}} + {{v_s}_{2}}),                                            0,                                            0,                                            0,               0, {w_{n_{(v_s)}}},               0],\n",
       "[                                           0,                                            0,                                            0,                                            0,                                            0,                                            0, {w_{n_{(v_s)}}}*(-{{g_n}_{0}} + {{v_s}_{0}}), {w_{n_{(v_s)}}}*(-{{g_n}_{1}} + {{v_s}_{1}}), {w_{n_{(v_s)}}}*(-{{g_n}_{2}} + {{v_s}_{2}}),               0,               0, {w_{n_{(v_s)}}}]])"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    },
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "(3, 12)\n",
      "<class 'sympy.matrices.dense.MutableDenseMatrix'>\n"
     ]
    }
   ],
   "source": [
    "from sympy import Matrix\n",
    "from SymE3.core import Matrix3, Scalar, Point, CustomFunction, TotalFunction\n",
    "\n",
    "def show(v):\n",
    "    display(v)\n",
    "    if hasattr(v, 'shape'):\n",
    "        print(v.shape)\n",
    "    print(type(v))\n",
    "    \n",
    "t_z = Point(\"{t_z}\")\n",
    "t_n = Point(\"{t_n}\")\n",
    "g_z = Point(\"{g_z}\")\n",
    "g_n = Point(\"{g_n}\")\n",
    "v_s = Point(\"{v_s}\")\n",
    "q_s = Point(\"{q_s}\")\n",
    "w_nvs = Scalar(\"{w_{n_{(v_s)}}}\")\n",
    "R_n = Matrix3(\"{R_n}\")\n",
    "R_z = Matrix3(\"{R_z}\")\n",
    "\n",
    "def rot(R):\n",
    "    return Matrix([[R[:, 0].dot(R[:, 1])], \n",
    "                   [R[:, 0].dot(R[:, 2])],\n",
    "                   [R[:, 1].dot(R[:, 2])],\n",
    "                   [R[:, 0].dot(R[:, 0]) - 1],\n",
    "                   [R[:, 1].dot(R[:, 1]) - 1],\n",
    "                   [R[:, 2].dot(R[:, 2]) - 1]])\n",
    "\n",
    "Rot = CustomFunction(\"Rot\", rot, 3, 6, 3)\n",
    "\n",
    "# Rotation cost\n",
    "e = Rot(R_z)\n",
    "f = TotalFunction(e)\n",
    "\n",
    "print(\"Function:\")\n",
    "show(f)\n",
    "\n",
    "print(\"Expanded:\")\n",
    "fe = f.as_explicit()\n",
    "show(fe)\n",
    "\n",
    "print(\"Derivative:\")\n",
    "# This jacobian is an element of the matrix per column in row major order\n",
    "df_dRt = f.diff(R_z, t_z)\n",
    "show(df_dRt)\n",
    "\n",
    "# Regularization cost\n",
    "e = R_z * (g_n - g_z) + g_z + t_z - (g_n + t_n)\n",
    "\n",
    "f = TotalFunction(e)\n",
    "\n",
    "print(\"Function:\")\n",
    "show(f)\n",
    "\n",
    "print(\"Expanded:\")\n",
    "fe = f.as_explicit()\n",
    "show(fe)\n",
    "\n",
    "print(\"Derivative:\")\n",
    "# This jacobian is an element of the matrix per column in row major order\n",
    "df_dRt = f.diff(R_z, t_z, t_n)\n",
    "show(df_dRt)\n",
    "\n",
    "# Constraint cost\n",
    "e = (w_nvs * (R_n * (v_s - g_n) + g_n + t_n)) - q_s\n",
    "\n",
    "f = TotalFunction(e)\n",
    "\n",
    "print(\"Function:\")\n",
    "show(f)\n",
    "\n",
    "print(\"Expanded:\")\n",
    "fe = f.as_explicit()\n",
    "show(fe)\n",
    "\n",
    "print(\"Derivative:\")\n",
    "# This jacobian is an element of the matrix per column in row major order\n",
    "df_dRt = f.diff(R_n, t_n)\n",
    "show(df_dRt)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "fb028ec9-45be-478a-afd3-b4befc0e2d65",
   "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.10.6"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 5
}