<jupyter_start><jupyter_text>Double Pendulum  Notebook SetupThe following cell will install Drake.<jupyter_code>try:
  import pydrake
  import underactuated
except ImportError:
  !curl -s https://raw.githubusercontent.com/lyltc1/ControlPractice/master/script/jupyter_setup.py > jupyter_setup.py
  from jupyter_setup import setup_underactuated
  setup_underactuated()

# Setup matplotlib backend (to notebook, if possible, or inline).  
from script.jupyter import setup_matplotlib_backend
plt_is_interactive = setup_matplotlib_backend('notebook')
from math import sin
from IPython import get_ipython
from IPython.display import HTML

from pydrake.all import (AddMultibodyPlantSceneGraph, DiagramBuilder, Parser,
                         PlanarSceneGraphVisualizer, Simulator, VectorSystem)
from underactuated import FindResource, ManipulatorDynamics


class Controller(VectorSystem):
    """Defines a feedback controller for the double pendulum.

    The controller applies torques at the joints in order to:

    1) cancel out the dynamics of the double pendulum,
    2) make the first joint swing with the dynamics of a single pendulum, and
    3) drive the second joint towards zero.

    The magnitude of gravity for the imposed single pendulum dynamics is taken
    as a constructor argument.  So you can do fun things like pretending that
    gravity is zero, or even inverting gravity!
    """

    def __init__(self, multibody_plant, gravity):
        # 4 inputs (double pend state), 2 torque outputs.
        VectorSystem.__init__(self, 4, 2)
        self.plant = multibody_plant
        self.g = gravity

    def DoCalcVectorOutput(self, context, double_pend_state, unused, torque):
        # Extract manipulator dynamics.
        q = double_pend_state[:2]
        v = double_pend_state[-2:]
        (M, Cv, tauG, B, tauExt) = ManipulatorDynamics(self.plant, q, v)

        # Desired pendulum parameters.
        length = 2.
        b = .1

        # Control gains for stabilizing the second joint.
        kp = 1
        kd = .1

        # Cancel double pend dynamics and inject single pend dynamics.
        torque[:] = Cv - tauG - tauExt + M.dot(
            [self.g / length * sin(q[0]) - b * v[0], -kp * q[1] - kd * v[1]])


def simulate(gravity=-9.8):      
    builder = DiagramBuilder()
    plant, scene_graph = AddMultibodyPlantSceneGraph(builder, time_step=0.0)

    # Load the double pendulum from Universal Robot Description Format
    parser = Parser(plant, scene_graph)
    parser.AddModelFromFile(FindResource("models/double_pendulum.urdf"))
    plant.Finalize()

    controller = builder.AddSystem(Controller(plant, gravity))
    builder.Connect(plant.get_state_output_port(), controller.get_input_port(0))
    builder.Connect(controller.get_output_port(0), plant.get_actuation_input_port())

    visualizer = builder.AddSystem(
        PlanarSceneGraphVisualizer(scene_graph, xlim=[-2.8, 2.8], ylim=[-2.8, 2.8], 
                                   show=plt_is_interactive))
    builder.Connect(scene_graph.get_pose_bundle_output_port(),
                    visualizer.get_input_port(0))

    diagram = builder.Build()

    # Set up a simulator to run this diagram
    simulator = Simulator(diagram)
    if plt_is_interactive:
        simulator.set_target_realtime_rate(1.)

    # Set the initial conditions
    context = simulator.get_mutable_context()
    context.SetContinuousState((1., 0., 0.2, 0.))  # (θ₁, θ₂, θ̇₁, θ̇₂)

    # Simulate
    if not plt_is_interactive:
        visualizer.start_recording()
    duration = 3.0 if get_ipython() else 0.1 # sets a shorter duration during testing
    simulator.AdvanceTo(duration)
    if not plt_is_interactive:
        ani = visualizer.get_recording_as_animation()
        display(HTML(ani.to_jshtml()))
simulate()<jupyter_output><empty_output>
{'fraction_non_alphanumeric': 0.08536266038229903, 'fraction_numerical': 0.01125949201361613, 'mean_word_length': 3.3163841807909606, 'pattern_counts': {'":': 0, '<': 5, '<?xml version=': 0, '>': 6, 'https://': 1, 'lorem ipsum': 0, 'www.': 0, 'xml': 0}, 'pii_count': 0, 'substrings_counts': 0, 'word_list_counts': {'cursed_substrings.json': 1, 'profanity_word_list.json': 0, 'sexual_word_list.json': 0, 'zh_pornsignals.json': 0}}
{'chain_length': '1', 'dir': 'jupyter-structured-clean-dedup', 'id': '269295', 'license': 'MIT', 'n_tokens_mistral': 1212, 'n_tokens_neox': 1153, 'n_words': 321, 'path': 'DoublePendulum.ipynb', 'repo_name': 'lyltc1/ControlPractice', 'size': '3819'}