{ "cells": [ { "cell_type": "markdown", "id": "f96fd41d", "metadata": { "collapsed": false, "jupyter": { "outputs_hidden": false } }, "source": [ "# Robotics Algorithms\n", "The purpose of this notebook is to demonstrate how **kdflex**'s various algorithms can be used. This notebook uses the Barret WAM arm to showcase these algorithms.\n", "\n", "Resources\n", "- [Other notebook examples](https://portal.karanadyn.com/docs/examples_page.html) \n", "- [kdFlex Documentation](https://portal.karanadyn.com/docs/index.html)\n", "\n", "**Acknowledgements**\n", "The Barret WAM urdf is sourced from the public repository [barrett-ros2-pkg](https://git.barrett.com/software/barrett-ros2-pkg.git). Thank you to the authors and contributors." ] }, { "cell_type": "markdown", "id": "5978859c", "metadata": { "collapsed": false, "jupyter": { "outputs_hidden": false } }, "source": [ "Import **kdFlex** Python modules objects for use later" ] }, { "cell_type": "code", "execution_count": 1, "id": "dce602b8", "metadata": { "collapsed": false, "jupyter": { "outputs_hidden": false } }, "outputs": [], "source": [ "import atexit\n", "import numpy as np\n", "from pathlib import Path\n", "\n", "import Karana.Core as kc\n", "import Karana.Frame as kf\n", "import Karana.Dynamics as kd\n", "from Karana.KUtils.BasicPrefab import BasicPrefabDS\n", "import Karana.Dynamics.SOADyn_types as kdt" ] }, { "cell_type": "markdown", "id": "bcb35c5b", "metadata": { "collapsed": false, "jupyter": { "outputs_hidden": false } }, "source": [ "Build the filepath to the URDF file we will import" ] }, { "cell_type": "code", "execution_count": 2, "id": "fd13f1bc", "metadata": { "collapsed": false, "jupyter": { "outputs_hidden": false } }, "outputs": [], "source": [ "resources = Path().absolute().parent / \"resources\"\n", "urdf_file = resources / \"wam7dof\" / \"wam7dof_hand.urdf\"" ] }, { "cell_type": "markdown", "id": "64ddd559", "metadata": { "collapsed": false, "jupyter": { "outputs_hidden": false } }, "source": [ "Import the URDF file to create a SOADyn_types.MultibodyDS. The \"DS\" is short for \"Data Structure\". This is kdFlex's native, file-format agnostic way of specifying a multibody. We'll use this later on to instantiate the multibody." ] }, { "cell_type": "code", "execution_count": 3, "id": "8e99d2b9", "metadata": { "collapsed": false, "jupyter": { "outputs_hidden": false } }, "outputs": [ { "name": "stdout", "output_type": "stream", "text": [ "LEGEND: [ ] [/num embedded bodies > 0] 0>\n", "|-wam7dof_MBVROOT_\n", " |-[REVOLUTE] FirstLink\n", " |-[REVOLUTE] SecondLink\n", " |-[REVOLUTE] ThirdLink\n", " |-[REVOLUTE] FourthLink\n", " |-[REVOLUTE] FifthLink\n", " |-[REVOLUTE] SixthLink\n", " |-[REVOLUTE] SeventhLink\n", " |-[LOCKED] bhand_base\n", " |-[REVOLUTE] bhand_link11\n", " |-[REVOLUTE] bhand_link12\n", " |-[REVOLUTE] bhand_link13\n", " |-[REVOLUTE] bhand_link21\n", " |-[REVOLUTE] bhand_link22\n", " |-[REVOLUTE] bhand_link23\n", " |-[REVOLUTE] bhand_link32\n", " |-[REVOLUTE] bhand_link33\n", "\n" ] } ], "source": [ "sg_ds = BasicPrefabDS.fromFile(urdf_file).params.subtree\n", "\n", "frame_container = kf.FrameContainer(\"root\")\n", "sg_ds2 = kdt.SubGraphDS(name=sg_ds.name, base_bodies=sg_ds.base_bodies)\n", "multibody = sg_ds2.toMultibody(frame_container)\n", "\n", "# print(multibody_info)\n", "\n", "multibody.ensureHealthy()\n", "multibody.resetData()\n", "multibody.dumpTree()" ] }, { "cell_type": "markdown", "id": "97db71f8", "metadata": { "collapsed": false, "jupyter": { "outputs_hidden": false } }, "source": [ "Set up the 3D graphics." ] }, { "cell_type": "code", "execution_count": 4, "id": "024e4376", "metadata": { "collapsed": false, "jupyter": { "outputs_hidden": false } }, "outputs": [ { "data": { "text/html": [ "\n", "

\n", " \n", "

\n", " " ], "text/plain": [ "" ] }, "metadata": {}, "output_type": "display_data" } ], "source": [ "cleanup_graphics, web_scene = multibody.setupGraphics(port=0)\n", "web_scene.defaultCamera().pointCameraAt(\n", " offset=[0.2, 0.5, 1.7],\n", " target=[0, 0, 1],\n", " up=[1, 0, 0],\n", ")\n", "proxy_scene = multibody.getScene()" ] }, { "cell_type": "markdown", "id": "2b90d05e", "metadata": { "collapsed": false, "jupyter": { "outputs_hidden": false } }, "source": [ "Display the bodies in the multibody model." ] }, { "cell_type": "code", "execution_count": 5, "id": "e9ab8bad", "metadata": { "collapsed": false, "jupyter": { "outputs_hidden": false } }, "outputs": [ { "name": "stdout", "output_type": "stream", "text": [ "[WebUI] Web server is running on port 38295\n", "You may be able to connect in your browser at:\n", "\t\u001b[1mhttp://localhost:38295\u001b[0m\n", "\n", "LEGEND: [prescribed subhinges] [flex dofs/offset]\n", "\n", " Body Parent Hinge Dofs \n", " ____ ______ _____ ____ \n", " 1. FirstLink wam7dof_MBVROOT_ REVOLUTE 1/0 \n", " 2. SecondLink FirstLink REVOLUTE 1/1 \n", " 3. ThirdLink SecondLink REVOLUTE 1/2 \n", " 4. FourthLink ThirdLink REVOLUTE 1/3 \n", " 5. FifthLink FourthLink REVOLUTE 1/4 \n", " 6. SixthLink FifthLink REVOLUTE 1/5 \n", " 7. SeventhLink SixthLink REVOLUTE 1/6 \n", " 8. bhand_base SeventhLink LOCKED 0/7 \n", " 9. bhand_link11 bhand_base REVOLUTE 1/7 \n", " 10. bhand_link12 bhand_link11 REVOLUTE 1/8 \n", " 11. bhand_link13 bhand_link12 REVOLUTE 1/9 \n", " 12. bhand_link21 bhand_base REVOLUTE 1/10 \n", " 13. bhand_link22 bhand_link21 REVOLUTE 1/11 \n", " 14. bhand_link23 bhand_link22 REVOLUTE 1/12 \n", " 15. bhand_link32 bhand_base REVOLUTE 1/13 \n", " 16. bhand_link33 bhand_link32 REVOLUTE 1/14 \n", " ____ \n", " 15 \n", "\n" ] } ], "source": [ "multibody.displayModel()" ] }, { "cell_type": "markdown", "id": "77a82391", "metadata": { "collapsed": false, "jupyter": { "outputs_hidden": false } }, "source": [ "Display the topological structure of the multibody system" ] }, { "cell_type": "markdown", "id": "f4914aa3", "metadata": { "collapsed": false, "jupyter": { "outputs_hidden": false } }, "source": [ "# Testing robotics algorithms" ] }, { "cell_type": "code", "execution_count": 6, "id": "0ada5435-e53b-4018-b1e6-7d9b57b5f716", "metadata": { "collapsed": false, "jupyter": { "outputs_hidden": false } }, "outputs": [ { "name": "stdout", "output_type": "stream", "text": [ "Uniform Gravity Acceleration Set\n", "Udot: [0. 0. 0. 0. 0. 0. 0. 0. 0. 0. 0. 0. 0. 0. 0.]\n" ] } ], "source": [ "multibody.setUniformGravAccel([0, 0, 9.8])\n", "\n", "print(\"Uniform Gravity Acceleration Set\")\n", "print(f\"Udot: {multibody.getUdot()}\")" ] }, { "cell_type": "code", "execution_count": 7, "id": "ca165b01-b2aa-4485-86c8-b440be76cfb0", "metadata": { "collapsed": false, "jupyter": { "outputs_hidden": false } }, "outputs": [ { "name": "stdout", "output_type": "stream", "text": [ "Without Gravity Compensation\n", "Udot: [-1.25788279e-04 -1.24733017e+01 3.58365891e-02 4.93454986e+01\n", " -8.47109747e+01 -3.70940309e+01 8.46752641e+01 4.09569893e-07\n", " -1.68881352e-01 -5.29529891e-02 4.09569893e-07 -1.68881352e-01\n", " -5.29529892e-02 2.74942539e-01 -5.31081971e-02]\n" ] } ], "source": [ "print(\"Without Gravity Compensation\")\n", "multibody.setUdot(0)\n", "kd.Algorithms.evalForwardDynamics(multibody)\n", "print(f\"Udot: {multibody.getUdot()}\")\n", "assert not np.allclose(multibody.getUdot(), 0)" ] }, { "cell_type": "code", "execution_count": 8, "id": "7a490185-2e1b-4f06-a452-e1871b9236a8", "metadata": { "collapsed": false, "jupyter": { "outputs_hidden": false } }, "outputs": [ { "name": "stdout", "output_type": "stream", "text": [ "With Gravity Compensation\n", "Gravity Compensation Torque: [ 8.91121417e-05 4.24085429e-01 -2.44276030e-07 -2.63264584e+00\n", " 3.16021202e-06 1.66622295e-01 6.11944300e-07 -6.11951481e-07\n", " -6.86000000e-02 0.00000000e+00 -6.11951481e-07 -6.86000000e-02\n", " 0.00000000e+00 -6.86000000e-02 0.00000000e+00]\n", "Udot: [ 0.00000000e+00 0.00000000e+00 3.11755858e-15 -9.76298595e-19\n", " -3.09934158e-15 -8.18770417e-18 -1.82169618e-17 7.78027041e-23\n", " -9.18981707e-18 2.91143785e-21 7.78027041e-23 -9.18980914e-18\n", " 2.90350589e-21 9.19042584e-18 -3.52021250e-21]\n" ] } ], "source": [ "print(\"With Gravity Compensation\")\n", "multibody.setUdot(0)\n", "T = kd.Algorithms.evalGravityCompensation(multibody)\n", "print(f\"Gravity Compensation Torque: {T}\")\n", "multibody.setT(T)\n", "kd.Algorithms.evalForwardDynamics(multibody)\n", "print(f\"Udot: {multibody.getUdot()}\")\n", "assert np.allclose(multibody.getUdot(), 0)" ] }, { "cell_type": "code", "execution_count": 9, "id": "29e06a59", "metadata": { "collapsed": false, "jupyter": { "outputs_hidden": false } }, "outputs": [ { "name": "stdout", "output_type": "stream", "text": [ "Evaluated the Computed Torque\n", "Reset Q, U, and Udot\n", "Computed Torque: [ 8.91121417e-05 4.24085429e-01 -2.44276030e-07 -2.63264584e+00\n", " 3.16021202e-06 1.66622295e-01 6.11944300e-07 -6.11951481e-07\n", " -6.86000000e-02 0.00000000e+00 -6.11951481e-07 -6.86000000e-02\n", " 0.00000000e+00 -6.86000000e-02 0.00000000e+00]\n", "Udot: [ 0.00000000e+00 0.00000000e+00 3.11755858e-15 -9.76298595e-19\n", " -3.09934158e-15 -8.18770417e-18 -1.82169618e-17 7.78027041e-23\n", " -9.18981707e-18 2.91143785e-21 7.78027041e-23 -9.18980914e-18\n", " 2.90350589e-21 9.19042584e-18 -3.52021250e-21]\n" ] } ], "source": [ "print(\"Evaluated the Computed Torque\")\n", "print(\"Reset Q, U, and Udot\")\n", "multibody.setUdot(0)\n", "multibody.setQ(0)\n", "multibody.setU(0)\n", "multibody.setT(0)\n", "T = kd.Algorithms.evalComputedTorque(multibody)\n", "print(f\"Computed Torque: {T}\")\n", "multibody.setT(T)\n", "kd.Algorithms.evalForwardDynamics(multibody)\n", "print(f\"Udot: {multibody.getUdot()}\")\n", "assert np.allclose(multibody.getUdot(), 0)" ] }, { "cell_type": "markdown", "id": "0841ae40", "metadata": { "collapsed": false, "jupyter": { "outputs_hidden": false } }, "source": [ "# Testing other control related algorithms" ] }, { "cell_type": "code", "execution_count": null, "id": "37fa9e56", "metadata": { "collapsed": false, "jupyter": { "outputs_hidden": false } }, "outputs": [], "source": [] }, { "cell_type": "markdown", "id": "0ec19236", "metadata": { "collapsed": false, "jupyter": { "outputs_hidden": false } }, "source": [ "Cleanup" ] }, { "cell_type": "code", "execution_count": 10, "id": "136c1c30", "metadata": { "collapsed": false, "jupyter": { "outputs_hidden": false } }, "outputs": [ { "data": { "text/plain": [ "" ] }, "execution_count": 10, "metadata": {}, "output_type": "execute_result" } ], "source": [ "def cleanup():\n", " \"\"\"Cleanup the simulation.\"\"\"\n", " global sg_ds, sg_ds2, web_scene, proxy_scene\n", " del sg_ds, sg_ds2, web_scene, proxy_scene\n", "\n", " cleanup_graphics()\n", "\n", " kc.discard(multibody)\n", " kc.discard(frame_container)\n", "\n", "\n", "atexit.register(cleanup)" ] } ], "metadata": { "docs": { "tags": [ "import", "math", "robot" ] }, "kernelspec": { "argv": [ "python", "-m", "ipykernel_launcher", "-f", "{connection_file}" ], "display_name": "Python 3 (ipykernel)", "env": null, "interrupt_mode": "signal", "kernel_protocol_version": "5.5", "language": "python", "metadata": { "debugger": true }, "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.12.3" }, "name": "notebook.ipynb" }, "nbformat": 4, "nbformat_minor": 5 }