running_as_notebook = False  # Manually set this to True if you are a human (see #13862).from IPython.display import display
from ipywidgets import ToggleButton
import numpy as np

from pydrake.common.value import Value
from pydrake.geometry import DrakeVisualizer, FramePoseVector, SceneGraph
from pydrake.math import RigidTransform
from pydrake.multibody.plant import MultibodyPlant
from pydrake.multibody.parsing import Parser
from pydrake.multibody.tree import BodyIndex
from pydrake.systems.analysis import Simulator                                      
from pydrake.systems.framework import DiagramBuilder, LeafSystem
from pydrake.systems.jupyter_widgets import PoseSliders


class PoseToFramePoseVector(LeafSystem):
    """
    Wraps a single pose input into a FramePoseVector.
    """

    def __init__(self, frame_id):
        LeafSystem.__init__(self)
        self.frame_id = frame_id
        self.DeclareAbstractInputPort(
          "pose", Value(RigidTransform.Identity()))
        self.DeclareAbstractOutputPort(
          "vector", lambda: Value(FramePoseVector()), self.CalcOutput)

    def CalcOutput(self, context, output):
        pose = self.EvalAbstractInput(context, 0).get_value()
        output.get_mutable_value().set_value(id=frame_id, value=pose)
      

builder = DiagramBuilder()

# Note: Don't use AddMultibodyPlantSceneGraph because we are only using
# MultibodyPlant for parsing, then wiring our sliders directly to SceneGraph.
scene_graph = builder.AddSystem(SceneGraph())
plant = MultibodyPlant(time_step=0.0)
plant.RegisterAsSourceForSceneGraph(scene_graph)
Parser(plant, scene_graph).AddModelsFromUrl(
    url="package://drake_models/ycb/006_mustard_bottle.sdf")
plant.Finalize()

frame_id = plant.GetBodyFrameIdOrThrow(BodyIndex(1))                                            
                                            
# Add pose sliders.
sliders = builder.AddSystem(PoseSliders(
    visible=PoseSliders.Visible(),
    min_range=PoseSliders.MinRange(), 
    max_range=PoseSliders.MaxRange(),
    value=PoseSliders.Value()))
to_vector = builder.AddSystem(PoseToFramePoseVector(frame_id))
builder.Connect(sliders.get_output_port(0), to_vector.get_input_port(0))
builder.Connect(
    to_vector.get_output_port(0),
    scene_graph.get_source_pose_port(plant.get_source_id()))

# Note: We can't use AddDefaultVisualization because there is no plant
# in the Diagram.
DrakeVisualizer.AddToBuilder(builder, scene_graph)

diagram = builder.Build()
simulator = Simulator(diagram)

if running_as_notebook:  
    simulator.set_target_realtime_rate(1.0)
    stop_button = ToggleButton(value=False, description='Stop Simulation')
    display(stop_button)
    while not stop_button.value:
        simulator.AdvanceTo(simulator.get_context().get_time() + 5.0)
    stop_button.value = False
else:  # running as a test.
    simulator.AdvanceTo(0.1)
from ipywidgets import Checkbox, FloatSlider, FloatText, ToggleButtons

from pydrake.systems.jupyter_widgets import WidgetSystem

checkbox = Checkbox(value=False, description='Check box')
slider = FloatSlider(value=3.27, min=0, max=10.0, step=0.1, description='Slider', continuous_update=True)
text = FloatText(value=7.5, description='Text')
wsg = ToggleButtons(value=0.107, description="SchunkWsg", options=[('Open', 0.107), ('Close', 0.002)])

sys = WidgetSystem([checkbox], [slider], [text], [wsg], [checkbox, slider, text, wsg])
context = sys.CreateDefaultContext()for i in range(sys.num_output_ports()):
    print(sys.get_output_port(i).Eval(context))