{ "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 math import pi\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", "import Karana.KUtils as ku\n", "from Karana.KUtils import BasicPrefab\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 = ku.BasicPrefab.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 }