RobotLocomotion / drake

Model-based design and verification for robotics.
https://drake.mit.edu
Other
3.37k stars 1.27k forks source link

new graphviz system naming is fragile; plays badly with runpy.run_path #20914

Closed RussTedrake closed 9 months ago

RussTedrake commented 10 months ago

What happened?

I just spend an embarassing amount of time debugging an obscure error on the autograder (which was initially hard to reproduce locally). I suspect it is due to #20204.

Long story short, running python3 cleaned_notebook.py works fine. Running python3 -c "from runpy import run_path; run_path('cleaned_notebook.py')" causes dot to fail with Unknown HTML element <run_path>.

Running from the command line, the name in the diagram given the systems defined immediately in the py file is e.g. <B>__main__.InnerController</B><BR/>. But running through run_path, then naming scheme produces illegal dot output <B><run_path>.InnerController</B><BR/>

We should not allow GetGraphvizString to emit illegal graphviz.

Here are the contents of cleaned_notebook.py

#!/usr/bin/env python
# coding: utf-8

# # Pendulum with Vibrating Base

# In[ ]:

import matplotlib.pyplot as plt
import numpy as np
from pydrake.all import (
    AddMultibodyPlantSceneGraph,
    DiagramBuilder,
    LogVectorOutput,
    MatrixGain,
    MeshcatVisualizer,
    Multiplexer,
    Parser,
    Simulator,
    StartMeshcat,
    VectorSystem,
    plot_system_graphviz,
)

from underactuated import ConfigureParser, ManipulatorDynamics

# In[ ]:

# Start the visualizer (run this cell only once, each instance consumes a port)
meshcat = StartMeshcat()

# ## Problem Description
# In this problem you will write the controller to make the pendulum with vibrating base spin at constant velocity.
# At the end of the notebook, you will be able to check your work in a simulation environment we set up for you.
# 
# **These are the main steps of the exercise:**
# 1. Construct the physical model of the vibrating pendulum. This is done automatically by "parsing" a `.urdf` (Unified Robot Description Format) file.
# 2. Implement the controller you derived in the written part of this exercise. _This is the only piece of code you will need to write._
# 3. Wire up the closed-loop block diagram: connect the controller output with the system input, the system output with the visualizer etc.
# 4. Set up and run a simulation.

# In[ ]:

# parameters of the harmonic motion of the base
# defined globally in the notebook
h = 1.0
omega = np.pi

# ## Parse the `.urdf`
# The first block of our diagram is the vibrating pendulum.
# No need to write its equations of motion by hand: all the parameters of the system are described in its `.urdf` file.
# Drake can directly parse this file, and construct a `MultibodyPlant` (i.e. the vibrating-pendulum block in our diagram).
# The `.urdf` file contains all the physical data of the system, the visualization parameters (shapes and colors of the bodies), etc.
# Its html-like syntax is very easy to understand, give it a try!
# 
# Our robot has two bodies:
# 1. The base. This moves on a 1D rail and oscillates according to the harmonic law $h \sin (\omega t)$.
# 2. The pendulum. It is connected to the base through a pin. This is the body you will need to control.
# 
# **Attention!** Since the robot has two bodies, it also has two configuration variables.
# When writing the controller, we will take care of the first (position of the base) and ensure that it oscillates as required.
# Then the problem will be reduced to the control of the pendulum only.

# In[ ]:

# think of the builder as the construction site of our block diagram
builder = DiagramBuilder()

# instantiate the vibrating pendulum and the scene graph
# the scene graph is a container for the geometries of all the physical systems in our diagram
vibrating_pendulum, scene_graph = AddMultibodyPlantSceneGraph(
    builder,
    time_step=0.0,  # discrete update period , set to zero since system is continuous
)

# parse the urdf and populate the vibrating pendulum
parser = Parser(vibrating_pendulum)
ConfigureParser(parser)
parser.AddModelsFromUrl("package://underactuated/models/vibrating_pendulum.urdf")
vibrating_pendulum.Finalize()

