├── .gitignore ├── .travis.yml ├── Dockerfile ├── LICENSE ├── README.md ├── cartpole └── cartpole.urdf ├── drake_rigid_body_simulation_tutorial.ipynb ├── drake_symbolic_and_autodiff_tutorial.ipynb ├── drake_systems_tutorial.ipynb ├── kuka_controllers.py ├── kuka_ik.py ├── kuka_pydrake_sim.py ├── kuka_utils.py └── run_tests.sh /.gitignore: -------------------------------------------------------------------------------- 1 | *.pyc 2 | .ipynb_checkpoints 3 | view.gv 4 | view.gv.pdf 5 | -------------------------------------------------------------------------------- /.travis.yml: -------------------------------------------------------------------------------- 1 | language: python 2 | python: 3 | - 2.7 4 | 5 | sudo: false 6 | 7 | os: 8 | - linux 9 | 10 | 11 | services: 12 | - docker 13 | 14 | env: 15 | global: 16 | - CONDA_DEPS="pytest numpy" 17 | - PYTHONPATH="$HOME/underactuated:/opt/drake/lib/python2.7/site-packages" 18 | matrix: 19 | - DRAKE_URL="https://drake-packages.csail.mit.edu/drake/nightly/drake-20180604-xenial.tar.gz" 20 | 21 | install: 22 | - docker build -t test --build-arg DRAKE_URL=$DRAKE_URL . 23 | 24 | script: 25 | - docker run --name test test -------------------------------------------------------------------------------- /Dockerfile: -------------------------------------------------------------------------------- 1 | FROM ubuntu:16.04 2 | 3 | ARG DRAKE_URL 4 | 5 | RUN apt-get update 6 | RUN apt install -y sudo graphviz python-pip curl git 7 | RUN pip install pip ipykernel==4.10.0 ipython==5.5.0 jupyter graphviz meshcat numpy "tornado<6,>5" 8 | RUN curl -o drake.tar.gz $DRAKE_URL && sudo tar -xzf drake.tar.gz -C /opt 9 | RUN yes | sudo /opt/drake/share/drake/setup/install_prereqs 10 | RUN git clone https://github.com/RussTedrake/underactuated /underactuated && cd /underactuated && git checkout 17687cb52ff8febd77a8f881729317dff3ee8c67 11 | RUN yes | sudo /underactuated/scripts/setup/ubuntu/16.04/install_prereqs 12 | RUN apt install -y python-tk xvfb mesa-utils libegl1-mesa libgl1-mesa-glx libglu1-mesa libx11-6 x11-common x11-xserver-utils 13 | 14 | ENV PYTHONPATH=/underactuated/src:/opt/drake/lib/python2.7/site-packages 15 | COPY ./ /test_dir 16 | 17 | ENTRYPOINT bash -c "/test_dir/run_tests.sh" 18 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2018 Gregory Izatt 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Drake Concepts Tutorial 2 | 3 | [![Build Status](https://travis-ci.org/gizatt/drake_periscope_tutorial.svg?branch=master)](https://travis-ci.org/gizatt/drake_periscope_tutorial) 4 | 5 | *(Build tested on [Drake binaries from 20180604](https://drake-packages.csail.mit.edu/drake/nightly/drake-20180604-xenial.tar.gz).)* 6 | 7 | *This tutorial is now vastly out of date. See the Deprecation Information section for details of what you can trust here, and where else you should be looking.* 8 | 9 | This repo contains tutorial info for Drake (see http://drake.mit.edu). It's a sort of code equivalent or companion to [this guide](https://docs.google.com/document/d/16gUlJtwtPeNNLs7vk6IbuXXYKyJTdhoEt8BnXbWg52Y/edit?usp=sharing). 10 | 11 | ## DEPRECATION INFORMATION 12 | 13 | You can hopefully trust this repo to keep working, as it's under CI against pegged (i.e. old!) Drake + support-code versions. The system block workflow and system architecture for a simple manipulation system is still relevant and informative to peruse. However, a few core underlying things have changed: 14 | 15 | - Modern Drake has transitioned from RigidBodyTree to MultiBodyPlant. The interfaces are similar, but MBP is the cleaner/faster/fancier "modern" version. 16 | - Misc API in this repo has changed or gone out of date, especially as pertains to Abstract-valued input and State, but probably also lots of other little things. 17 | - Many conveniences have been introduced in Drake that replace large chunks of the code here (like the Kuka controller code here). 18 | 19 | To see a more modern example of this kind of system, you can check out *(updated Apr 10, 2019)*: 20 | - [MIT's 6.881 coursework](https://manipulation.csail.mit.edu/) -- everything needed to follow along with the assignments, which are all based around a simulation of a Kuka arm doing manipulation tasks, should be public. 21 | - The Drake [ManipulationStation](https://github.com/RobotLocomotion/drake/tree/master/examples/manipulation_station) example, which is the actively maintained kernel of what supported 6.881. The Python example files can be run as long as Drake is installed on your system (and is on your PYTHONPATH). The C++ examples can be run if you build Drake from source. Instructions for doing both of those things are [here](https://drake.mit.edu/installation.html). 22 | 23 | 24 | ## PREREQS 25 | 26 | You'll have to install Drake (from binaries or source, your choice) following the instructions on the Drake website, or in [this guide](https://docs.google.com/document/d/16gUlJtwtPeNNLs7vk6IbuXXYKyJTdhoEt8BnXbWg52Y/edit?usp=sharing). Note that this does *not* work on recent Drake versions any more; for the best experience, use the [Drake binaries from 20180604](https://drake-packages.csail.mit.edu/drake/nightly/drake-20180604-xenial.tar.gz). For a codebase similar to this maintained against up-to-date Drake revision, see [this repo](https://github.com/gizatt/pydrake_kuka) or, better yet, the [codebase maintained by MIT's 6.881 course](https://github.com/RobotLocomotion/6-881-examples). 27 | 28 | You'll also need to use [jupyter](http://jupyter-notebook-beginner-guide.readthedocs.io/en/latest/what_is_jupyter.html) to view the notebook (.ipynb) files, [graphviz](https://pypi.org/project/graphviz/), and [meshcat-python](https://github.com/rdeits/meshcat-python) to view the 3D visualizations in some of the examples. You'll also need some more standard libraries (e.g. numpy). You can install all of these with 29 | 30 | ``` 31 | apt-get install graphviz 32 | pip install jupyter graphviz meshcat numpy matplotlib 33 | ``` 34 | 35 | And finally, you'll need to have the [Drake textbook example code](https://github.com/RussTedrake/underactuated) available and on your PYTHONPATH. Due to some deprecations, you'll need to checkout an old-ish version -- you can pull it down with 36 | 37 | ``` 38 | git clone https://github.com/RussTedrake/underactuated ~/underactuated && cd ~/underactuated && git checkout 17687cb52ff8febd77a8f881729317dff3ee8c67 39 | ``` 40 | 41 | and add it to your PYTHONPATH with 42 | 43 | ``` 44 | export PYTHONPATH=~/underactuated/src:$PYTHONPATH 45 | ``` 46 | 47 | (you probably want to add that to the end of your `~/.bashrc`). 48 | 49 | ## USE 50 | 51 | To view the notebook files, use the command `jupyter notebook` from a terminal in the same directory (or a parent directory) as the notebook files. Then use the web-browser-based notebook browser to open up the notebook files, and use the notebook interface to play with the notebook. See a [guide like this one](http://jupyter-notebook-beginner-guide.readthedocs.io/en/latest/what_is_jupyter.html) for info on how to use the Jupyter notebook. Be sure to have both [Drake](http://drake.mit.edu/python_bindings.html) and the textbook code on your PYTHONPATH before launching jupyter! 52 | 53 | To run the Kuka simulation, first run `meshcat-server` in a new terminal. It should report a web-url -- something like `127.0.0.1:7000/static/`. Open that in a browser -- this is your 3D viewer. Then run `python kuka_pydrake_sim.py` and you should see the arm spawn in the viewer before doing some movements. 54 | -------------------------------------------------------------------------------- /cartpole/cartpole.urdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | 84 | 85 | 86 | 87 | 88 | 89 | 90 | 91 | 92 | 93 | 94 | 95 | 96 | 97 | 98 | 99 | 1 100 | 101 | 102 | -------------------------------------------------------------------------------- /drake_rigid_body_simulation_tutorial.ipynb: -------------------------------------------------------------------------------- 1 | { 2 | "cells": [ 3 | { 4 | "cell_type": "markdown", 5 | "metadata": {}, 6 | "source": [ 7 | "# Rigid Body Dynamics and Simulation\n", 8 | "\n", 9 | "Finally, let's see how to start assembling the tools mentioned in the Systems and Symbolic/Autodiff tutorials to make robots do interesting things.\n", 10 | "\n", 11 | "For these examples, we'll explore simulating the ultra-classic cart pole, pictured below.\n", 12 | "\n", 13 | "\"drawing\"" 14 | ] 15 | }, 16 | { 17 | "cell_type": "code", 18 | "execution_count": 14, 19 | "metadata": {}, 20 | "outputs": [ 21 | { 22 | "name": "stdout", 23 | "output_type": "stream", 24 | "text": [ 25 | "nq: 2\n", 26 | "nv: 2\n" 27 | ] 28 | } 29 | ], 30 | "source": [ 31 | "import math\n", 32 | "import matplotlib.pyplot as plt\n", 33 | "import numpy as np\n", 34 | "\n", 35 | "from pydrake.all import (BasicVector, DiagramBuilder, FloatingBaseType,\n", 36 | " LinearQuadraticRegulator, RigidBodyPlant,\n", 37 | " RigidBodyTree, Simulator, SignalLogger)\n", 38 | "from underactuated import (PlanarRigidBodyVisualizer)\n", 39 | "from IPython.display import HTML\n", 40 | "\n", 41 | "# Load in the cartpole from its URDF\n", 42 | "tree = RigidBodyTree(\"cartpole/cartpole.urdf\",\n", 43 | " FloatingBaseType.kFixed)\n", 44 | "\n", 45 | "# Define an upright state\n", 46 | "def UprightState():\n", 47 | " state = (0, math.pi, 0, 0)\n", 48 | " return state\n", 49 | "\n", 50 | "print \"nq: \", tree.get_num_positions()\n", 51 | "print \"nv: \", tree.get_num_velocities()" 52 | ] 53 | }, 54 | { 55 | "cell_type": "markdown", 56 | "metadata": {}, 57 | "source": [ 58 | "That RigidBodyTree keeps track of the kinematics and dynamics (minus contact interactions) of the robot. You can use it to, for example, calculate forward kinematics:" 59 | ] 60 | }, 61 | { 62 | "cell_type": "code", 63 | "execution_count": 27, 64 | "metadata": {}, 65 | "outputs": [ 66 | { 67 | "name": "stdout", 68 | "output_type": "stream", 69 | "text": [ 70 | "[[-6.123234e-17]\n", 71 | " [ 0.000000e+00]\n", 72 | " [ 7.500000e-01]]\n" 73 | ] 74 | } 75 | ], 76 | "source": [ 77 | "kinsol = tree.doKinematics(UprightState())\n", 78 | "world_body_index = tree.world().get_body_index()\n", 79 | "target_body_index = tree.FindBody(\"pole\").get_body_index()\n", 80 | "end_of_pole_in_world_frame = tree.transformPoints(kinsol, [0., 0., -0.5], target_body_index, world_body_index)\n", 81 | "print end_of_pole_in_world_frame" 82 | ] 83 | }, 84 | { 85 | "cell_type": "markdown", 86 | "metadata": {}, 87 | "source": [ 88 | "As a more complete demo, we can create an LQR solution around that upright fixed point and simulate it!\n", 89 | "\n", 90 | "See the quickstart guide for a written explanation of the many pieces of this." 91 | ] 92 | }, 93 | { 94 | "cell_type": "code", 95 | "execution_count": 10, 96 | "metadata": {}, 97 | "outputs": [ 98 | { 99 | "name": "stdout", 100 | "output_type": "stream", 101 | "text": [ 102 | "Spawning PlanarRigidBodyVisualizer for tree with 1 actuators\n" 103 | ] 104 | }, 105 | { 106 | "data": { 107 | "text/html": [ 108 | "" 516 | ], 517 | "text/plain": [ 518 | "" 519 | ] 520 | }, 521 | "execution_count": 10, 522 | "metadata": {}, 523 | "output_type": "execute_result" 524 | } 525 | ], 526 | "source": [ 527 | "def BalancingLQR(robot):\n", 528 | " # Design an LQR controller for stabilizing the CartPole around the upright.\n", 529 | " # Returns a (static) AffineSystem that implements the controller (in\n", 530 | " # the original CartPole coordinates).\n", 531 | "\n", 532 | " context = robot.CreateDefaultContext()\n", 533 | " context.FixInputPort(0, BasicVector([0]))\n", 534 | "\n", 535 | " context.get_mutable_continuous_state_vector().SetFromVector(UprightState())\n", 536 | "\n", 537 | " Q = np.diag((10., 10., 1., 1.))\n", 538 | " R = [1]\n", 539 | "\n", 540 | " return LinearQuadraticRegulator(robot, context, Q, R)\n", 541 | "\n", 542 | "\n", 543 | "builder = DiagramBuilder()\n", 544 | "\n", 545 | "robot = builder.AddSystem(RigidBodyPlant(tree))\n", 546 | "controller = builder.AddSystem(BalancingLQR(robot))\n", 547 | "builder.Connect(robot.get_output_port(0), controller.get_input_port(0))\n", 548 | "builder.Connect(controller.get_output_port(0), robot.get_input_port(0))\n", 549 | "\n", 550 | "logger = builder.AddSystem(SignalLogger(robot.get_output_port(0).size()))\n", 551 | "logger._DeclarePeriodicPublish(1. / 30., 0.0)\n", 552 | "builder.Connect(robot.get_output_port(0), logger.get_input_port(0))\n", 553 | "\n", 554 | "diagram = builder.Build()\n", 555 | "simulator = Simulator(diagram)\n", 556 | "simulator.set_publish_every_time_step(False)\n", 557 | "context = simulator.get_mutable_context()\n", 558 | "\n", 559 | "state = context.get_mutable_continuous_state_vector()\n", 560 | "state.SetFromVector(UprightState() + 0.1*np.random.randn(4,))\n", 561 | "simulator.StepTo(10.)\n", 562 | "\n", 563 | "prbv = PlanarRigidBodyVisualizer(tree, xlim=[-2.5, 2.5], ylim=[-1, 2.5])\n", 564 | "ani = prbv.animate(logger, resample=30, repeat=True)\n", 565 | "plt.close(prbv.fig)\n", 566 | "HTML(ani.to_html5_video())" 567 | ] 568 | } 569 | ], 570 | "metadata": { 571 | "kernelspec": { 572 | "display_name": "Python 2", 573 | "language": "python", 574 | "name": "python2" 575 | }, 576 | "language_info": { 577 | "codemirror_mode": { 578 | "name": "ipython", 579 | "version": 2 580 | }, 581 | "file_extension": ".py", 582 | "mimetype": "text/x-python", 583 | "name": "python", 584 | "nbconvert_exporter": "python", 585 | "pygments_lexer": "ipython2", 586 | "version": "2.7.14" 587 | } 588 | }, 589 | "nbformat": 4, 590 | "nbformat_minor": 2 591 | } 592 | -------------------------------------------------------------------------------- /drake_symbolic_and_autodiff_tutorial.ipynb: -------------------------------------------------------------------------------- 1 | { 2 | "cells": [ 3 | { 4 | "cell_type": "code", 5 | "execution_count": 1, 6 | "metadata": {}, 7 | "outputs": [], 8 | "source": [ 9 | "%load_ext autoreload\n", 10 | "%autoreload 2\n", 11 | "import matplotlib as mpl\n", 12 | "mpl.rcParams['axes.grid'] = True" 13 | ] 14 | }, 15 | { 16 | "cell_type": "markdown", 17 | "metadata": {}, 18 | "source": [ 19 | "# Symbolic Computation and Optimization\n", 20 | "\n", 21 | "Core idea: make your program keep track of what computations you have performed." 22 | ] 23 | }, 24 | { 25 | "cell_type": "code", 26 | "execution_count": 2, 27 | "metadata": {}, 28 | "outputs": [ 29 | { 30 | "name": "stdout", 31 | "output_type": "stream", 32 | "text": [ 33 | "z: (x + pow(y, 2))\n", 34 | "formula: ((x + pow(y, 2)) = 1)\n" 35 | ] 36 | } 37 | ], 38 | "source": [ 39 | "from pydrake.all import (\n", 40 | " Variable\n", 41 | ")\n", 42 | "# Declare some Variables\n", 43 | "x = Variable(\"x\")\n", 44 | "y = Variable(\"y\")\n", 45 | "# Declare an Expression\n", 46 | "z = x + y**2\n", 47 | "print \"z: \", z\n", 48 | "# Declare a Formula using z\n", 49 | "z_equals_one = (z == 1)\n", 50 | "print \"formula: \", z_equals_one" 51 | ] 52 | }, 53 | { 54 | "cell_type": "markdown", 55 | "metadata": {}, 56 | "source": [ 57 | "You can do simple things, like do evaluations or substitutions..." 58 | ] 59 | }, 60 | { 61 | "cell_type": "code", 62 | "execution_count": 3, 63 | "metadata": {}, 64 | "outputs": [ 65 | { 66 | "name": "stdout", 67 | "output_type": "stream", 68 | "text": [ 69 | "Using x = 3.000000, y = 4.000000...\n", 70 | "Partial evaluation (for only x): z = (3 + pow(y, 2))\n", 71 | "Partial evaluation (for only y): z = (16 + x)\n", 72 | "Full evaluation: z = 19.0\n", 73 | "Substituting x = y^2: z = (2 * pow(y, 2))\n" 74 | ] 75 | } 76 | ], 77 | "source": [ 78 | "xi = 3.\n", 79 | "yi = 4.\n", 80 | "print \"Using x = %f, y = %f...\" % (xi, yi)\n", 81 | "# Partial evaluation (specifying *some* variables)\n", 82 | "print \"Partial evaluation (for only x): z = \", z.EvaluatePartial({x:xi})\n", 83 | "print \"Partial evaluation (for only y): z = \", z.EvaluatePartial({y:yi})\n", 84 | "# Full evaluation (specifying *all* variables)\n", 85 | "print \"Full evaluation: z = \", z.Evaluate({x:xi, y:yi})\n", 86 | "print \"Substituting x = y^2: z = \", z.Substitute({x:y**2})" 87 | ] 88 | }, 89 | { 90 | "cell_type": "markdown", 91 | "metadata": {}, 92 | "source": [ 93 | "And you can ask for derivatives..." 94 | ] 95 | }, 96 | { 97 | "cell_type": "code", 98 | "execution_count": 4, 99 | "metadata": {}, 100 | "outputs": [ 101 | { 102 | "name": "stdout", 103 | "output_type": "stream", 104 | "text": [ 105 | "Derivative of z w.r.t. x: 1\n", 106 | "Derivative of z w.r.t. y: (2 * y)\n", 107 | "2nd derivative of z w.r.t. y: 2\n", 108 | "You can ask for Jacobians if you want: [ ]\n" 109 | ] 110 | } 111 | ], 112 | "source": [ 113 | "print \"Derivative of z w.r.t. x: \", z.Differentiate(x)\n", 114 | "print \"Derivative of z w.r.t. y: \", z.Differentiate(y)\n", 115 | "print \"2nd derivative of z w.r.t. y: \", z.Differentiate(y).Differentiate(y)\n", 116 | "print \"You can ask for Jacobians if you want: \", z.Jacobian([x, y])" 117 | ] 118 | }, 119 | { 120 | "cell_type": "markdown", 121 | "metadata": {}, 122 | "source": [ 123 | "## Mathematical Programming\n", 124 | "\n", 125 | "As you might guess, you can build this up to cool stuff! The primary thing we use this for is setting up and solving optimizations. By specifying an optimization (i.e. an objective + a list of constraints) symbolically, Drake figures out what class of optimization it is and dispatches to an appropriate solver.\n", 126 | "\n", 127 | "http://drake.mit.edu/doxygen_cxx/group__solvers.html" 128 | ] 129 | }, 130 | { 131 | "cell_type": "code", 132 | "execution_count": 10, 133 | "metadata": {}, 134 | "outputs": [ 135 | { 136 | "name": "stdout", 137 | "output_type": "stream", 138 | "text": [ 139 | "Result: SolutionResult.kSolutionFound\n", 140 | "Solver used: OSQP\n", 141 | "Optimizing x: 0.0\n" 142 | ] 143 | } 144 | ], 145 | "source": [ 146 | "from pydrake.all import (MathematicalProgram)\n", 147 | "prog = MathematicalProgram()\n", 148 | "# NewContinuousVariables spawns a 1x1 array of decision\n", 149 | "# variables. We're more concerned with individual variables\n", 150 | "# for this example, so take out the first element of that\n", 151 | "# array to get the actual Variable.\n", 152 | "x = prog.NewContinuousVariables(1, \"x\")[0]\n", 153 | "prog.AddQuadraticCost(x**2)\n", 154 | "prog.AddBoundingBoxConstraint(0., 1., x)\n", 155 | "result = prog.Solve()\n", 156 | "print \"Result: \", result\n", 157 | "print \"Solver used: \", prog.GetSolverId().name()\n", 158 | "print \"Optimizing x: \", prog.GetSolution(x)" 159 | ] 160 | }, 161 | { 162 | "cell_type": "code", 163 | "execution_count": 11, 164 | "metadata": {}, 165 | "outputs": [ 166 | { 167 | "name": "stdout", 168 | "output_type": "stream", 169 | "text": [ 170 | "Result: SolutionResult.kSolutionFound\n", 171 | "Solver used: SNOPT\n", 172 | "Optimizing x: 0.0\n" 173 | ] 174 | } 175 | ], 176 | "source": [ 177 | "prog = MathematicalProgram()\n", 178 | "x = prog.NewContinuousVariables(1, \"x\")[0]\n", 179 | "prog.AddCost(x**4)\n", 180 | "prog.AddBoundingBoxConstraint(0., 1., x)\n", 181 | "result = prog.Solve()\n", 182 | "print \"Result: \", result\n", 183 | "print \"Solver used: \", prog.GetSolverId().name()\n", 184 | "print \"Optimizing x: \", prog.GetSolution(x)" 185 | ] 186 | }, 187 | { 188 | "cell_type": "markdown", 189 | "metadata": {}, 190 | "source": [ 191 | "## SOS Stability Analysis Example\n", 192 | "\n", 193 | "As you might guess, you can build this up to cool stuff! For example, after defining some dynamics, you can pass symbolic variables through it, and then do stability analysis." 194 | ] 195 | }, 196 | { 197 | "cell_type": "code", 198 | "execution_count": 7, 199 | "metadata": {}, 200 | "outputs": [ 201 | { 202 | "data": { 203 | "image/png": "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\n", 204 | "text/plain": [ 205 | "" 206 | ] 207 | }, 208 | "metadata": {}, 209 | "output_type": "display_data" 210 | } 211 | ], 212 | "source": [ 213 | "# Define dynamics\n", 214 | "def dynamics(x):\n", 215 | " return -x**3 - x\n", 216 | "\n", 217 | "# Do some visual inspection of this system\n", 218 | "import matplotlib.pyplot as plt\n", 219 | "import numpy as np\n", 220 | "from pydrake.all import sin\n", 221 | "x_sample = np.linspace(-1, 1, 1000)\n", 222 | "f = np.array([dynamics(x_i) for x_i in x_sample])\n", 223 | "plt.plot(x_sample, f)\n", 224 | "plt.xlabel(\"x\")\n", 225 | "plt.ylabel(\"x dot\")\n", 226 | "plt.title(\"xdot = \" + str(dynamics(Variable(\"x\"))));" 227 | ] 228 | }, 229 | { 230 | "cell_type": "code", 231 | "execution_count": 8, 232 | "metadata": {}, 233 | "outputs": [ 234 | { 235 | "name": "stdout", 236 | "output_type": "stream", 237 | "text": [ 238 | "Verified globally stable!\n", 239 | "Resulting Lyapunov function: 0.010768550023664367*x(0)^2 + -8.3573990004666846e-19*x(0)^3 + 0.012087454329122366*x(0)^4 + -2.5625928704208748e-14*1 + -2.7488648052396447e-18*x(0)\n" 240 | ] 241 | }, 242 | { 243 | "data": { 244 | "image/png": "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\n", 245 | "text/plain": [ 246 | "" 247 | ] 248 | }, 249 | "metadata": {}, 250 | "output_type": "display_data" 251 | } 252 | ], 253 | "source": [ 254 | "from pydrake.all import (\n", 255 | " MathematicalProgram,\n", 256 | " Polynomial,\n", 257 | " SolutionResult,\n", 258 | " Variables\n", 259 | ")\n", 260 | "prog = MathematicalProgram()\n", 261 | "x = prog.NewIndeterminates(1, \"x\")[0]\n", 262 | "\n", 263 | "# Search over Lyapunov candidates\n", 264 | "(V, constraint) = prog.NewSosPolynomial(Variables([x]), 4)\n", 265 | "# V(0) = 0\n", 266 | "prog.AddConstraint(V.ToExpression().EvaluatePartial({x: 0.}) == 0.)\n", 267 | "# V radially unbounded\n", 268 | "prog.AddSosConstraint(V - 0.01*Polynomial(x**2))\n", 269 | "\n", 270 | "# Calculate Vdot = dV/dx dx/dt\n", 271 | "# Because V is a polynomial, its Jacobian is a Polynomial.\n", 272 | "# To keep Vdot as a polynomial, we convert the dynamics to a\n", 273 | "# polynomial as well, which will fail if you specify non-polynomial\n", 274 | "# dynamics.\n", 275 | "Vdot = V.Jacobian([x]).dot(Polynomial(dynamics(x)))[0]\n", 276 | "# Vdot negative semidef\n", 277 | "# (assume dynamics(0) = 0) so skip that constraint\n", 278 | "prog.AddSosConstraint(-1.*Vdot)\n", 279 | "\n", 280 | "result = prog.Solve()\n", 281 | "if result == SolutionResult.kSolutionFound:\n", 282 | " print \"Verified globally stable!\"\n", 283 | " V_result = prog.SubstituteSolution(V)\n", 284 | " Vd_result = prog.SubstituteSolution(Vdot)\n", 285 | " print \"Resulting Lyapunov function: \", V_result\n", 286 | " x_sample = np.linspace(-1, 1, 1000)\n", 287 | " V_eval = np.array([V_result.Evaluate({x: x_sample_i}) for x_sample_i in x_sample])\n", 288 | " Vd_eval = np.array([Vd_result.Evaluate({x: x_sample_i}) for x_sample_i in x_sample])\n", 289 | " plt.subplot(1, 2, 1)\n", 290 | " plt.plot(x_sample, V_eval)\n", 291 | " plt.xlabel(\"x\")\n", 292 | " plt.title(\"V(x)\")\n", 293 | " plt.subplot(1, 2, 2)\n", 294 | " plt.plot(x_sample, Vd_eval)\n", 295 | " plt.xlabel(\"x\")\n", 296 | " plt.title(\"Vdot(x)\");\n", 297 | "else:\n", 298 | " print \"Could not prove globally stable.\"\n", 299 | " print \"Result: \", result" 300 | ] 301 | }, 302 | { 303 | "cell_type": "markdown", 304 | "metadata": {}, 305 | "source": [ 306 | "# Automatic Differentiation (\"autodiff\", specifically forward-mode)\n", 307 | "\n", 308 | "Core idea: Don't bother storing the complete symbolic expression (which can be huge), but instead remember your derivatives with respect to a list of variables of interest.\n", 309 | "\n", 310 | "Every Autodiff object has a value, and a list of derivatives\n", 311 | "\n", 312 | "$$\n", 313 | "\\left[ [f], [\\dfrac{df}{dx_1}, \\dfrac{df}{dx_2}, ...] \\right]\n", 314 | "$$\n", 315 | "\n", 316 | "and these objects can be combined by overloading every relevant operator." 317 | ] 318 | }, 319 | { 320 | "cell_type": "code", 321 | "execution_count": 9, 322 | "metadata": {}, 323 | "outputs": [ 324 | { 325 | "name": "stdout", 326 | "output_type": "stream", 327 | "text": [ 328 | "Input (-1.000000, -1.000000), value 2.0, derivative [-2. -2.]\n", 329 | "Input (-1.000000, 0.000000), value 1.0, derivative [-2. 0.]\n", 330 | "Input (-1.000000, 1.000000), value 2.0, derivative [-2. 2.]\n", 331 | "Input (0.000000, -1.000000), value 1.0, derivative [ 0. -2.]\n", 332 | "Input (0.000000, 0.000000), value 0.0, derivative [0. 0.]\n", 333 | "Input (0.000000, 1.000000), value 1.0, derivative [0. 2.]\n", 334 | "Input (1.000000, -1.000000), value 2.0, derivative [ 2. -2.]\n", 335 | "Input (1.000000, 0.000000), value 1.0, derivative [2. 0.]\n", 336 | "Input (1.000000, 1.000000), value 2.0, derivative [2. 2.]\n" 337 | ] 338 | } 339 | ], 340 | "source": [ 341 | "from pydrake.all import AutoDiffXd\n", 342 | "def inner_product(v):\n", 343 | " # Expects a [n x 1] array input\n", 344 | " return v.dot(v)\n", 345 | "for x in np.linspace(-1, 1., 3):\n", 346 | " for y in np.linspace(-1, 1., 3):\n", 347 | " x_ad = AutoDiffXd(x, [1., 0.])\n", 348 | " y_ad= AutoDiffXd(y, [0., 1.])\n", 349 | " f_ad = inner_product(np.array([x_ad, y_ad]))\n", 350 | " print \"Input (%f, %f), value %s, derivative %s\" % (x, y, str(f_ad.value()), str(f_ad.derivatives()))" 351 | ] 352 | }, 353 | { 354 | "cell_type": "markdown", 355 | "metadata": {}, 356 | "source": [ 357 | "Way more systems in Drake currently support Autodiff than Symbolic, though we hope to close that gap in the future!" 358 | ] 359 | } 360 | ], 361 | "metadata": { 362 | "kernelspec": { 363 | "display_name": "Python 2", 364 | "language": "python", 365 | "name": "python2" 366 | }, 367 | "language_info": { 368 | "codemirror_mode": { 369 | "name": "ipython", 370 | "version": 2 371 | }, 372 | "file_extension": ".py", 373 | "mimetype": "text/x-python", 374 | "name": "python", 375 | "nbconvert_exporter": "python", 376 | "pygments_lexer": "ipython2", 377 | "version": "2.7.14" 378 | } 379 | }, 380 | "nbformat": 4, 381 | "nbformat_minor": 2 382 | } 383 | -------------------------------------------------------------------------------- /drake_systems_tutorial.ipynb: -------------------------------------------------------------------------------- 1 | { 2 | "cells": [ 3 | { 4 | "cell_type": "markdown", 5 | "metadata": {}, 6 | "source": [ 7 | "# Block Diagram Systems\n", 8 | "\n", 9 | "Core idea: Functional blocks can be connected together, block-diagram style, to build systems of larger complexity. The fundamental unit -- a \"System\" -- has a very specific contract that supports both modularity and ease of analysis.\n", 10 | "\n", 11 | "High-level docs and solid examples:\n", 12 | "http://underactuated.csail.mit.edu/underactuated.html?chapter=systems\n", 13 | "\n", 14 | "Details docs:\n", 15 | "http://drake.mit.edu/doxygen_cxx/group__systems.html\n", 16 | "\n", 17 | "In short, systems can:\n", 18 | "- Have any number of vector-valued (or abstract-valued) inputs\n", 19 | "- Have any number of vector-valued (or abstract-valued) outputs\n", 20 | "- Have vector-valued continuous state, for which it supplies time derivatives\n", 21 | "- Have vector-valued or abstract-valued discrete state, for which it supplies update rules\n", 22 | "- Operate on float/double, but also autodiff and symbolic types.\n", 23 | "- Be assembled by connecting smaller systems.\n", 24 | "\n", 25 | "And systems must:\n", 26 | "- Be deterministic given their inputs. (Randomness must only come a special kind of input port.)\n", 27 | "- Declare certain flags (e.g. direct feedthrough)." 28 | ] 29 | }, 30 | { 31 | "cell_type": "code", 32 | "execution_count": 1, 33 | "metadata": {}, 34 | "outputs": [], 35 | "source": [ 36 | "from pydrake.all import (VectorSystem)\n", 37 | "# Subclasses VectorSystem, which provides\n", 38 | "# a convenient constructor for a System\n", 39 | "# with 1 vector-valued input and 1 vector-valued output.\n", 40 | "class SimpleContinuousTimeSystem(VectorSystem):\n", 41 | " def __init__(self):\n", 42 | " VectorSystem.__init__(self,\n", 43 | " 0, # Zero inputs.\n", 44 | " 1) # One output.\n", 45 | " self.set_name(\"Simple Continuous Time System\")\n", 46 | " self._DeclareContinuousState(1) # One state variable.\n", 47 | "\n", 48 | " # xdot(t) = -x(t) + x^3(t)\n", 49 | " def _DoCalcVectorTimeDerivatives(self, context, u, x, xdot):\n", 50 | " xdot[:] = -x + x**3\n", 51 | "\n", 52 | " # y(t) = x(t)\n", 53 | " def _DoCalcVectorOutput(self, context, u, x, y):\n", 54 | " y[:] = x\n", 55 | " \n", 56 | "# Same idea as above, but instead we create the system with\n", 57 | "# discrete state.\n", 58 | "class SimpleDiscreteTimeSystem(VectorSystem):\n", 59 | " def __init__(self):\n", 60 | " VectorSystem.__init__(self,\n", 61 | " 0, # Zero inputs.\n", 62 | " 1) # One output.\n", 63 | " self._DeclareDiscreteState(1) # One state variable.\n", 64 | " self._DeclarePeriodicDiscreteUpdate(1.0) # One second timestep.\n", 65 | "\n", 66 | " # x[n+1] = x[n]^3\n", 67 | " def _DoCalcVectorDiscreteVariableUpdates(self, context, u, x, xnext):\n", 68 | " xnext[:] = x**3\n", 69 | "\n", 70 | " # y[n] = x[n]\n", 71 | " def _DoCalcVectorOutput(self, context, u, x, y):\n", 72 | " y[:] = x" 73 | ] 74 | }, 75 | { 76 | "cell_type": "markdown", 77 | "metadata": {}, 78 | "source": [ 79 | "## Diagrams\n", 80 | "\n", 81 | "Systems on their own wouldn't be very interesting, so here's how to plug them together. In this case, we're appending a [\"SignalLogger\"](http://drake.mit.edu/doxygen_cxx/classdrake_1_1systems_1_1_signal_logger.html#details) system to record the output of the system we wrote." 82 | ] 83 | }, 84 | { 85 | "cell_type": "code", 86 | "execution_count": 2, 87 | "metadata": {}, 88 | "outputs": [ 89 | { 90 | "data": { 91 | "image/svg+xml": [ 92 | "\n", 93 | "\n", 95 | "\n", 97 | "\n", 98 | "\n", 100 | "\n", 101 | "_93895825801904\n", 102 | "\n", 103 | "cluster93895825801904diagram\n", 104 | "\n", 105 | "93895825801904\n", 106 | "\n", 107 | "cluster93895825801904subsystems\n", 108 | "\n", 109 | "\n", 110 | "\n", 111 | "93895827168272\n", 112 | "\n", 113 | "Simple Continuous Time System\n", 114 | "\n", 115 | " \n", 116 | "\n", 117 | "y0\n", 118 | "\n", 119 | "\n", 120 | "93895819107584\n", 121 | "\n", 122 | "drake/systems/SignalLogger@00005565d2f19d00\n", 123 | "\n", 124 | "u0\n", 125 | "\n", 126 | " \n", 127 | "\n", 128 | "\n", 129 | "93895827168272:y0->93895819107584:u0\n", 130 | "\n", 131 | "\n", 132 | "\n", 133 | "\n", 134 | "\n" 135 | ], 136 | "text/plain": [ 137 | "" 138 | ] 139 | }, 140 | "execution_count": 2, 141 | "metadata": {}, 142 | "output_type": "execute_result" 143 | } 144 | ], 145 | "source": [ 146 | "from pydrake.all import (DiagramBuilder, SignalLogger)\n", 147 | "# Create a simple block diagram containing our system.\n", 148 | "builder = DiagramBuilder()\n", 149 | "system = builder.AddSystem(SimpleContinuousTimeSystem())\n", 150 | "logger = builder.AddSystem(SignalLogger(1))\n", 151 | "builder.Connect(system.get_output_port(0), logger.get_input_port(0))\n", 152 | "diagram = builder.Build()\n", 153 | "\n", 154 | "# Visualize\n", 155 | "from graphviz import Source\n", 156 | "string = diagram.GetGraphvizString()\n", 157 | "Source(string)" 158 | ] 159 | }, 160 | { 161 | "cell_type": "markdown", 162 | "metadata": {}, 163 | "source": [ 164 | "## Diagrams and Simulation\n", 165 | "\n", 166 | "To make these systems do something interesting, we need to simulate them -- a combination of integrating the continuous state forward in time and keeping track of discrete state updates, while keeping inputs and outputs between systems synchonized. Drake's [Simulator](http://drake.mit.edu/doxygen_cxx/classdrake_1_1systems_1_1_simulator.html#details) handles this." 167 | ] 168 | }, 169 | { 170 | "cell_type": "code", 171 | "execution_count": 3, 172 | "metadata": {}, 173 | "outputs": [ 174 | { 175 | "data": { 176 | "image/png": "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\n", 177 | "text/plain": [ 178 | "" 179 | ] 180 | }, 181 | "metadata": {}, 182 | "output_type": "display_data" 183 | } 184 | ], 185 | "source": [ 186 | "import matplotlib.pyplot as plt\n", 187 | "from pydrake.all import (Simulator)\n", 188 | "\n", 189 | "# Create the simulator.\n", 190 | "simulator = Simulator(diagram)\n", 191 | "\n", 192 | "# Set the initial conditions, x(0).\n", 193 | "state = simulator.get_mutable_context().get_mutable_continuous_state_vector()\n", 194 | "state.SetFromVector([0.9])\n", 195 | "\n", 196 | "# Simulate for 10 seconds.\n", 197 | "simulator.StepTo(10)\n", 198 | "\n", 199 | "# Plot the results.\n", 200 | "plt.plot(logger.sample_times(), logger.data().transpose())\n", 201 | "plt.xlabel('t')\n", 202 | "plt.ylabel('x(t)');" 203 | ] 204 | } 205 | ], 206 | "metadata": { 207 | "kernelspec": { 208 | "display_name": "Python 2", 209 | "language": "python", 210 | "name": "python2" 211 | }, 212 | "language_info": { 213 | "codemirror_mode": { 214 | "name": "ipython", 215 | "version": 2 216 | }, 217 | "file_extension": ".py", 218 | "mimetype": "text/x-python", 219 | "name": "python", 220 | "nbconvert_exporter": "python", 221 | "pygments_lexer": "ipython2", 222 | "version": "2.7.14" 223 | } 224 | }, 225 | "nbformat": 4, 226 | "nbformat_minor": 2 227 | } 228 | -------------------------------------------------------------------------------- /kuka_controllers.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf8 -*- 2 | 3 | import numpy as np 4 | import pydrake 5 | from pydrake.all import ( 6 | BasicVector, 7 | LeafSystem, 8 | PortDataType, 9 | ) 10 | import kuka_utils 11 | 12 | 13 | class KukaController(LeafSystem): 14 | def __init__(self, rbt, plant, 15 | control_period=0.005, 16 | print_period=0.5): 17 | LeafSystem.__init__(self) 18 | self.set_name("Kuka Controller") 19 | 20 | self.controlled_joint_names = [ 21 | "iiwa_joint_1", 22 | "iiwa_joint_2", 23 | "iiwa_joint_3", 24 | "iiwa_joint_4", 25 | "iiwa_joint_5", 26 | "iiwa_joint_6", 27 | "iiwa_joint_7" 28 | ] 29 | 30 | self.controlled_inds, _ = kuka_utils.extract_position_indices( 31 | rbt, self.controlled_joint_names) 32 | # Extract the full-rank bit of B, and verify that it's full rank 33 | self.nq_reduced = len(self.controlled_inds) 34 | self.B = np.empty((self.nq_reduced, self.nq_reduced)) 35 | for k in range(self.nq_reduced): 36 | for l in range(self.nq_reduced): 37 | self.B[k, l] = rbt.B[self.controlled_inds[k], 38 | self.controlled_inds[l]] 39 | if np.linalg.matrix_rank(self.B) < self.nq_reduced: 40 | print "The joint set specified is underactuated." 41 | sys.exit(-1) 42 | self.B_inv = np.linalg.inv(self.B) 43 | # Copy lots of stuff 44 | self.rbt = rbt 45 | self.nq = rbt.get_num_positions() 46 | self.plant = plant 47 | self.nu = plant.get_input_port(0).size() 48 | self.print_period = print_period 49 | self.last_print_time = -print_period 50 | self.shut_up = False 51 | 52 | self.robot_state_input_port = \ 53 | self._DeclareInputPort(PortDataType.kVectorValued, 54 | rbt.get_num_positions() + 55 | rbt.get_num_velocities()) 56 | 57 | self.setpoint_input_port = \ 58 | self._DeclareInputPort(PortDataType.kVectorValued, 59 | rbt.get_num_positions() + 60 | rbt.get_num_velocities()) 61 | 62 | self._DeclareDiscreteState(self.nu) 63 | self._DeclarePeriodicDiscreteUpdate(period_sec=control_period) 64 | self._DeclareVectorOutputPort( 65 | BasicVector(self.nu), 66 | self._DoCalcVectorOutput) 67 | 68 | def _DoCalcDiscreteVariableUpdates(self, context, events, discrete_state): 69 | # Call base method to ensure we do not get recursion. 70 | # (This makes sure relevant event handlers get called.) 71 | LeafSystem._DoCalcDiscreteVariableUpdates( 72 | self, context, events, discrete_state) 73 | 74 | new_control_input = discrete_state. \ 75 | get_mutable_vector().get_mutable_value() 76 | x = self.EvalVectorInput( 77 | context, self.robot_state_input_port.get_index()).get_value() 78 | x_des = self.EvalVectorInput( 79 | context, self.setpoint_input_port.get_index()).get_value() 80 | q = x[:self.nq] 81 | v = x[self.nq:] 82 | q_des = x_des[:self.nq] 83 | v_des = x_des[self.nq:] 84 | 85 | qerr = (q_des[self.controlled_inds] - q[self.controlled_inds]) 86 | verr = (v_des[self.controlled_inds] - v[self.controlled_inds]) 87 | 88 | kinsol = self.rbt.doKinematics(q, v) 89 | # Get the full LHS of the manipulator equations 90 | # given the current config and desired accelerations 91 | vd_des = np.zeros(self.rbt.get_num_positions()) 92 | vd_des[self.controlled_inds] = 1000.*qerr + 100*verr 93 | lhs = self.rbt.inverseDynamics(kinsol, external_wrenches={}, vd=vd_des) 94 | new_u = self.B_inv.dot(lhs[self.controlled_inds]) 95 | new_control_input[:] = new_u 96 | 97 | def _DoCalcVectorOutput(self, context, y_data): 98 | if (self.print_period and 99 | context.get_time() - self.last_print_time 100 | >= self.print_period): 101 | print "t: ", context.get_time() 102 | self.last_print_time = context.get_time() 103 | control_output = context.get_discrete_state_vector().get_value() 104 | y = y_data.get_mutable_value() 105 | # Get the ith finger control output 106 | y[:] = control_output[:] 107 | 108 | 109 | class HandController(LeafSystem): 110 | def __init__(self, rbt, plant, 111 | control_period=0.001): 112 | LeafSystem.__init__(self) 113 | self.set_name("Hand Controller") 114 | 115 | self.controlled_joint_names = [ 116 | "left_finger_sliding_joint", 117 | "right_finger_sliding_joint" 118 | ] 119 | 120 | self.max_force = 100. # gripper max closing / opening force 121 | 122 | self.controlled_inds, _ = kuka_utils.extract_position_indices( 123 | rbt, self.controlled_joint_names) 124 | 125 | self.nu = plant.get_input_port(1).size() 126 | self.nq = rbt.get_num_positions() 127 | 128 | self.robot_state_input_port = \ 129 | self._DeclareInputPort(PortDataType.kVectorValued, 130 | rbt.get_num_positions() + 131 | rbt.get_num_velocities()) 132 | 133 | self.setpoint_input_port = \ 134 | self._DeclareInputPort(PortDataType.kVectorValued, 135 | 1) 136 | 137 | self._DeclareDiscreteState(self.nu) 138 | self._DeclarePeriodicDiscreteUpdate(period_sec=control_period) 139 | self._DeclareVectorOutputPort( 140 | BasicVector(self.nu), 141 | self._DoCalcVectorOutput) 142 | 143 | def _DoCalcDiscreteVariableUpdates(self, context, events, discrete_state): 144 | # Call base method to ensure we do not get recursion. 145 | # (This makes sure relevant event handlers get called.) 146 | LeafSystem._DoCalcDiscreteVariableUpdates( 147 | self, context, events, discrete_state) 148 | 149 | new_control_input = discrete_state. \ 150 | get_mutable_vector().get_mutable_value() 151 | x = self.EvalVectorInput( 152 | context, self.robot_state_input_port.get_index()).get_value() 153 | 154 | gripper_width_des = self.EvalVectorInput( 155 | context, self.setpoint_input_port.get_index()).get_value() 156 | 157 | q_full = x[:self.nq] 158 | v_full = x[self.nq:] 159 | 160 | q = q_full[self.controlled_inds] 161 | q_des = np.array([-gripper_width_des[0], gripper_width_des[0]]) 162 | v = v_full[self.controlled_inds] 163 | v_des = np.zeros(2) 164 | 165 | qerr = q_des - q 166 | verr = v_des - v 167 | 168 | Kp = 1000. 169 | Kv = 100. 170 | new_control_input[:] = np.clip( 171 | Kp * qerr + Kv * verr, -self.max_force, self.max_force) 172 | 173 | def _DoCalcVectorOutput(self, context, y_data): 174 | control_output = context.get_discrete_state_vector().get_value() 175 | y = y_data.get_mutable_value() 176 | # Get the ith finger control output 177 | y[:] = control_output[:] 178 | 179 | 180 | class ManipStateMachine(LeafSystem): 181 | ''' Encodes the high-level logic 182 | for the manipulation system. 183 | 184 | This implementation is fairly minimal. 185 | It is supplied with an open-loop 186 | trajectory (presumably, to grasp the object from a 187 | known position). At runtime, it spools 188 | out pose goals for the robot according to 189 | this trajectory. Once the trajectory has been 190 | executed, it closes the gripper, waits 191 | a second, and then plays the trajectory back in reverse 192 | to bring the robot back to its original posture. 193 | ''' 194 | def __init__(self, rbt, plant, qtraj): 195 | LeafSystem.__init__(self) 196 | self.set_name("Manipulation State Machine") 197 | 198 | self.qtraj = qtraj 199 | 200 | self.rbt = rbt 201 | self.nq = rbt.get_num_positions() 202 | self.plant = plant 203 | 204 | self.robot_state_input_port = \ 205 | self._DeclareInputPort(PortDataType.kVectorValued, 206 | rbt.get_num_positions() + 207 | rbt.get_num_velocities()) 208 | 209 | self._DeclareDiscreteState(1) 210 | self._DeclarePeriodicDiscreteUpdate(period_sec=0.001) 211 | 212 | self.kuka_setpoint_output_port = \ 213 | self._DeclareVectorOutputPort( 214 | BasicVector(rbt.get_num_positions() + 215 | rbt.get_num_velocities()), 216 | self._DoCalcKukaSetpointOutput) 217 | self.hand_setpoint_output_port = \ 218 | self._DeclareVectorOutputPort(BasicVector(1), 219 | self._DoCalcHandSetpointOutput) 220 | 221 | self._DeclarePeriodicPublish(0.01, 0.0) 222 | 223 | def _DoCalcDiscreteVariableUpdates(self, context, events, discrete_state): 224 | # Call base method to ensure we do not get recursion. 225 | LeafSystem._DoCalcDiscreteVariableUpdates( 226 | self, context, events, discrete_state) 227 | 228 | new_state = discrete_state. \ 229 | get_mutable_vector().get_mutable_value() 230 | # Close gripper after plan has been executed 231 | if context.get_time() > self.qtraj.end_time(): 232 | new_state[:] = 0. 233 | else: 234 | new_state[:] = 0.1 235 | 236 | def _DoCalcKukaSetpointOutput(self, context, y_data): 237 | 238 | t = context.get_time() 239 | 240 | t_end = self.qtraj.end_time() 241 | if t < t_end: 242 | virtual_time = t 243 | else: 244 | virtual_time = t_end - (t - t_end) 245 | 246 | dt = 0.01 # Look-ahead for estimating target velocity 247 | 248 | target_q = self.qtraj.value(virtual_time) 249 | target_qn = self.qtraj.value(virtual_time+dt) 250 | # This is pretty inefficient and inaccurate -- TODO(gizatt) 251 | # velocity target directly from the trajectory object somehow. 252 | target_v = (target_qn - target_q) / dt 253 | if t >= t_end: 254 | target_v *= -1. 255 | kuka_setpoint = y_data.get_mutable_value() 256 | kuka_setpoint[:self.nq] = target_q[:, 0] 257 | kuka_setpoint[self.nq:] = target_v[:, 0] 258 | 259 | def _DoCalcHandSetpointOutput(self, context, y_data): 260 | state = context.get_discrete_state_vector().get_value() 261 | y = y_data.get_mutable_value() 262 | # Get the ith finger control output 263 | y[:] = state[0] -------------------------------------------------------------------------------- /kuka_ik.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf8 -*- 2 | 3 | import numpy as np 4 | import pydrake 5 | from pydrake.all import ( 6 | PiecewisePolynomial 7 | ) 8 | from pydrake.solvers import ik 9 | 10 | import kuka_utils 11 | 12 | 13 | def plan_grasping_configuration(rbt, q0, target_ee_pose): 14 | ''' Performs IK for a single point in time 15 | to get the Kuka's gripper to a specified 16 | pose in space. ''' 17 | nq = rbt.get_num_positions() 18 | q_des_full = np.zeros(nq) 19 | 20 | controlled_joint_names = [ 21 | "iiwa_joint_1", 22 | "iiwa_joint_2", 23 | "iiwa_joint_3", 24 | "iiwa_joint_4", 25 | "iiwa_joint_5", 26 | "iiwa_joint_6", 27 | "iiwa_joint_7" 28 | ] 29 | free_config_inds, constrained_config_inds = \ 30 | kuka_utils.extract_position_indices(rbt, controlled_joint_names) 31 | 32 | # Assemble IK constraints 33 | constraints = [] 34 | 35 | # Constrain the non-searched-over joints 36 | posture_constraint = ik.PostureConstraint(rbt) 37 | posture_constraint.setJointLimits( 38 | constrained_config_inds, 39 | q0[constrained_config_inds]-0.01, q0[constrained_config_inds]+0.01) 40 | constraints.append(posture_constraint) 41 | 42 | # Constrain the ee frame to lie on the target point 43 | # facing in the target orientation 44 | ee_frame = rbt.findFrame("iiwa_frame_ee").get_frame_index() 45 | constraints.append( 46 | ik.WorldPositionConstraint( 47 | rbt, ee_frame, np.zeros((3, 1)), 48 | target_ee_pose[0:3]-0.01, target_ee_pose[0:3]+0.01) 49 | ) 50 | constraints.append( 51 | ik.WorldEulerConstraint( 52 | rbt, ee_frame, 53 | target_ee_pose[3:6]-0.01, target_ee_pose[3:6]+0.01) 54 | ) 55 | 56 | options = ik.IKoptions(rbt) 57 | results = ik.InverseKin( 58 | rbt, q0, q0, constraints, options) 59 | print results.q_sol, "info %d" % results.info[0] 60 | return results.q_sol[0], results.info[0] 61 | 62 | 63 | def plan_grasping_trajectory(rbt, q0, target_reach_pose, 64 | target_grasp_pose, n_knots, 65 | reach_time, grasp_time): 66 | ''' Solves IK at a series of sample times (connected with a 67 | cubic spline) to generate a trajectory to bring the Kuka from an 68 | initial pose q0 to a final end effector pose in the specified 69 | time, using the specified number of knot points. 70 | 71 | Uses an intermediate pose reach_pose as an intermediate target 72 | to hit at the knot point closest to reach_time. 73 | 74 | See http://drake.mit.edu/doxygen_cxx/rigid__body__ik_8h.html 75 | for the "inverseKinTraj" entry. At the moment, the Python binding 76 | for this function uses "inverseKinTrajSimple" -- i.e., it doesn't 77 | return derivatives. ''' 78 | nq = rbt.get_num_positions() 79 | q_des_full = np.zeros(nq) 80 | 81 | # Create knot points 82 | ts = np.linspace(0., grasp_time, n_knots) 83 | # Figure out the knot just before reach time 84 | reach_start_index = np.argmax(ts >= reach_time) - 1 85 | 86 | controlled_joint_names = [ 87 | "iiwa_joint_1", 88 | "iiwa_joint_2", 89 | "iiwa_joint_3", 90 | "iiwa_joint_4", 91 | "iiwa_joint_5", 92 | "iiwa_joint_6", 93 | "iiwa_joint_7" 94 | ] 95 | free_config_inds, constrained_config_inds = \ 96 | kuka_utils.extract_position_indices(rbt, controlled_joint_names) 97 | 98 | # Assemble IK constraints 99 | constraints = [] 100 | 101 | # Constrain the non-searched-over joints for all time 102 | all_tspan = np.array([0., grasp_time]) 103 | posture_constraint = ik.PostureConstraint(rbt, all_tspan) 104 | posture_constraint.setJointLimits( 105 | constrained_config_inds, 106 | q0[constrained_config_inds]-0.01, q0[constrained_config_inds]+0.01) 107 | constraints.append(posture_constraint) 108 | 109 | # Constrain all joints to be the initial posture at the start time 110 | start_tspan = np.array([0., 0.]) 111 | posture_constraint = ik.PostureConstraint(rbt, start_tspan) 112 | posture_constraint.setJointLimits( 113 | free_config_inds, 114 | q0[free_config_inds]-0.01, q0[free_config_inds]+0.01) 115 | constraints.append(posture_constraint) 116 | 117 | # Constrain the ee frame to lie on the target point 118 | # facing in the target orientation in between the 119 | # reach and final times 120 | ee_frame = rbt.findFrame("iiwa_frame_ee").get_frame_index() 121 | for i in range(reach_start_index, n_knots): 122 | this_tspan = np.array([ts[i], ts[i]]) 123 | interp = float(i - reach_start_index) / (n_knots - reach_start_index) 124 | target_pose = (1.-interp)*target_reach_pose + interp*target_grasp_pose 125 | constraints.append( 126 | ik.WorldPositionConstraint( 127 | rbt, ee_frame, np.zeros((3, 1)), 128 | target_pose[0:3]-0.01, target_pose[0:3]+0.01, 129 | tspan=this_tspan) 130 | ) 131 | constraints.append( 132 | ik.WorldEulerConstraint( 133 | rbt, ee_frame, 134 | target_pose[3:6]-0.05, target_pose[3:6]+0.05, 135 | tspan=this_tspan) 136 | ) 137 | 138 | # Seed and nom are both the initial repeated for the # 139 | # of knot points 140 | q_seed = np.tile(q0, [1, n_knots]) 141 | q_nom = np.tile(q0, [1, n_knots]) 142 | options = ik.IKoptions(rbt) 143 | # Set bounds on initial and final velocities 144 | zero_velocity = np.zeros(rbt.get_num_velocities()) 145 | options.setqd0(zero_velocity, zero_velocity) 146 | options.setqdf(zero_velocity, zero_velocity) 147 | results = ik.InverseKinTraj(rbt, ts, q_seed, q_nom, 148 | constraints, options) 149 | 150 | qtraj = PiecewisePolynomial.Pchip(ts, np.vstack(results.q_sol).T, True) 151 | 152 | print "IK returned a solution with info %d" % results.info[0] 153 | print "(Info 1 is good, other values are dangerous)" 154 | return qtraj, results.info[0] 155 | -------------------------------------------------------------------------------- /kuka_pydrake_sim.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf8 -*- 2 | 3 | import argparse 4 | import time 5 | 6 | import matplotlib.pyplot as plt 7 | import numpy as np 8 | 9 | import pydrake 10 | from pydrake.all import ( 11 | DiagramBuilder, 12 | RgbdCamera, 13 | RigidBodyFrame, 14 | RigidBodyPlant, 15 | RigidBodyTree, 16 | RungeKutta2Integrator, 17 | Shape, 18 | SignalLogger, 19 | Simulator, 20 | ) 21 | 22 | from underactuated.meshcat_rigid_body_visualizer import ( 23 | MeshcatRigidBodyVisualizer) 24 | 25 | import kuka_controllers 26 | import kuka_ik 27 | import kuka_utils 28 | 29 | if __name__ == "__main__": 30 | np.set_printoptions(precision=5, suppress=True) 31 | parser = argparse.ArgumentParser() 32 | parser.add_argument("-T", "--duration", 33 | type=float, 34 | help="Duration to run sim.", 35 | default=4.0) 36 | parser.add_argument("--test", 37 | action="store_true", 38 | help="Help out CI by launching a meshcat server for " 39 | "the duration of the test.") 40 | args = parser.parse_args() 41 | 42 | meshcat_server_p = None 43 | if args.test: 44 | print "Spawning" 45 | import subprocess 46 | meshcat_server_p = subprocess.Popen(["meshcat-server"]) 47 | else: 48 | print "Warning: if you have not yet run meshcat-server in another " \ 49 | "terminal, this will hang." 50 | 51 | # Construct the robot and its environment 52 | rbt = RigidBodyTree() 53 | kuka_utils.setup_kuka(rbt) 54 | 55 | # Set up a visualizer for the robot 56 | pbrv = MeshcatRigidBodyVisualizer(rbt, draw_timestep=0.01) 57 | # (wait while the visualizer warms up and loads in the models) 58 | time.sleep(2.0) 59 | 60 | # Plan a robot motion to maneuver from the initial posture 61 | # to a posture that we know should grab the object. 62 | # (Grasp planning is left as an exercise :)) 63 | q0 = rbt.getZeroConfiguration() 64 | qtraj, info = kuka_ik.plan_grasping_trajectory( 65 | rbt, 66 | q0=q0, 67 | target_reach_pose=np.array([0.6, 0., 1.0, -0.75, 0., -1.57]), 68 | target_grasp_pose=np.array([0.8, 0., 0.9, -0.75, 0., -1.57]), 69 | n_knots=20, 70 | reach_time=1.5, 71 | grasp_time=2.0) 72 | 73 | # Make our RBT into a plant for simulation 74 | rbplant = RigidBodyPlant(rbt) 75 | rbplant.set_name("Rigid Body Plant") 76 | 77 | # Build up our simulation by spawning controllers and loggers 78 | # and connecting them to our plant. 79 | builder = DiagramBuilder() 80 | # The diagram takes ownership of all systems 81 | # placed into it. 82 | rbplant_sys = builder.AddSystem(rbplant) 83 | 84 | # Create a high-level state machine to guide the robot 85 | # motion... 86 | manip_state_machine = builder.AddSystem( 87 | kuka_controllers.ManipStateMachine(rbt, rbplant_sys, qtraj)) 88 | builder.Connect(rbplant_sys.state_output_port(), 89 | manip_state_machine.robot_state_input_port) 90 | 91 | # And spawn the controller that drives the Kuka to its 92 | # desired posture. 93 | kuka_controller = builder.AddSystem( 94 | kuka_controllers.KukaController(rbt, rbplant_sys)) 95 | builder.Connect(rbplant_sys.state_output_port(), 96 | kuka_controller.robot_state_input_port) 97 | builder.Connect(manip_state_machine.kuka_setpoint_output_port, 98 | kuka_controller.setpoint_input_port) 99 | builder.Connect(kuka_controller.get_output_port(0), 100 | rbplant_sys.get_input_port(0)) 101 | 102 | # Same for the hand 103 | hand_controller = builder.AddSystem( 104 | kuka_controllers.HandController(rbt, rbplant_sys)) 105 | builder.Connect(rbplant_sys.state_output_port(), 106 | hand_controller.robot_state_input_port) 107 | builder.Connect(manip_state_machine.hand_setpoint_output_port, 108 | hand_controller.setpoint_input_port) 109 | builder.Connect(hand_controller.get_output_port(0), 110 | rbplant_sys.get_input_port(1)) 111 | 112 | # Hook up the visualizer we created earlier. 113 | visualizer = builder.AddSystem(pbrv) 114 | builder.Connect(rbplant_sys.state_output_port(), 115 | visualizer.get_input_port(0)) 116 | 117 | # Add a camera, too, though no controller or estimator 118 | # will consume the output of it. 119 | # - Add frame for camera fixture. 120 | camera_frame = RigidBodyFrame( 121 | name="rgbd camera frame", body=rbt.world(), 122 | xyz=[2, 0., 1.5], rpy=[-np.pi/4, 0., -np.pi]) 123 | rbt.addFrame(camera_frame) 124 | camera = builder.AddSystem( 125 | RgbdCamera(name="camera", tree=rbt, frame=camera_frame, 126 | z_near=0.5, z_far=2.0, fov_y=np.pi / 4, 127 | width=320, height=240, 128 | show_window=False)) 129 | builder.Connect(rbplant_sys.state_output_port(), 130 | camera.get_input_port(0)) 131 | 132 | camera_meshcat_visualizer = builder.AddSystem( 133 | kuka_utils.RgbdCameraMeshcatVisualizer(camera, rbt)) 134 | builder.Connect(camera.depth_image_output_port(), 135 | camera_meshcat_visualizer.camera_input_port) 136 | builder.Connect(rbplant_sys.state_output_port(), 137 | camera_meshcat_visualizer.state_input_port) 138 | 139 | # Hook up loggers for the robot state, the robot setpoints, 140 | # and the torque inputs. 141 | def log_output(output_port, rate): 142 | logger = builder.AddSystem(SignalLogger(output_port.size())) 143 | logger._DeclarePeriodicPublish(1. / rate, 0.0) 144 | builder.Connect(output_port, logger.get_input_port(0)) 145 | return logger 146 | state_log = log_output(rbplant_sys.get_output_port(0), 60.) 147 | setpoint_log = log_output( 148 | manip_state_machine.kuka_setpoint_output_port, 60.) 149 | kuka_control_log = log_output( 150 | kuka_controller.get_output_port(0), 60.) 151 | 152 | # Done! Compile it all together and visualize it. 153 | diagram = builder.Build() 154 | kuka_utils.render_system_with_graphviz(diagram, "view.gv") 155 | 156 | # Create a simulator for it. 157 | simulator = Simulator(diagram) 158 | simulator.Initialize() 159 | simulator.set_target_realtime_rate(1.0) 160 | # Simulator time steps will be very small, so don't 161 | # force the rest of the system to update every single time. 162 | simulator.set_publish_every_time_step(False) 163 | 164 | # The simulator simulates forward from a given Context, 165 | # so we adjust the simulator's initial Context to set up 166 | # the initial state. 167 | state = simulator.get_mutable_context().\ 168 | get_mutable_continuous_state_vector() 169 | initial_state = np.zeros(state.size()) 170 | initial_state[0:q0.shape[0]] = q0 171 | state.SetFromVector(initial_state) 172 | 173 | # From iiwa_wsg_simulation.cc: 174 | # When using the default RK3 integrator, the simulation stops 175 | # advancing once the gripper grasps the box. Grasping makes the 176 | # problem computationally stiff, which brings the default RK3 177 | # integrator to its knees. 178 | timestep = 0.0002 179 | simulator.reset_integrator( 180 | RungeKutta2Integrator(diagram, timestep, 181 | simulator.get_mutable_context())) 182 | 183 | # This kicks off simulation. Most of the run time will be spent 184 | # in this call. 185 | simulator.StepTo(args.duration) 186 | print("Final state: ", state.CopyToVector()) 187 | 188 | if args.test is not True: 189 | # Do some plotting to show off accessing signal logger data. 190 | nq = rbt.get_num_positions() 191 | plt.figure() 192 | plt.subplot(3, 1, 1) 193 | dims_to_draw = range(7) 194 | color = iter(plt.cm.rainbow(np.linspace(0, 1, 7))) 195 | for i in dims_to_draw: 196 | colorthis = next(color) 197 | plt.plot(state_log.sample_times(), 198 | state_log.data()[i, :], 199 | color=colorthis, 200 | linestyle='solid', 201 | label="q[%d]" % i) 202 | plt.plot(setpoint_log.sample_times(), 203 | setpoint_log.data()[i, :], 204 | color=colorthis, 205 | linestyle='dashed', 206 | label="q_des[%d]" % i) 207 | plt.ylabel("m") 208 | plt.grid(True) 209 | plt.legend(bbox_to_anchor=(1.05, 1), loc=2, borderaxespad=0.) 210 | 211 | plt.subplot(3, 1, 2) 212 | color = iter(plt.cm.rainbow(np.linspace(0, 1, 7))) 213 | for i in dims_to_draw: 214 | colorthis = next(color) 215 | plt.plot(state_log.sample_times(), 216 | state_log.data()[nq + i, :], 217 | color=colorthis, 218 | linestyle='solid', 219 | label="v[%d]" % i) 220 | plt.plot(setpoint_log.sample_times(), 221 | setpoint_log.data()[nq + i, :], 222 | color=colorthis, 223 | linestyle='dashed', 224 | label="v_des[%d]" % i) 225 | plt.ylabel("m/s") 226 | plt.grid(True) 227 | plt.legend(bbox_to_anchor=(1.05, 1), loc=2, borderaxespad=0.) 228 | 229 | plt.subplot(3, 1, 3) 230 | color = iter(plt.cm.rainbow(np.linspace(0, 1, 7))) 231 | for i in dims_to_draw: 232 | colorthis = next(color) 233 | plt.plot(kuka_control_log.sample_times(), 234 | kuka_control_log.data()[i, :], 235 | color=colorthis, 236 | linestyle=':', 237 | label="u[%d]" % i) 238 | plt.xlabel("t") 239 | plt.ylabel("N/m") 240 | plt.grid(True) 241 | plt.legend(bbox_to_anchor=(1.05, 1), loc=2, borderaxespad=0.) 242 | plt.show() 243 | 244 | if meshcat_server_p is not None: 245 | meshcat_server_p.kill() 246 | meshcat_server_p.wait() 247 | -------------------------------------------------------------------------------- /kuka_utils.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf8 -*- 2 | 3 | import os.path 4 | from matplotlib import cm 5 | import numpy as np 6 | 7 | import pydrake 8 | from pydrake.all import ( 9 | AddFlatTerrainToWorld, 10 | AddModelInstancesFromSdfString, 11 | AddModelInstanceFromUrdfFile, 12 | AddModelInstanceFromUrdfStringSearchingInRosPackages, 13 | FloatingBaseType, 14 | LeafSystem, 15 | PortDataType, 16 | RigidBodyFrame, 17 | RollPitchYaw, 18 | RotationMatrix 19 | ) 20 | 21 | import meshcat 22 | import meshcat.transformations as tf 23 | import meshcat.geometry as g 24 | 25 | 26 | def extract_position_indices(rbt, controlled_joint_names): 27 | ''' Given a RigidBodyTree and a list of 28 | joint names, returns, in separate lists, the 29 | position indices (i.e. offsets into the RBT positions vector) 30 | corresponding to those joints, and the rest of the 31 | position indices. ''' 32 | controlled_config_inds = [] 33 | other_config_inds = [] 34 | for i in range(rbt.get_num_bodies()): 35 | body = rbt.get_body(i) 36 | if body.has_joint(): 37 | joint = body.getJoint() 38 | if joint.get_name() in controlled_joint_names: 39 | controlled_config_inds += range( 40 | body.get_position_start_index(), 41 | body.get_position_start_index() + 42 | joint.get_num_positions()) 43 | else: 44 | other_config_inds += range( 45 | body.get_position_start_index(), 46 | body.get_position_start_index() + 47 | joint.get_num_positions()) 48 | if len(controlled_joint_names) != len(controlled_config_inds): 49 | raise ValueError("Didn't find all " 50 | "requested controlled joint names.") 51 | 52 | return controlled_config_inds, other_config_inds 53 | 54 | 55 | def setup_kuka(rbt): 56 | iiwa_urdf_path = os.path.join( 57 | pydrake.getDrakePath(), 58 | "manipulation", "models", "iiwa_description", "urdf", 59 | "iiwa14_polytope_collision.urdf") 60 | 61 | wsg50_sdf_path = os.path.join( 62 | pydrake.getDrakePath(), 63 | "manipulation", "models", "wsg_50_description", "sdf", 64 | "schunk_wsg_50.sdf") 65 | 66 | table_sdf_path = os.path.join( 67 | pydrake.getDrakePath(), 68 | "examples", "kuka_iiwa_arm", "models", "table", 69 | "extra_heavy_duty_table_surface_only_collision.sdf") 70 | 71 | object_urdf_path = os.path.join( 72 | pydrake.getDrakePath(), 73 | "examples", "kuka_iiwa_arm", "models", "objects", 74 | "block_for_pick_and_place.urdf") 75 | 76 | AddFlatTerrainToWorld(rbt) 77 | table_frame_robot = RigidBodyFrame( 78 | "table_frame_robot", rbt.world(), 79 | [0.0, 0, 0], [0, 0, 0]) 80 | AddModelInstancesFromSdfString( 81 | open(table_sdf_path).read(), FloatingBaseType.kFixed, 82 | table_frame_robot, rbt) 83 | table_frame_fwd = RigidBodyFrame( 84 | "table_frame_fwd", rbt.world(), 85 | [0.8, 0, 0], [0, 0, 0]) 86 | AddModelInstancesFromSdfString( 87 | open(table_sdf_path).read(), FloatingBaseType.kFixed, 88 | table_frame_fwd, rbt) 89 | 90 | table_top_z_in_world = 0.736 + 0.057 / 2 91 | 92 | robot_base_frame = RigidBodyFrame( 93 | "robot_base_frame", rbt.world(), 94 | [0.0, 0, table_top_z_in_world], [0, 0, 0]) 95 | AddModelInstanceFromUrdfFile(iiwa_urdf_path, FloatingBaseType.kFixed, 96 | robot_base_frame, rbt) 97 | 98 | object_init_frame = RigidBodyFrame( 99 | "object_init_frame", rbt.world(), 100 | [0.8, 0, table_top_z_in_world+0.1], [0, 0, 0]) 101 | AddModelInstanceFromUrdfFile(object_urdf_path, 102 | FloatingBaseType.kRollPitchYaw, 103 | object_init_frame, rbt) 104 | 105 | # Add gripper 106 | gripper_frame = rbt.findFrame("iiwa_frame_ee") 107 | AddModelInstancesFromSdfString( 108 | open(wsg50_sdf_path).read(), FloatingBaseType.kFixed, 109 | gripper_frame, rbt) 110 | 111 | 112 | def render_system_with_graphviz(system, output_file="system_view.gz"): 113 | ''' Renders the Drake system (presumably a diagram, 114 | otherwise this graph will be fairly trivial) using 115 | graphviz to a specified file. ''' 116 | from graphviz import Source 117 | string = system.GetGraphvizString() 118 | src = Source(string) 119 | src.render(output_file, view=False) 120 | 121 | 122 | class RgbdCameraMeshcatVisualizer(LeafSystem): 123 | def __init__(self, 124 | camera, 125 | rbt, 126 | draw_timestep=0.033333, 127 | prefix="RBCameraViz", 128 | zmq_url="tcp://127.0.0.1:6000"): 129 | LeafSystem.__init__(self) 130 | self.set_name('camera meshcat visualization') 131 | self.timestep = draw_timestep 132 | self._DeclarePeriodicPublish(draw_timestep, 0.0) 133 | self.camera = camera 134 | self.rbt = rbt 135 | self.prefix = prefix 136 | 137 | self.camera_input_port = \ 138 | self._DeclareInputPort(PortDataType.kAbstractValued, 139 | camera.depth_image_output_port().size()) 140 | self.state_input_port = \ 141 | self._DeclareInputPort(PortDataType.kVectorValued, 142 | rbt.get_num_positions() + 143 | rbt.get_num_velocities()) 144 | 145 | # Set up meshcat 146 | self.vis = meshcat.Visualizer(zmq_url=zmq_url) 147 | self.vis[prefix].delete() 148 | 149 | def _DoPublish(self, context, event): 150 | u_data = self.EvalAbstractInput(context, 0).get_value() 151 | x = self.EvalVectorInput(context, 1).get_value() 152 | w, h, _ = u_data.data.shape 153 | depth_image = u_data.data[:, :, 0] 154 | 155 | # Convert depth image to point cloud, with +z being 156 | # camera "forward" 157 | Kinv = np.linalg.inv( 158 | self.camera.depth_camera_info().intrinsic_matrix()) 159 | U, V = np.meshgrid(np.arange(h), np.arange(w)) 160 | points_in_camera_frame = np.vstack([ 161 | U.flatten(), 162 | V.flatten(), 163 | np.ones(w*h)]) 164 | points_in_camera_frame = Kinv.dot(points_in_camera_frame) * \ 165 | depth_image.flatten() 166 | 167 | # The depth camera has some offset from the camera's root frame, 168 | # so take than into account. 169 | pose_mat = self.camera.depth_camera_optical_pose().matrix() 170 | points_in_camera_frame = pose_mat[0:3, 0:3].dot(points_in_camera_frame) 171 | points_in_camera_frame += np.tile(pose_mat[0:3, 3], [w*h, 1]).T 172 | 173 | kinsol = self.rbt.doKinematics(x[:self.rbt.get_num_positions()]) 174 | points_in_world_frame = self.rbt.transformPoints( 175 | kinsol, 176 | points_in_camera_frame, 177 | self.camera.frame().get_frame_index(), 178 | 0) 179 | 180 | # Color points according to their normalized height 181 | min_height = 0.0 182 | max_height = 2.0 183 | colors = cm.jet( 184 | (points_in_world_frame[2, :]-min_height)/(max_height-min_height) 185 | ).T[0:3, :] 186 | 187 | self.vis[self.prefix]["points"].set_object( 188 | g.PointCloud(position=points_in_world_frame, 189 | color=colors, 190 | size=0.005)) 191 | -------------------------------------------------------------------------------- /run_tests.sh: -------------------------------------------------------------------------------- 1 | #! /bin/bash 2 | cd /test_dir 3 | 4 | # Launch a fake X-server in the background 5 | Xvfb :100 -ac -screen 0 800x600x24 & 6 | 7 | # Give that a sec to take effect 8 | sleep 1 9 | 10 | # Launch a complete robot context and execute some canned movement. 11 | DISPLAY=:100 python kuka_pydrake_sim.py --test 12 | exit_status=$? 13 | if [ ! $exit_status -eq 0 ]; then 14 | echo "Error code in kuka_pydrake_sim.py: " $exit_status 15 | exit $exit_status 16 | fi --------------------------------------------------------------------------------