From ffded1c96d8364bebe9bdbf18615dc62e2a534e3 Mon Sep 17 00:00:00 2001 From: Russ Tedrake Date: Thu, 25 Apr 2024 08:27:17 -0400 Subject: [PATCH] move remaining chapter notebooks into subdirectories fix broken link to zmp a little progress on stochastic control notes --- book/BUILD.bazel | 18 - book/humanoids.html | 2 +- book/humanoids/BUILD.bazel | 2 +- book/index.html | 5 +- book/limit_cycles/BUILD.bazel | 14 + book/{ => limit_cycles}/limit_cycles.ipynb | 0 book/multibody/BUILD.bazel | 16 + book/{ => multibody}/multibody.ipynb | 0 book/robust.html | 65 +- book/simple_legs.ipynb | 727 --------------------- book/stochastic.html | 17 +- book/stochastic/BUILD.bazel | 14 + book/{ => stochastic}/stochastic.ipynb | 0 pyproject.toml | 2 +- 14 files changed, 116 insertions(+), 766 deletions(-) create mode 100644 book/limit_cycles/BUILD.bazel rename book/{ => limit_cycles}/limit_cycles.ipynb (100%) create mode 100644 book/multibody/BUILD.bazel rename book/{ => multibody}/multibody.ipynb (100%) delete mode 100644 book/simple_legs.ipynb create mode 100644 book/stochastic/BUILD.bazel rename book/{ => stochastic}/stochastic.ipynb (100%) diff --git a/book/BUILD.bazel b/book/BUILD.bazel index 14ba4876..6b0eebe7 100644 --- a/book/BUILD.bazel +++ b/book/BUILD.bazel @@ -53,11 +53,6 @@ rt_html_test( rt_html_test( srcs = ["stochastic.html"], ) -rt_ipynb_test( - name = "stochastic", - srcs = ["stochastic.ipynb"], - deps = ["//underactuated"], -) rt_html_test( srcs = ["dp.html"], @@ -110,11 +105,6 @@ rt_html_test( rt_html_test( srcs = ["limit_cycles.html"], ) -rt_ipynb_test( - name = "limit_cycles", - srcs = ["limit_cycles.ipynb"], - deps = ["//underactuated"], -) rt_html_test( srcs = ["contact.html"], @@ -140,14 +130,6 @@ rt_html_test( srcs = ["multibody.html"], ) -rt_ipynb_test( - name = "multibody", - srcs = ["multibody.ipynb"], - deps = [ - requirement("drake"), - ], -) - rt_html_test( srcs = ["optimization.html"], ) diff --git a/book/humanoids.html b/book/humanoids.html index f6138b4c..a67857ff 100644 --- a/book/humanoids.html +++ b/book/humanoids.html @@ -396,7 +396,7 @@

Underactuated RoboticsThe implementation that we used in the DARPA Robotics Challenge is described in Tedrake15 and is available in Drake as ZmpPlanner.

+ href="https://drake.mit.edu/doxygen_cxx/classdrake_1_1planning_1_1_zmp_planner.html">ZmpPlanner.

The ZMP Planner

diff --git a/book/humanoids/BUILD.bazel b/book/humanoids/BUILD.bazel index 4319413e..9b43bb9a 100644 --- a/book/humanoids/BUILD.bazel +++ b/book/humanoids/BUILD.bazel @@ -23,7 +23,7 @@ rt_ipynb_test( # deps = ["//underactuated"], #) -# TODO(russt): Waiting for drake PR #21305 to merge. +# TODO(russt): Waiting for next drake version bump. #rt_ipynb_test( # name = "zmp_planner", # srcs = ["zmp_planner.ipynb"], diff --git a/book/index.html b/book/index.html index 02b02f80..71fcd5c5 100644 --- a/book/index.html +++ b/book/index.html @@ -251,7 +251,7 @@

Table of Contents

  • Chapter 6: Model Systems with Stochasticity
      • -
    • The Master Equation
      • +
    • The Master Equation
    • Stationary Distributions
    • Extended Example: The Rimless Wheel on Rough Terrain
      • @@ -504,6 +504,9 @@

      Table of Contents

    • Stochastic models
    • Costs and constraints for stochastic systems
    • Finite Markov Decision Processes
      • +
        +
      • Dynamics of a Markov chain
        • +
    • Linear optimal control
      • Stochastic LQR
        • diff --git a/book/limit_cycles/BUILD.bazel b/book/limit_cycles/BUILD.bazel new file mode 100644 index 00000000..449d226f --- /dev/null +++ b/book/limit_cycles/BUILD.bazel @@ -0,0 +1,14 @@ +# -*- mode: python -*- +# vi: set ft=python : + +# Copyright 2020-2022 Massachusetts Institute of Technology. +# Licensed under the BSD 3-Clause License. See LICENSE.TXT for details. + +load("@pip_deps//:requirements.bzl", "requirement") +load("//book/htmlbook/tools/jupyter:defs.bzl", "rt_ipynb_test") + +rt_ipynb_test( + name = "limit_cycles", + srcs = ["limit_cycles.ipynb"], + deps = ["//underactuated"], +) diff --git a/book/limit_cycles.ipynb b/book/limit_cycles/limit_cycles.ipynb similarity index 100% rename from book/limit_cycles.ipynb rename to book/limit_cycles/limit_cycles.ipynb diff --git a/book/multibody/BUILD.bazel b/book/multibody/BUILD.bazel new file mode 100644 index 00000000..fbd1a0d8 --- /dev/null +++ b/book/multibody/BUILD.bazel @@ -0,0 +1,16 @@ +# -*- mode: python -*- +# vi: set ft=python : + +# Copyright 2020-2022 Massachusetts Institute of Technology. +# Licensed under the BSD 3-Clause License. See LICENSE.TXT for details. + +load("@pip_deps//:requirements.bzl", "requirement") +load("//book/htmlbook/tools/jupyter:defs.bzl", "rt_ipynb_test") + +rt_ipynb_test( + name = "multibody", + srcs = ["multibody.ipynb"], + deps = [ + requirement("drake"), + ], +) diff --git a/book/multibody.ipynb b/book/multibody/multibody.ipynb similarity index 100% rename from book/multibody.ipynb rename to book/multibody/multibody.ipynb diff --git a/book/robust.html b/book/robust.html index 60083866..338df135 100644 --- a/book/robust.html +++ b/book/robust.html @@ -213,19 +213,68 @@

        Underactuated Robotics

        Finite Markov Decision Processes

        -

        The Bellman equation.

        - -

        Discounted cost. Infinite-horizon average cost.

        + Finalize if I want to use p_n(s) or \Pr_n(s) here, and use it throughout the + notes. + +

        We already had quick preview into stochastic optimal control in one of the cases + where it is particularly easy: finite Markov Decision + Processes (MDPs).

        + +

        Recall that in this setting, where the state space and action spaces are finite, + we write the dynamics completely in terms of the transition probabilities + \[p(s[n+1] = s' | s[n] = s, a[n] = a). \] Conveniently, we can represent the + entire state probability distribution as a vector, $p_n(s[n] = s),$ and write the + dynamics as a transition matrix for each action: $$T_{i,j}(a) = p(s_j | s_i, a).$$ + The stochastic dynamics of the master equation + are then given by $$p_{n+1}(s') = {\bf T}(a[n]) p_{n}(s).$$ Take a moment to + appreciate this -- if we make a discrete-state / discrete-action approximation of + even the most complicated and rich nonlinear dynamical system, then the dynamics of + the state probability distribution are defined by an (action-dependent) linear map. + Note that ${\bf T}$ is not an arbitrary matrix of real numbers but rather a (left) + "stochastic matrix": + we always have the $T_{ij} \in [0, 1]$ and $\sum_{i}T_{ij} = 1.$

        + +

        Dynamics of a Markov chain

        + + +

        The dynamics of a closed-loop system (e.g. with no action inputs, or a fixed + policy, $\pi$), then the MDP equations reduce back to the simpler form of a Markov + chain: $$p_{n+1}(s') = {\bf T} p_{n}(s).$$ To evaluate the long-term dynamics of a + Markov chain, we have $$p_{n}(s') = {\bf T}^n p_{0}(s).$$ The eigenvalues of a + stochastic matrix are also bounded: $\forall i, \lambda_i \in [0, 1].$ Moreover, + it can be shown that every stochastic matrix has at least one eigenvalue of $1$; + any eigenvector corresponding to an eigenvalue of 1 is a stationary + distribution (a fixed point of the master equation) of the Markov chain. If + the Markov chain is "irreducible" and "aperiodic", then the stationary + distribution is unique and the Markov chain converges to it from any initial + condition.

        + +

        Discretized cubic polynomial w/ noise

        + +

        I used the discrete-time approximation of cubic polynomial with Gaussian noise as an + example when we were first building our intuition about stochastic dynamics. + Let's now make a finite-state Markov chain approximation of those dynamics, by + discretizing the state space in 100 bins over the domain $x \ in [-2, 2].$

        + +

        Let's examine the eigenvalues of this stochastic transition matrix...

        + + Finish coding up the example... + +         + example: discretized cubic polynomial (bistable version) + additive Gaussian noise + Metastability -

        We already had quick preview into stochastic optimal control in one of - the cases where it is particularly easy: finite - Markov Decision Processes (MDPs).

        + example: discretized cubic polynomial negated. Metastability. Rimless wheel on rough terrain. - Perhaps a example of something other than expected-value cost - (worst case, e.g. Katie's metastability?)         + Discounted cost. Infinite-horizon average cost. + + Perhaps a example of something other than expected-value cost + (worst case, e.g. Katie's metastability?) +

        Linear optimal control

        diff --git a/book/simple_legs.ipynb b/book/simple_legs.ipynb deleted file mode 100644 index d26f4e02..00000000 --- a/book/simple_legs.ipynb +++ /dev/null @@ -1,727 +0,0 @@ -{ - "cells": [ - { - "cell_type": "markdown", - "metadata": {}, - "source": [ - "This notebook provides examples to go along with the [textbook](https://underactuated.csail.mit.edu/simple_legs.html). I recommend having both windows open, side-by-side!\n" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [ - "import copy\n", - "\n", - "import matplotlib.pyplot as plt\n", - "import mpld3\n", - "import numpy as np\n", - "from IPython.display import display\n", - "from pydrake.all import (\n", - " BasicVector,\n", - " ConstantVectorSource,\n", - " DiagramBuilder,\n", - " LeafSystem,\n", - " LogVectorOutput,\n", - " MeshcatVisualizer,\n", - " PortDataType,\n", - " PublishEvent,\n", - " PyPlotVisualizer,\n", - " SceneGraph,\n", - " Simulator,\n", - " StartMeshcat,\n", - " UnrestrictedUpdateEvent,\n", - " WitnessFunctionDirection,\n", - " namedview,\n", - ")\n", - "from pydrake.examples import (\n", - " CompassGait,\n", - " CompassGaitGeometry,\n", - " RimlessWheel,\n", - " RimlessWheelGeometry,\n", - " RimlessWheelParams,\n", - ")\n", - "\n", - "from underactuated import running_as_notebook\n", - "from underactuated.jupyter import AdvanceToAndVisualize\n", - "\n", - "if running_as_notebook:\n", - " mpld3.enable_notebook()" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [ - "# Start the visualizer (run this cell only once, each instance consumes a port)\n", - "meshcat = StartMeshcat()" - ] - }, - { - "cell_type": "markdown", - "metadata": {}, - "source": [ - "# The Rimless Wheel" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [ - "def rimless_wheel(slope=0.08, initial_angle=0, initial_angular_velocity=5.0):\n", - " params = RimlessWheelParams()\n", - " params.set_slope(slope)\n", - "\n", - " builder = DiagramBuilder()\n", - " rimless_wheel = builder.AddSystem(RimlessWheel())\n", - " scene_graph = builder.AddSystem(SceneGraph())\n", - " RimlessWheelGeometry.AddToBuilder(\n", - " builder,\n", - " rimless_wheel.get_floating_base_state_output_port(),\n", - " params,\n", - " scene_graph,\n", - " )\n", - " visualizer = MeshcatVisualizer.AddToBuilder(builder, scene_graph, meshcat)\n", - " meshcat.Set2dRenderMode(xmin=-2, xmax=14, ymin=-2, ymax=3)\n", - "\n", - " diagram = builder.Build()\n", - " simulator = Simulator(diagram)\n", - "\n", - " context = simulator.get_mutable_context()\n", - "\n", - " diagram.GetMutableSubsystemContext(rimless_wheel, context).get_numeric_parameter(\n", - " 0\n", - " ).set_slope(slope)\n", - " context.SetAccuracy(1e-4)\n", - " context.SetContinuousState([initial_angle, initial_angular_velocity])\n", - " simulator.Initialize()\n", - " simulator.set_target_realtime_rate(1.0 if running_as_notebook else 0.0)\n", - " visualizer.StartRecording()\n", - " simulator.AdvanceTo(5.0 if running_as_notebook else 0.1)\n", - " visualizer.PublishRecording()\n", - "\n", - "\n", - "rimless_wheel()" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [ - "rimless_wheel(initial_angular_velocity=5.0)" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [ - "rimless_wheel(initial_angular_velocity=10.0)" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [ - "rimless_wheel(initial_angular_velocity=0.95)" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [ - "rimless_wheel(initial_angular_velocity=-5.0)" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [ - "rimless_wheel(initial_angular_velocity=-4.8)" - ] - }, - { - "cell_type": "markdown", - "metadata": {}, - "source": [ - "Here is a little interactive plot to allow you to visualize the trajectories of the rimless wheel as you vary the initial velocity." - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [ - "# TODO(russt): Port to meshcat and make it interactive, once again.\n", - "\n", - "\n", - "def rimless_wheel_return_map(initial_angular_velocity=1.5, duration=1.5):\n", - " rimless_wheel = RimlessWheel()\n", - " simulator = Simulator(rimless_wheel)\n", - " context = simulator.get_mutable_context()\n", - " params = context.get_numeric_parameter(0)\n", - " qmin = params.slope() - rimless_wheel.calc_alpha(params) - 0.1\n", - " qmax = params.slope() + rimless_wheel.calc_alpha(params) + 0.1\n", - " qdmin = -2\n", - " qdmax = 2\n", - "\n", - " context.SetAccuracy(1e-2)\n", - " integrator = simulator.get_mutable_integrator()\n", - "\n", - " fig, ax = plt.subplots(figsize=(10, 6))\n", - "\n", - " # TODO(russt): make the slope interactive, too.\n", - " def simulate(initial_angular_velocity=1.5, duration=1.5):\n", - " rimless_wheel.SetDefaultContext(context)\n", - " context.SetTime(0.0)\n", - " if initial_angular_velocity >= 0:\n", - " initial_angle = params.slope() - rimless_wheel.calc_alpha(params)\n", - " else:\n", - " initial_angle = params.slope() + rimless_wheel.calc_alpha(params)\n", - " if initial_angular_velocity == 0:\n", - " # Set double_support = True.\n", - " context.get_mutable_abstract_state(0).set_value(True)\n", - "\n", - " context.SetContinuousState([initial_angle, initial_angular_velocity])\n", - "\n", - " integrator.StartDenseIntegration()\n", - " simulator.Initialize()\n", - " simulator.AdvanceTo(duration if running_as_notebook else 0.1)\n", - " pp = integrator.StopDenseIntegration()\n", - "\n", - " return pp.vector_values(pp.get_segment_times())\n", - "\n", - " if False:\n", - " data = simulate()\n", - " (line,) = ax.plot(data[0, :], data[1, :], \"b\")\n", - " (pt,) = ax.plot(data[0, 0], data[1, 0], \"b*\", markersize=12)\n", - "\n", - " def update(initial_angular_velocity):\n", - " data = simulate(initial_angular_velocity)\n", - " line.set_xdata(data[0, :])\n", - " line.set_ydata(data[1, :])\n", - " pt.set_xdata(data[0, 0])\n", - " pt.set_ydata(data[1, 0])\n", - " fig.canvas.draw()\n", - "\n", - " interact(\n", - " update,\n", - " initial_angular_velocity=widgets.FloatSlider(\n", - " min=qdmin, max=qdmax, step=0.1, value=1.1\n", - " ),\n", - " )\n", - "\n", - " else:\n", - " data = simulate(initial_angular_velocity, duration)\n", - " ax.plot(data[0, :], data[1, :], \"b\")\n", - " ax.plot(data[0, 0], data[1, 0], \"b*\", markersize=12)\n", - "\n", - " # Plot the energy contours.\n", - " nq = 151\n", - " nqd = 151\n", - " mgl = params.mass() * params.gravity() * params.length()\n", - " q = np.linspace(qmin, qmax, nq)\n", - " qd = np.linspace(qdmin, qdmax, nqd)\n", - " Q, QD = np.meshgrid(q, qd)\n", - " Energy = 0.5 * params.mass() * params.length() ** 2 * QD**2 + mgl * np.cos(Q)\n", - " ax.contour(\n", - " Q,\n", - " QD,\n", - " Energy,\n", - " alpha=0.5,\n", - " linestyles=\"dashed\",\n", - " colors=\"black\",\n", - " linewidths=0.5,\n", - " )\n", - "\n", - " ax.plot(\n", - " params.slope() - rimless_wheel.calc_alpha(params) * np.array([1, 1]),\n", - " np.array([0, qdmax]),\n", - " \"k--\",\n", - " )\n", - " ax.plot(\n", - " params.slope() - rimless_wheel.calc_alpha(params) * np.array([1, 1]),\n", - " np.array([0, qdmin]),\n", - " \"k\",\n", - " linewidth=0.25,\n", - " )\n", - " ax.plot(\n", - " params.slope() + rimless_wheel.calc_alpha(params) * np.array([1, 1]),\n", - " np.array([0, qdmin]),\n", - " \"k--\",\n", - " )\n", - " ax.plot(\n", - " params.slope() + rimless_wheel.calc_alpha(params) * np.array([1, 1]),\n", - " np.array([0, qdmax]),\n", - " \"k\",\n", - " linewidth=0.25,\n", - " )\n", - " ax.plot([qmin, qmax], [0, 0], \"k\", linewidth=0.25)\n", - " ax.plot([0, 0], [qdmin, qdmax], \"k\", linewidth=0.25)\n", - " ax.set_xlabel(\"theta\")\n", - " ax.set_ylabel(\"thetadot\")\n", - " ax.axis([qmin, qmax, qdmin, qdmax])\n", - " ax.set_title(\n", - " \"Trajectories of the Rimless Wheel (w/ contours of \" \"constant energy)\"\n", - " )\n", - " display(mpld3.display())\n", - "\n", - "\n", - "# Interesting angular velocities to try: 5, 10, 0.95, -5, -4.8\n", - "rimless_wheel_return_map(initial_angular_velocity=-4.7, duration=5)" - ] - }, - { - "cell_type": "markdown", - "metadata": {}, - "source": [ - "# The Compass Gait" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [ - "def compass_gait():\n", - " builder = DiagramBuilder()\n", - " compass_gait = builder.AddSystem(CompassGait())\n", - "\n", - " hip_torque = builder.AddSystem(ConstantVectorSource([0.0]))\n", - " builder.Connect(hip_torque.get_output_port(0), compass_gait.get_input_port(0))\n", - "\n", - " scene_graph = builder.AddSystem(SceneGraph())\n", - " CompassGaitGeometry.AddToBuilder(\n", - " builder,\n", - " compass_gait.get_floating_base_state_output_port(),\n", - " scene_graph,\n", - " )\n", - " visualizer = MeshcatVisualizer.AddToBuilder(builder, scene_graph, meshcat)\n", - " meshcat.Set2dRenderMode(xmin=-1, xmax=5, ymin=-1, ymax=2)\n", - "\n", - " logger = LogVectorOutput(compass_gait.get_output_port(1), builder)\n", - "\n", - " diagram = builder.Build()\n", - " simulator = Simulator(diagram)\n", - " simulator.set_target_realtime_rate(1.0 if running_as_notebook else 0.0)\n", - "\n", - " context = simulator.get_mutable_context()\n", - " context.SetAccuracy(1e-4)\n", - " context.SetContinuousState([0.0, 0.0, 0.4, -2.0])\n", - "\n", - " visualizer.StartRecording()\n", - " simulator.AdvanceTo(8.0)\n", - " visualizer.PublishRecording()\n", - " log = logger.FindLog(context)\n", - " plt.figure()\n", - " plt.plot(log.data()[4, :], log.data()[11, :])\n", - " plt.xlabel(\"left leg angle\")\n", - " plt.ylabel(\"left leg angular velocity\")\n", - " display(mpld3.display())\n", - "\n", - "\n", - "compass_gait()" - ] - }, - { - "cell_type": "markdown", - "metadata": {}, - "source": [ - "# The Spring-Loaded Inverted Pendulum (SLIP)" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [ - "SLIPState = namedview(\n", - " \"SLIPState\", [\"x\", \"z\", \"r\", \"theta\", \"xdot\", \"zdot\", \"rdot\", \"thetadot\"]\n", - ")\n", - "\n", - "\n", - "class SpringLoadedInvertedPendulum(LeafSystem):\n", - " def __init__(self):\n", - " LeafSystem.__init__(self)\n", - "\n", - " self.DeclareVectorInputPort(\"touchdown_angle\", 1)\n", - " state_index = self.DeclareContinuousState(BasicVector(np.zeros(8)), 4, 4, 0)\n", - " self.DeclareStateOutputPort(\"state\", state_index)\n", - "\n", - " # Parameters from Geyer05, p.23\n", - " self.mass = 80.0 # kg\n", - " self.r0 = 1.0 # m\n", - " self.gravity = 9.81 # m/s^2\n", - " # Define spring constant in terms of the dimensionless number.\n", - " # Definition in section 2.4.3, values in figure 2.4.\n", - " # Note: Geyer05 says 10.8 (which doesn't work? -- I get no fixed pts).\n", - " dimensionless_spring_constant = 10.7\n", - " self.stiffness = (\n", - " dimensionless_spring_constant * self.mass * self.gravity / self.r0\n", - " )\n", - "\n", - " self.last_apex = None # placeholder for writing return map result.\n", - "\n", - " self.touchdown_witness = self.MakeWitnessFunction(\n", - " \"touchdown\",\n", - " WitnessFunctionDirection.kPositiveThenNonPositive,\n", - " self.foot_height,\n", - " UnrestrictedUpdateEvent(self.touchdown),\n", - " )\n", - " self.takeoff_witness = self.MakeWitnessFunction(\n", - " \"takeoff\",\n", - " WitnessFunctionDirection.kPositiveThenNonPositive,\n", - " self.leg_compression,\n", - " UnrestrictedUpdateEvent(self.takeoff),\n", - " )\n", - " self.apex_witness = self.MakeWitnessFunction(\n", - " \"apex\",\n", - " WitnessFunctionDirection.kPositiveThenNonPositive,\n", - " self.apex,\n", - " PublishEvent(self.publish_apex),\n", - " )\n", - "\n", - " def foot_height(self, context):\n", - " s = SLIPState(context.get_continuous_state_vector().CopyToVector())\n", - " return s.z - self.r0 * np.cos(s.theta)\n", - "\n", - " def touchdown(self, context, event, state):\n", - " s = SLIPState(context.get_mutable_continuous_state_vector().CopyToVector())\n", - "\n", - " # print(\"touchdown\")\n", - "\n", - " # Update rdot and thetadot to match xdot and ydot, using\n", - " # x = -r*sin(theta), z = r*cos(theta)\n", - " # => xdot = -rdot*s - r*c*thetadot, zdot = rdot*c - r*s*thetadot\n", - " # => xdot*c + zdot*s = -r*thetadot\n", - " # r^2 = x^2 + z^2\n", - " # => 2r*rdot = 2x*xdot + 2z*zdot\n", - " # => rdot = -xdot*sin(theta) + zdot*cos(theta)\n", - " # (matches Geyer05 Eq. 2.24 up to the symbol changes)\n", - " s.r = self.r0\n", - " s.rdot = -s.xdot * np.sin(s.theta) + s.zdot * np.cos(s.theta)\n", - " s.thetadot = -(s.xdot * np.cos(s.theta) + s.zdot * np.sin(s.theta)) / self.r0\n", - " state.get_mutable_continuous_state().get_mutable_vector().SetFromVector(s[:])\n", - "\n", - " def leg_compression(self, context):\n", - " s = SLIPState(context.get_continuous_state_vector().CopyToVector())\n", - " return self.r0 - s.r\n", - "\n", - " def takeoff(self, context, event, state):\n", - " s = SLIPState(context.get_mutable_continuous_state_vector().CopyToVector())\n", - "\n", - " # print(\"takeoff\")\n", - "\n", - " # Setup flight state (these lines aren't strictly required, since we\n", - " # choose to also integrate x and z in stance below).\n", - " s.z = self.r0 * np.cos(s.theta)\n", - " s.xdot = -s.rdot * np.sin(s.theta) - self.r0 * s.thetadot * np.cos(s.theta)\n", - " s.zdot = s.rdot * np.cos(s.theta) - self.r0 * s.thetadot * np.sin(s.theta)\n", - "\n", - " # Update theta to commanded leg angle.\n", - " s.theta = self.EvalVectorInput(context, 0).GetAtIndex(0)\n", - " s.thetadot = 0\n", - " s.r = self.r0\n", - " s.rdot = 0\n", - "\n", - " state.get_mutable_continuous_state().get_mutable_vector().SetFromVector(s[:])\n", - "\n", - " def apex(self, context):\n", - " return context.get_continuous_state_vector().GetAtIndex(5) # zdot\n", - "\n", - " def publish_apex(self, context, event):\n", - " # TODO(russt): provide an option to terminate here instead, pending\n", - " # resolution of #4447.\n", - " # print(\"apex\")\n", - " if self.last_apex is None:\n", - " s = SLIPState(context.get_mutable_continuous_state_vector().CopyToVector())\n", - " self.last_apex = s.z\n", - "\n", - " def apex_velocity_from_dimensionless_system_energy(self, Etilde, z):\n", - " E = Etilde * self.mass * self.gravity * self.r0\n", - " # E = 0.5*m*v^2 + m*g*z\n", - " xdot = np.sqrt(2.0 / self.mass * (E - self.mass * self.gravity * z))\n", - " return xdot\n", - "\n", - " def energy_flight(self, context):\n", - " s = SLIPState(context.get_mutable_continuous_state_vector().CopyToVector())\n", - " return (\n", - " 0.5 * self.mass * (s.xdot**2 + s.zdot**2) + self.mass * self.gravity * s.z\n", - " )\n", - "\n", - " def energy_stance(self, context):\n", - " s = SLIPState(context.get_mutable_continuous_state_vector().CopyToVector())\n", - " return (\n", - " 0.5 * self.mass * (s.rdot**2 + s.r**2 * s.thetadot**2)\n", - " + self.mass * self.gravity * s.r * np.cos(s.theta)\n", - " + 0.5 * self.stiffness * (self.r0 - s.r) ** 2\n", - " )\n", - "\n", - " def DoGetWitnessFunctions(self, context):\n", - " return [\n", - " self.touchdown_witness,\n", - " self.takeoff_witness,\n", - " self.apex_witness,\n", - " ]\n", - "\n", - " def DoCalcTimeDerivatives(self, context, derivatives):\n", - " s = SLIPState(context.get_continuous_state_vector().CopyToVector())\n", - " sdot = SLIPState(np.zeros(8))\n", - " sdot[0:4] = s[4:8]\n", - "\n", - " if self.foot_height(context) < 0:\n", - " # then we're in \"stance\"\n", - " sdot.rdot = (\n", - " self.stiffness / self.mass * (self.r0 - s.r)\n", - " + s.r * s.thetadot**2\n", - " - self.gravity * np.cos(s.theta)\n", - " )\n", - " sdot.thetadot = (\n", - " self.gravity / s.r * np.sin(s.theta) - 2 * s.rdot * s.thetadot / s.r\n", - " )\n", - "\n", - " # Integrate x and z also, just for the sake of visualization (all\n", - " # the integrated values except x will be overwritten in the\n", - " # take-off reset).\n", - " # x = -r*sin(theta), y = r*cos(theta) =>\n", - " sdot.xdot = (\n", - " -sdot.rdot * np.sin(s.theta)\n", - " - 2 * s.rdot * s.thetadot * np.cos(s.theta)\n", - " + s.r * s.thetadot**2 * np.sin(s.theta)\n", - " - s.r * sdot.thetadot * np.cos(s.theta)\n", - " )\n", - " sdot.zdot = (\n", - " sdot.rdot * np.cos(s.theta)\n", - " - 2 * s.rdot * s.thetadot * np.sin(s.theta)\n", - " - s.r * sdot.thetadot * np.sin(s.theta)\n", - " - s.r * s.thetadot**2 * np.cos(s.theta)\n", - " )\n", - "\n", - " else:\n", - " sdot.xdot = 0\n", - " sdot.zdot = -self.gravity\n", - " sdot.rdot = 0\n", - " sdot.thetadot = 0\n", - "\n", - " derivatives.get_mutable_vector().SetFromVector(sdot[:])\n", - "\n", - "\n", - "class SLIPVisualizer(PyPlotVisualizer):\n", - " def __init__(self, ax=None, show=True):\n", - " PyPlotVisualizer.__init__(self, ax=ax, show=show, figsize=(12, 4))\n", - " self.DeclareInputPort(\"slip_state\", PortDataType.kVectorValued, 8)\n", - " self.ax.set_aspect(\"equal\")\n", - " self.ax.set_xlim(0, 2)\n", - " self.ax.set_ylim(-0.1, 1.5)\n", - "\n", - " # Draw the ground.\n", - " self.ax.plot([-50, 50], [0, 0], \"k\")\n", - "\n", - " a = np.linspace(0, 2 * np.pi, 50)\n", - " radius = 0.1\n", - " self.hip_fill = self.ax.fill(\n", - " radius * np.sin(a),\n", - " radius * np.cos(a),\n", - " zorder=1,\n", - " edgecolor=\"k\",\n", - " facecolor=[0.6, 0.6, 0.6],\n", - " )\n", - " self.hip = copy.copy(self.hip_fill[0].get_path().vertices)\n", - "\n", - " self.leg_line = [self.ax.plot([0, 0], [0, -1], \"k\")[0]]\n", - " self.leg_data = [self.leg_line[0].get_xydata().T]\n", - " for i in range(1, 13):\n", - " self.leg_line.append(\n", - " self.ax.plot(\n", - " 0.1\n", - " * np.array(\n", - " [\n", - " np.sin((i - 1) * np.pi / 2.0),\n", - " np.sin(i * np.pi / 2.0),\n", - " ]\n", - " ),\n", - " -0.2 - 0.7 / 13 * np.array([i - 1, i]),\n", - " \"k\",\n", - " )[0]\n", - " )\n", - " self.leg_data.append(self.leg_line[i].get_xydata().T)\n", - "\n", - " def draw(self, context):\n", - " state = SLIPState(self.EvalVectorInput(context, 0).CopyToVector())\n", - "\n", - " self.hip_fill[0].get_path().vertices[:, 0] = state.x + self.hip[:, 0]\n", - " self.hip_fill[0].get_path().vertices[:, 1] = state.z + self.hip[:, 1]\n", - "\n", - " R = np.array(\n", - " [\n", - " [np.cos(state.theta), -np.sin(state.theta)],\n", - " [np.sin(state.theta), np.cos(state.theta)],\n", - " ]\n", - " )\n", - " for i in range(len(self.leg_line)):\n", - " self.leg_line[i].set_xdata(\n", - " state.x + state.r * R[0, :].dot(self.leg_data[i])\n", - " )\n", - " self.leg_line[i].set_ydata(\n", - " state.z + state.r * R[1, :].dot(self.leg_data[i])\n", - " )\n", - "\n", - " self.ax.set_title(\"t = {:.1f}\".format(context.get_time()))" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [ - "def slip_demo():\n", - " builder = DiagramBuilder()\n", - " plant = builder.AddSystem(SpringLoadedInvertedPendulum())\n", - "\n", - " # Parameters from Geyer05, Figure 2.4\n", - " # Note: Geyer uses angle of attack = 90 - touchdown_angle\n", - " touchdown_angle = np.deg2rad(30)\n", - " Etilde = 1.61\n", - "\n", - " s = SLIPState(np.zeros(8))\n", - " s.z = 0.9\n", - " s.theta = touchdown_angle\n", - " s.r = 1\n", - " s.xdot = plant.apex_velocity_from_dimensionless_system_energy(Etilde, s.z)\n", - "\n", - " visualizer = builder.AddSystem(SLIPVisualizer(show=False))\n", - " builder.Connect(plant.get_output_port(0), visualizer.get_input_port(0))\n", - "\n", - " LogVectorOutput(plant.get_output_port(0), builder)\n", - "\n", - " command = builder.AddSystem(ConstantVectorSource([touchdown_angle]))\n", - " builder.Connect(command.get_output_port(0), plant.get_input_port(0))\n", - "\n", - " diagram = builder.Build()\n", - " simulator = Simulator(diagram)\n", - " context = simulator.get_mutable_context()\n", - " context.SetAccuracy(1e-10)\n", - "\n", - " context.get_mutable_continuous_state_vector().SetFromVector(s[:])\n", - "\n", - " AdvanceToAndVisualize(simulator, visualizer, 2.0)\n", - "\n", - "\n", - "slip_demo()" - ] - }, - { - "cell_type": "markdown", - "metadata": {}, - "source": [ - "## Apex-to-apex map" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [ - "def slip_map():\n", - " plant = SpringLoadedInvertedPendulum()\n", - "\n", - " # Parameters from Geyer05, Figure 2.4\n", - " # Note: Geyer uses angle of attack = 90 - touchdown_angle\n", - " touchdown_angle = np.deg2rad(30)\n", - " Etilde = 1.61\n", - "\n", - " s = SLIPState(np.zeros(8))\n", - " s.theta = touchdown_angle\n", - " s.r = 1\n", - "\n", - " simulator = Simulator(plant)\n", - " context = simulator.get_mutable_context()\n", - " plant.get_input_port(0).FixValue(context, [touchdown_angle])\n", - " context.SetAccuracy(1e-5)\n", - "\n", - " zs = np.linspace(np.cos(touchdown_angle) + 0.001, 0.95, 25)\n", - " zns = 0 * zs\n", - " for i in range(len(zs)):\n", - " s.z = zs[i]\n", - " s.xdot = plant.apex_velocity_from_dimensionless_system_energy(Etilde, s.z)\n", - " context.SetTime(0.0)\n", - " context.get_mutable_continuous_state_vector().SetFromVector(s[:])\n", - " simulator.Initialize()\n", - " # Note: With this duration, I sometimes get an extra \"touchdown\" after the\n", - " # apex, which results in apex-touchdown; touchdown-takeoff-apex on the\n", - " # console. It's not a double reset, the consecutive touchdowns are two\n", - " # different sims.\n", - " simulator.AdvanceTo(0.6)\n", - " zns[i] = plant.last_apex\n", - " plant.last_apex = None\n", - "\n", - " fig, ax = plt.subplots()\n", - " ax.plot(zs, zns)\n", - " ax.plot(zs, zs)\n", - " ax.axis(\"equal\")\n", - " ax.set_xlim([zs[0], zs[-1]])\n", - " ax.set_ylim([zs[0], zs[-1]])\n", - " ax.set_xlabel(\"apex height z[n]\")\n", - " ax.set_ylabel(\"apex height z[n+1]\")\n", - "\n", - " display(mpld3.display())\n", - "\n", - "\n", - "slip_map()" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [] - } - ], - "metadata": { - "kernelspec": { - "display_name": "Python 3", - "language": "python", - "name": "python3" - }, - "language_info": { - "codemirror_mode": { - "name": "ipython", - "version": 3 - }, - "file_extension": ".py", - "mimetype": "text/x-python", - "name": "python", - "nbconvert_exporter": "python", - "pygments_lexer": "ipython3", - "version": "3.10.9 (main, Dec 15 2022, 10:44:50) [Clang 14.0.0 (clang-1400.0.29.202)]" - }, - "vscode": { - "interpreter": { - "hash": "b0fa6594d8f4cbf19f97940f81e996739fb7646882a419484c72d19e05852a7e" - } - } - }, - "nbformat": 4, - "nbformat_minor": 4 -} diff --git a/book/stochastic.html b/book/stochastic.html index ae427d4a..93c58d03 100644 --- a/book/stochastic.html +++ b/book/stochastic.html @@ -100,7 +100,7 @@

        Underactuated Robotics -

        The Master Equation

        +

        The Master Equation

        Let's start by looking at some simple examples.

        @@ -320,15 +320,14 @@

        Underactuated Robotics -

        The Cubic Example + Noise

        +

        The Cubic Polynomial + Noise

        -

        Now let's consider the discrete-time approximation of \[ \dot{x} = -x + - x^3, \] again with additive noise: \[ x[n+1] = x[n] + h (-x[n] + x[n]^3 + - w[n]). \] With $w[n]=0$, the system has only a single stable fixed point - (at the origin), whose deterministic region of attraction is given by $x - \in (-1,1)$. If we again simulate the system from a set of random initial - conditions and plot the histogram, we will see something like the figure - below.

        +

        Now let's consider the discrete-time approximation of \[ \dot{x} = -x + x^3, \] + again with additive noise: \[ x[n+1] = x[n] + h (-x[n] + x[n]^3 + w[n]). \] With + $w[n]=0$, the system has only a single stable fixed point (at the origin), whose + deterministic region of attraction is given by $x \in (-1,1)$. If we again + simulate the system from a set of random initial conditions and plot the + histogram, we will see something like the figure below.

        Histogram of the states of the bistable system with noise diff --git a/book/stochastic/BUILD.bazel b/book/stochastic/BUILD.bazel new file mode 100644 index 00000000..80e5dda4 --- /dev/null +++ b/book/stochastic/BUILD.bazel @@ -0,0 +1,14 @@ +# -*- mode: python -*- +# vi: set ft=python : + +# Copyright 2020-2022 Massachusetts Institute of Technology. +# Licensed under the BSD 3-Clause License. See LICENSE.TXT for details. + +load("@pip_deps//:requirements.bzl", "requirement") +load("//book/htmlbook/tools/jupyter:defs.bzl", "rt_ipynb_test") + +rt_ipynb_test( + name = "stochastic", + srcs = ["stochastic.ipynb"], + deps = ["//underactuated"], +) diff --git a/book/stochastic.ipynb b/book/stochastic/stochastic.ipynb similarity index 100% rename from book/stochastic.ipynb rename to book/stochastic/stochastic.ipynb diff --git a/pyproject.toml b/pyproject.toml index 9e7be70c..8bdaf1d3 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -2,7 +2,7 @@ name = "underactuated" # Use e.g. 2023.10.4.rc0 if I need to release a release candidate. # Use e.g. 2023.10.4.post1 if I need to rerelease on the same day. -version = "2024.04.12" +version = "2024.04.25" description = "MIT 6.821 - Underactuated Robotics" authors = ["Russ Tedrake "] license = "BSD License"