Skip to content

refactor(cuda): rewrite revolute joint using F-space Jacobian formulation #404

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Merged
MuGdxy merged 2 commits into from
Mar 25, 2026
Merged

refactor(cuda): rewrite revolute joint using F-space Jacobian formulation #404

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
00d2891
refactor(cuda): rewrite revolute joint using F-space Jacobian formula…
Ligo04 Mar 25, 2026
00b897f
fix(cuda): add gradient_only guard to driving revolute joint and clea…
Ligo04 Mar 25, 2026
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
187 changes: 154 additions & 33 deletions scripts/symbol_calculation/affine_body_revolute_joint.ipynb
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -6,37 +6,92 @@
"metadata": {},
"outputs": [],
"source": [
"import sympy as sp\n",
"import sys\n",
"sys.path.append('../')\n",
"\n",
"sys.path.append(\"../\")\n",
"\n",
"import sympy as sp\n",
"import pathlib as pl\n",
"from SymEigen import *\n",
"from sympy import symbols\n",
"from project_dir import backend_source_dir\n",
"from affine_body_core import compute_J_point, compute_J_vec\n",
"\n",
"Gen = EigenFunctionGenerator()\n",
"Gen.MacroBeforeFunction(\"__host__ __device__\")\n",
"Gen.DisableLatexComment()\n"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"ref: [Affine Body Revolute Joint](https://spirimirror.github.io/libuipc-doc/specification/constitutions/affine_body_revolute_joint/)\n",
"\n",
"![Affine Body Revolute Joint](../../docs/specification/constitutions/media/affine_body_revolute_joint_fig1.drawio.svg)\n",
"\n",
"$$\n",
"C_0 = (\\mathbf{q}_i^0 - \\mathbf{q}_j^0 ) = \\mathbf{0}\n",
"$$\n",
"\n",
"$$\n",
"C_1 = (\\mathbf{q}_i^1 - \\mathbf{q}_j^1) = \\mathbf{0}\n",
"$$"
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {},
"outputs": [],
"source": [
"kappa = Eigen.Scalar(\"kappa\")\n",
"x0_bar = Eigen.Vector(\"x0_bar\", 3)\n",
"x1_bar = Eigen.Vector(\"x1_bar\", 3)\n",
"x2_bar = Eigen.Vector(\"x2_bar\", 3)\n",
"x3_bar = Eigen.Vector(\"x3_bar\", 3)\n",
"\n",
"\n",
"def compute_J(xbar):\n",
" J = sp.Matrix.zeros(3,12)\n",
" J[0:3,0:3] = sp.eye(3)\n",
" J[0,3:6] = xbar.T\n",
" J[1,6:9] = xbar.T\n",
" J[2,9:12] = xbar.T\n",
" return J\n",
"J0 = compute_J(x0_bar)\n",
"J1 = compute_J(x1_bar)\n",
"J2 = compute_J(x2_bar)\n",
"J3 = compute_J(x3_bar)\n",
"\n",
"Cl = Gen.Closure(kappa, x0_bar, x1_bar, x2_bar, x3_bar)"
"\n",
"# Basis vectors\n",
"qi0_bar = Eigen.Vector(\"qi0_bar\", 3)\n",
"qi1_bar = Eigen.Vector(\"qi1_bar\", 3)\n",
"qj0_bar = Eigen.Vector(\"qj0_bar\", 3)\n",
"qj1_bar = Eigen.Vector(\"qj1_bar\", 3)\n",
"\n",
"# Affine Body DOF vectors\n",
"qi = Eigen.Vector(\"qi\", 12)\n",
"qj = Eigen.Vector(\"qj\", 12)"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"$$\n",
"C_0 = (\\mathbf{q}_i^0 - \\mathbf{q}_j^0 ) = \\mathbf{0}\n",
"$$\n",
"\n",
"$$\n",
"C_1 = (\\mathbf{q}_i^1 - \\mathbf{q}_j^1) = \\mathbf{0}\n",
"$$\n",
"$$\n",
"\\mathbf{F}_{axis} = \\begin{bmatrix}\n",
"\\mathbf{q}_i^0 - \\mathbf{q}_j^0 \\\\\n",
"\\mathbf{q}_i^1 - \\mathbf{q}_j^1 \\\\\n",
"\\end{bmatrix}_{6 \\times 1}\n",
"$$\n",
"\n",
"Frame Affine Body Mapping:\n",
"\n",
"$$\n",
"\\mathbf{J}_{axis} = \\begin{bmatrix}\n",
"\\mathbf{J}(\\bar{\\mathbf{q}}_i^0) & -\\mathbf{J}(\\bar{\\mathbf{q}}_j^0) \\\\\n",
"\\mathbf{J}(\\bar{\\mathbf{q}}_i^1) & -\\mathbf{J}(\\bar{\\mathbf{q}}_j^1) \\\\\n",
"\\end{bmatrix}_{6 \\times 24}\n",
"$$\n",
"\n",
"$$\n",
"\\mathbf{F}_{axis} = \\mathbf{J}_{axis} \\cdot\n",
"\\begin{bmatrix}\n",
"\\mathbf{q}_i \\\\\n",
"\\mathbf{q}_j \\\\\n",
"\\end{bmatrix}\n",
"$$"
]
},
{
Expand All @@ -45,8 +100,78 @@
"metadata": {},
"outputs": [],
"source": [
"Hess = kappa[0,0] * (J0.T@J1 + J2.T@J3)\n",
"Hess"
"# Mapping\n",
"Jaxis = sp.Matrix.zeros(6, 24)\n",
"Jaxis[0:3, 0:12] = compute_J_point(qi0_bar)\n",
"Jaxis[0:3, 12:24] = -compute_J_point(qj0_bar)\n",
"Jaxis[3:6, 0:12] = compute_J_point(qi1_bar)\n",
"Jaxis[3:6, 12:24] = -compute_J_point(qj1_bar)\n",
"\n",
"\n",
"content = \"\"\n",
"\n",
"# from ABD q to F\n",
"Faxis = Jaxis @ sp.Matrix.vstack(qi, qj)\n",
"Cl_Faxis = Gen.Closure(\n",
" qi0_bar,\n",
" qi1_bar,\n",
" qi, # Affine Body i\n",
" qj0_bar,\n",
" qj1_bar,\n",
" qj,\n",
") # Affine Body j\n",
"\n",
"# from F Gradient to ABD Gradient\n",
"Gaxis = Eigen.Vector(\"Gaxis\", 6)\n",
"JaxisT_Gaxis = Jaxis.T @ Gaxis\n",
"Cl_Gaxis = Gen.Closure(\n",
" Gaxis,\n",
" qi0_bar,\n",
" qi1_bar, # Affine Body i\n",
" qj0_bar,\n",
" qj1_bar,\n",
") # Affine Body j\n",
"# from F Hessian to ABD Hessian\n",
"Haxis = Eigen.Matrix(\"Haxis\", 6, 6)\n",
"JaxisT_Haxis_Jaxis = Jaxis.T @ Haxis @ Jaxis\n",
"Cl_Haxis = Gen.Closure(\n",
" Haxis,\n",
" qi0_bar,\n",
" qi1_bar, # Affine Body i\n",
" qj0_bar,\n",
" qj1_bar,\n",
") # Affine Body j\n",
"\n",
"content += f\"\"\" \n",
"// Revolute Joint: C0 C1\n",
"// Mapping between ABD qi qj to Faxis\n",
"\n",
"{Cl_Faxis(\"Faxis\", Faxis)}\n",
"{Cl_Gaxis(\"JaxisT_Gaxis\", JaxisT_Gaxis)}\n",
"{Cl_Haxis(\"JaxisT_Haxis_Jaxis\", JaxisT_Haxis_Jaxis)}\n",
"\n",
"\"\"\"\n",
"\n",
"Faxis = Eigen.Vector(\"Faxis\", 6)\n",
"dij0 = Faxis[0:3, 0]\n",
"dij1 = Faxis[3:6, 0]\n",
"\n",
"Eaxis = 0.5 * kappa * (dij0.dot(dij0) + dij1.dot(dij1))\n",
"# E01 = 0\n",
"Cl_Eaxis = Gen.Closure(kappa, Faxis)\n",
"\n",
"\n",
"dEaxisdFaxis = VecDiff(Eaxis, Faxis)\n",
"ddEaxisddFaxis = VecDiff(dEaxisdFaxis, Faxis)\n",
"\n",
"content += f\"\"\" // Revolute Joint Energy and Derivatives\n",
"\n",
"{Cl_Eaxis(\"Eaxis\", Eaxis)}\n",
"{Cl_Eaxis(\"dEaxisdFaxis\", dEaxisdFaxis)}\n",
"{Cl_Eaxis(\"ddEaxisddFaxis\", ddEaxisddFaxis)}\n",
"\n",
"// -------------------------------------------------------------------------\n",
"\"\"\""
]
},
{
Expand All @@ -55,20 +180,16 @@
"metadata": {},
"outputs": [],
"source": [
"s = f'''// Affine Body Revolute Joint Energy\n",
"{Cl(\"Hess\",Hess)}\n",
"'''\n",
"print(s)\n",
"\n",
"f = open(backend_source_dir('cuda') / 'affine_body/constitutions/sym/affine_body_revolute_joint.inl', 'w')\n",
"f.write(s)\n",
"f.close()"
"path = backend_source_dir(\"cuda\") / \"affine_body/constitutions/sym\" / \"affine_body_revolute_joint.inl\"\n",
"with open(path, \"w\") as f:\n",
" f.write(content)\n",
"print(f\"Written to {path}\")\n"
]
}
],
"metadata": {
"kernelspec": {
"display_name": "base",
"display_name": ".venv",
"language": "python",
"name": "python3"
},
Expand All @@ -82,7 +203,7 @@
"name": "python",
"nbconvert_exporter": "python",
"pygments_lexer": "ipython3",
"version": "3.12.7"
"version": "3.11.15"
}
},
"nbformat": 4,
Expand Down
Loading
Loading