# we set names for the systems to make our diagram more readable
vibrating_pendulum.set_name("Vibrating Pendulum")
scene_graph.set_name("Scene Graph")

# ## Write the Controller
# In this section we define two controllers:
# 1. An inner controller that makes the base oscillate with the harmonic motion. We wrote this for you.
# 2. The outer controller to make the pendulum spin at constant velocity. You will write part of this.
# 
# The final diagram will have the following structure:
# 
# ![figure](https://raw.githubusercontent.com/RussTedrake/underactuated/master/book/figures/exercises/vibrating_pendulum.jpg)

# In[ ]:

# this controller enforces the harmonic motion to the base
class InnerController(VectorSystem):
    def __init__(self, vibrating_pendulum):
        # 5 inputs: state of base + pendulum, pendulum torque
        # 2 outputs: force on base + torque on pendulum
        VectorSystem.__init__(self, 5, 2)
        self.vibrating_pendulum = vibrating_pendulum

    def DoCalcVectorOutput(
        self,
        context,
        controller_input,  # state of base + pendulum, pendulum torque
        controller_state,  # unused input (static controller)
        controller_output,  # force on base + torque on pendulum
    ):
        # unpack state
        q = controller_input[:2]  # base position + pendulum angle
        q_dot = controller_input[2:4]  # time derivative of q

        # extract manipulator equations: M*a + Cv = tauG + B*u + tauExt
        # (for this system B is the identity and the external forces tauExt are zero
        # hence, for simplicity, we will just drop them from the code)
        M, Cv, tauG, B, tauExt = ManipulatorDynamics(self.vibrating_pendulum, q, q_dot)
        Minv = np.linalg.inv(M)
        tau = tauG - Cv

        # desired acceleration of the base
        # note that this depends on time
        t = context.get_time()
        a_base = -(omega**2) * h * np.sin(omega * t)

        # cancel out the dynamics of the pendulum
        # and enforce harmonic motion to the base
        # (to fully explain these lines we would need a small math derivation,
        # since this is not the goal of the exercise we skip it,
        # if you want, you can try your own, it is not hard)
        torque = controller_input[-1]
        force = -tau[0]  # cancel gravity, centrifugal, and Coriolis
        force += (
            -(tau[1] + torque) * Minv[0, 1] / Minv[0, 0]
        )  # cancel pendulum effects on the base
        force += a_base / Minv[0, 0]  # enforce desired acceleration

        # control signal
        controller_output[:] = [force, torque]

# In[ ]:

# this is the controller that makes the pendulum spin at constant velocity
# you will write the control law in it in the next cell
# by defining the function called "pendulum_torque"
class OuterController(VectorSystem):
    def __init__(self, vibrating_pendulum, pendulum_torque):
        # 2 inputs: pendulum state
        # 1 output: torque on pendulum
        VectorSystem.__init__(self, 2, 1)
        self.vibrating_pendulum = vibrating_pendulum
        self.pendulum_torque = pendulum_torque

    def DoCalcVectorOutput(
        self,
        context,
        controller_input,  # pendulum state
        controller_state,  # unused input (static controller)
        controller_output,  # torque on pendulum
    ):
        # unpack state
        theta, theta_dot = controller_input

        # get pendulum parameters
        # absolute values to make these parameters positive
        pendulum = self.vibrating_pendulum.GetBodyByName("pendulum")
        m = pendulum.default_mass()
        g = np.abs(self.vibrating_pendulum.gravity_field().gravity_vector()[2])
        l = np.abs(pendulum.default_com()[2])

        # controller
        t = context.get_time()
        controller_output[:] = [self.pendulum_torque(m, g, l, theta, theta_dot, t)]

# In the following cell, modify the function `pendulum_torque` to implement the control law you derived in the point (b) of the exercise.
# The function must return the torque to be applied to the pendulum (`int` or `float`).
# Currently, it just returns zero torque, and the pendulum oscillates freely.
# 
# The parameters of this function are:
# - `m`: mass of the bob of the pendulum,
# - `g`: gravity acceleration ($>0$),
# - `l`: length of the pendulum rod,
# - `theta`: angle of the pendulum,
# - `theta_dot`: angular velocity of the pendulum,
# - `t`: time.
# 
# **Very important:**
# To complete this assignment, you only need to work in the following cell.

# In[ ]:

def pendulum_torque(m, g, l, theta, theta_dot, t):
    torque = 0  # modify here
    return torque

# ## Wire up the Block Diagram
# Now it is time to construct the block diagram: connect the controllers to the system, the sensors to the controllers, etc.
# We aim to construct the diagram shown above, with the addition of a visualizer, which will be connected with the system state.
# 
# **Troubleshooting:**
# Note that we already initialized the `builder` of the block diagram when parsing the `.urdf` file.
# Hence, by running the following cell multiple times, you would actually try to wire the blocks in the diagram more than once.
# This is not acceptable, and Drake will raise the error `RuntimeError: Input port is already wired.`
# If you wish to modify the next cell and rerun the program to see the effects of your modification, you must rerun the cell where the `builder` is initialized first (i.e. the cell with the line `builder = DiagramBuilder()`).

# In[ ]:

# instantiate controllers
inner_controller = builder.AddNamedSystem(
    "Inner Controller", InnerController(vibrating_pendulum)
)
outer_controller = builder.AddNamedSystem(
    "Outer Controller", OuterController(vibrating_pendulum, pendulum_torque)
)

# instantiate visualizer
visualizer = MeshcatVisualizer.AddToBuilder(builder, scene_graph, meshcat)
visualizer.set_name("Visualizer")
meshcat.Set2dRenderMode(xmin=-2.5, xmax=2.5, ymin=-1.5, ymax=2.5)

# logger that records the state trajectory during simulation
logger = LogVectorOutput(vibrating_pendulum.get_state_output_port(), builder)
logger.set_name("Logger")

# mux block to squeeze the (base + pendulum) state and
# the outer control signal in a single cable
mux = builder.AddNamedSystem("Mux", Multiplexer([4, 1]))

# selector that extracts the pendulum state from the state of the base and the pendulum
selector = builder.AddNamedSystem(
    "Pendulum-State Selector",
    MatrixGain(
        np.array(
            [
                [0, 1, 0, 0],  # selects the angle of the pendulum
                [0, 0, 0, 1],  # selects the angular velocity of the pendulum
            ]
        )
    ),
)

# (base + pendulum) state, outer control -> mux
builder.Connect(vibrating_pendulum.get_state_output_port(), mux.get_input_port(0))
builder.Connect(outer_controller.get_output_port(0), mux.get_input_port(1))

# mux -> inner controller
builder.Connect(mux.get_output_port(0), inner_controller.get_input_port(0))

# (base + pendulum) state -> selector
builder.Connect(vibrating_pendulum.get_state_output_port(), selector.get_input_port(0))

# selector -> outer controller
builder.Connect(selector.get_output_port(0), outer_controller.get_input_port(0))

# inner controller -> system input
builder.Connect(
    inner_controller.get_output_port(0),
    vibrating_pendulum.get_actuation_input_port(),
)

# finalize block diagram
diagram = builder.Build()
diagram.set_name("Block Diagram for the Control of the Vibrating Pendulum")

# When connecting all the blocks by hand, it is possible to do some mistakes.
# To double check your work, you can use the function `plot_system_graphviz`, which plots the overall block diagram you built.
# You can compare the automatically-generated block diagram with the one above

# In[ ]:

# give names to the blocks (just to make the plot nicer)

# plot current diagram
plt.figure(figsize=(20, 10))
print(diagram.GetGraphvizString())
plot_system_graphviz(diagram)

# ## Closed-Loop Simulation
# Now we can finally simulate the system in closed loop with the controllers we wrote.
# In the meanwhile, we will also set up a "video recording" with which we will be able to playback the simulation.
# System trajectories will be stored in the `logger` and plotted in the last cell.

# In[ ]:

# initial state of the pendulum
theta = 0.0
theta_dot = 0.0

# simulation time
sim_time = 10

# Make sure you have MeshCat visualizer window open in your browser (the URL is printed from the `StartMeshcat` cell near the top).  Then run the simulation:

# In[ ]:

# start recording the video for the animation of the simulation
visualizer.StartRecording()

# set up a simulator
simulator = Simulator(diagram)
simulator.set_publish_every_time_step(False)

# set the initial conditions
# do not change the initial conditions of the base
# since they must agree with the harmonic motion h*sin(omega*t)
context = simulator.get_mutable_context()
context.SetTime(0.0)  # reset current time
context.SetContinuousState(
    (
        0.0,  # initial position of the base, DO NOT CHANGE!
        theta,  # initial angle of the pendulum
        h * omega,  # initial velocity of the base, DO NOT CHANGE!
        theta_dot,  # initial angular velocity of the pendulum
    )
)

# simulate from zero to sim_time
simulator.Initialize()
simulator.AdvanceTo(sim_time)

# stop recording and send the animation to meshcat
visualizer.StopRecording()
visualizer.PublishRecording()

# We conclude by plotting the position of the base and the angular velocity of the pendulum as functions of time.
# If we did our job correctly,
# - the first should coincide with the desired position $h \sin (\omega t)$,
# - the second should coincide with the response of the first-order system $\ddot \theta = f(\dot \theta)$ you came up with in point (a) of the exercise.

# In[ ]:

# base position as a function of time
log = logger.FindLog(context)
plt.plot(log.sample_times(), log.data()[0, :], label="Base position (m)")

# pendulum angular velocity as a function of time
plt.plot(
    log.sample_times(),
    log.data()[-1, :],
    label="Pendulum angular velocity (rad/s)",
)

# misc plot settings
plt.xlabel("Time (s)")
plt.xlim(0, sim_time)
plt.grid(True)
plt.legend()

# ## How Will this Notebook Be Graded?
# If you are enrolled in the class, this notebook will be graded using [Gradescope](https://www.gradescope.com).
# We will send you the details of how to access the course page in Gradescope by email.
# 
# We will replicate your work by running your notebook and checking the relevant functions (pendulum_torque) for correct outputs.
# We will also test your work by looking at the response of the angular velocity $\dot \theta(t)$ from the plot above.
# You will get full score if *all* the following tests succeed:
# - The response $\dot \theta(t)$ is a nondecreasing function (a first order system, such as $\ddot \theta = f(\dot \theta)$, cannot oscillate).
# - The terminal velocity $\dot \theta (t=10\text{ s})$ is less than $1.001 \text{ rad/s}$.
# - The terminal velocity $\dot \theta (t=10\text{ s})$ is greater than $0.99 \text{ rad/s}$.
# 
# While the first two conditions should always hold (you did things properly), to fulfill the third you might want to increase your controller gains!

# ## Autograding
# You can check your work by running the following cell:

# In[ ]:

from underactuated.exercises.grader import Grader
from underactuated.exercises.pend.test_vibrating_pendulum import TestVibratingPendulum

Grader.grade_output([TestVibratingPendulum], [locals()], "results.json")
Grader.print_test_results("results.json")

Version

No response

What operating system are you using?

Ubuntu 22.04

What installation option are you using?

pip install drake

Relevant log output

No response

RussTedrake commented 10 months ago

cc @wernerpe

jwnimmer-tri commented 9 months ago

Let me make sure I understand the call to action: we should be sure to escape any special HTML characters when inserting the system.get_name() and type(system) strings into the Graphviz output. Is that right?

According to here, that just means < and > and & need escaping.

The relevant code is here: https://github.com/RobotLocomotion/drake/blob/199717d89dccca3848718b185e258378ec4a92a8/systems/framework/system_base.cc#L80-L90

I suppose we might as well escape the port names as well: https://github.com/RobotLocomotion/drake/blob/199717d89dccca3848718b185e258378ec4a92a8/systems/framework/system_base.cc#L226

RussTedrake commented 9 months ago

Yes. I think that's the most robust solution. Thanks.