### Start Meshcat Visualizer

Source: https://github.com/russtedrake/manipulation/blob/master/book/clutter/point_cloud_processing.ipynb

Initializes and starts the Meshcat visualizer, which is used to display 3D objects and point clouds interactively. This is a crucial step for visualizing the results of point cloud processing.

```python
# Start the visualizer.
meshcat = StartMeshcat()
```

--------------------------------

### Writing Example Setup in Python

Source: https://github.com/russtedrake/manipulation/blob/master/book/force/writing.ipynb

This function demonstrates setting up a physics simulation with a drawing object (chalk) and a surface (board) using Drake. It configures the plant, scene graph, and visualizer, including adding geometry and joints.

```python
def writing_example():
    builder = DiagramBuilder()

    time_step = 0.001
    plant, scene_graph = AddMultibodyPlantSceneGraph(builder, time_step)
    controller_plant = MultibodyPlant(time_step)
    # Add the chalk to both
    chalk_instance = AddShape(
        plant,
        Capsule(0.01, 0.2),
        "chalk",
        mass=1,
        mu=1,
        color=[1, 0.34, 0.2, 1.0],
    )
    AddFloatingXyzJoint(
        plant, plant.GetFrameByName("chalk"), chalk_instance, actuators=True
    )
    chalk_instance = AddShape(
        controller_plant,
        Capsule(0.01, 0.2),
        "chalk",
        mass=1,
        mu=1,
        color=[1, 0.34, 0.2, 1.0],
    )
    AddFloatingXyzJoint(
        controller_plant,
        controller_plant.GetFrameByName("chalk"),
        controller_plant.GetModelInstanceByName("chalk"),
        actuators=True,
    )

    # Add a writing surface to the sim plant only
    board_instance = AddShape(
        plant, Box(2, 2, 0.2), "board", color=[0.05, 0.05, 0.05, 1]
    )
    plant.WeldFrames(
        plant.world_frame(),
        plant.GetFrameByName("board"),
        RigidTransform([0, 0, -0.1]),
    )

    plant.Finalize()
    controller_plant.Finalize()

    q0 = [0, 0, 0.2]
    plant.SetDefaultPositions(q0)

    MeshcatVisualizer.AddToBuilder(builder, scene_graph, meshcat)
    meshcat.Delete()
```

--------------------------------

### Setup IIWA Robot and Jacobian Example in Drake

Source: https://github.com/russtedrake/manipulation/blob/master/book/pick/jacobian.ipynb

Configures a Drake diagram with an iiwa robot, a wsg gripper, and a MeshCat visualizer. It includes a custom system to print the kinematic Jacobian of the robot's end-effector and sets up interactive sliders for controlling joint positions.

```python
def pick_and_place_jacobians_example():
    builder = DiagramBuilder()

    plant, scene_graph = AddMultibodyPlantSceneGraph(builder, time_step=0)
    parser = Parser(plant)
    ConfigureParser(parser)
    parser.AddModelsFromUrl("package://manipulation/iiwa_and_wsg.dmd.yaml")
    plant.Finalize()

    meshcat.Delete()
    visualizer = MeshcatVisualizer.AddToBuilder(
        builder, scene_graph.get_query_output_port(), meshcat
    )

    G = plant.GetBodyByName("body").body_frame()
    print_jacobian = builder.AddSystem(PrintJacobian(plant, G))
    builder.Connect(plant.get_state_output_port(), print_jacobian.get_input_port())

    # If you want to set the initial positions manually, use this:
    # plant.SetPositions(plant.GetMyContextFromRoot(context),
    #                   plant.GetModelInstanceByName("iiwa"),
    #                   [0, 0, 0, 0, 0, 0, 0])

    default_interactive_timeout = None if running_as_notebook else 1.0
    sliders = builder.AddSystem(JointSliders(meshcat, plant))
    diagram = builder.Build()
    sliders.Run(diagram, default_interactive_timeout)
    meshcat.DeleteAddedControls()


meshcat.DeleteAddedControls()
pick_and_place_jacobians_example()
```

--------------------------------

### Initialize and Run Simulation

Source: https://github.com/russtedrake/manipulation/blob/master/book/clutter/clutter_clearing.ipynb

This snippet covers initializing a Simulator with a built diagram, getting the simulation context, and then setting the initial poses for floating base bodies in a plant. It concludes with advancing the simulation to a specific time and flushing Meshcat.

```python
simulator = Simulator(diagram)
    context = simulator.get_context()

    plant_context = plant.GetMyMutableContextFromRoot(context)
    z = 0.2
    for body_index in plant.GetFloatingBaseBodies():
        tf = RigidTransform(
            UniformlyRandomRotationMatrix(generator),
            [rng.uniform(0.35, 0.65), rng.uniform(-0.12, 0.28), z],
        )
        plant.SetFreeBodyPose(plant_context, plant.get_body(body_index), tf)
        z += 0.1

    simulator.AdvanceTo(0.1)
    meshcat.Flush()  # Wait for the large object meshes to get to meshcat.
```

--------------------------------

### Initialize Meshcat Visualizer

Source: https://github.com/russtedrake/manipulation/blob/master/book/figures/grasp_frames.ipynb

Starts an instance of the Meshcat visualizer, which will be used to display the 3D scene. This is a prerequisite for visualization.

```python
meshcat = StartMeshcat()

```

--------------------------------

### Import Drake Libraries and Start Meshcat Visualizer (Python)

Source: https://github.com/russtedrake/manipulation/blob/master/book/clutter/stiffness.ipynb

Imports necessary libraries from the Drake toolkit, including NumPy, Rgba, StartMeshcat, and benchmarks. It then starts the Meshcat visualizer, which is used for interactive visualization of the system dynamics.

```python
import numpy as np
from pydrake.all import Rgba, StartMeshcat
from pydrake.multibody.benchmarks import MassDamperSpringAnalyticalSolution

from manipulation import running_as_notebook
```

```python
# Start the visualizer.
meshcat = StartMeshcat()
```

--------------------------------

### Setup Diagram for Visualization

Source: https://github.com/russtedrake/manipulation/blob/master/book/robot/simulation.ipynb

Initializes a DiagramBuilder and adds MultibodyPlant and SceneGraph components to it. It then loads the robot model into both the plant and scene graph and finalizes the plant.

```python
meshcat.Delete()
meshcat.DeleteAddedControls()
builder = DiagramBuilder()

# Adds both MultibodyPlant and the SceneGraph, and wires them together.
plant, scene_graph = AddMultibodyPlantSceneGraph(builder, time_step=1e-4)
# Note that we parse into both the plant and the scene_graph here.
Parser(plant, scene_graph).AddModelsFromUrl(
    "package://drake_models/iiwa_description/sdf/iiwa14_no_collision.sdf"
)
plant.WeldFrames(plant.world_frame(), plant.GetFrameByName("iiwa_link_0"))
plant.Finalize()
```

--------------------------------

### Gamepad Teleoperation Setup (Python)

Source: https://github.com/russtedrake/manipulation/blob/master/book/intro/intro.ipynb

This Python code snippet demonstrates the initial setup for teleoperation using a gamepad. It defines the scenario data, which includes directives for adding models and setting up drivers for the IIWA and WSG manipulators. This forms the basis for integrating gamepad controls, though the specific gamepad input handling is not shown here.

```python
scenario_data = """
directives:
- add_directives:
    file: package://manipulation/clutter.dmd.yaml
- add_model:
    name: foam_brick
    file: package://manipulation/hydro/061_foam_brick.sdf
    default_free_body_pose:
        base_link:
            translation: [0, -0.6, 0.2]
model_drivers:
    iiwa: !IiwaDriver
      control_mode: position_only
      hand_model_name: wsg
    wsg: !SchunkWsgDriver {}
"""


```

--------------------------------

### Setup PR2 Simulation with HardwareStation

Source: https://github.com/russtedrake/manipulation/blob/master/book/mobile/pr2.ipynb

Creates a basic simulation environment for the PR2 robot using Drake's HardwareStation. It loads a scenario defining the robot model and its drivers, then sets up a simulator and configures initial conditions, excluding mimic joints.

```yaml
directives:
- add_model:
    name: pr2
    file: package://drake_models/pr2_description/urdf/pr2_simplified.urdf
model_drivers:
    pr2: !JointStiffnessDriver
        gains:
            # TODO(russt): tune these!
            x_motor: {kp: 600, kd: 120}
            y_motor: {kp: 600, kd: 120}
            theta_motor: {kp: 600, kd: 120}
            torso_lift_motor: {kp: 600, kd: 120}
            head_pan_motor: {kp: 100, kd: 20}
            head_tilt_motor: {kp: 100, kd: 20}
            r_upper_arm_roll_motor: {kp: 600, kd: 120}
            r_shoulder_pan_motor: {kp: 600, kd: 120}
            r_shoulder_lift_motor: {kp: 600, kd: 120}
            r_forearm_roll_motor: {kp: 400, kd: 80}
            r_elbow_flex_motor: {kp: 400, kd: 80}
            r_wrist_flex_motor: {kp: 200, kd: 40}
            r_wrist_roll_motor: {kp: 200, kd: 40}
            r_gripper_l_finger_motor: {kp: 100, kd: 20}
            l_upper_arm_roll_motor: {kp: 600, kd: 120}
            l_shoulder_pan_motor: {kp: 600, kd: 120}
            l_shoulder_lift_motor: {kp: 600, kd: 120}
            l_forearm_roll_motor: {kp: 400, kd: 80}
            l_elbow_flex_motor: {kp: 400, kd: 80}
            l_wrist_flex_motor: {kp: 200, kd: 40}
            l_wrist_roll_motor: {kp: 200, kd: 40}
            l_gripper_l_finger_motor: {kp: 100, kd: 20}
```

```python
scenario_data = """
directives:
- add_model:
    name: pr2
    file: package://drake_models/pr2_description/urdf/pr2_simplified.urdf
model_drivers:
    pr2: !JointStiffnessDriver
        gains:
            # TODO(russt): tune these!
            x_motor: {kp: 600, kd: 120}
            y_motor: {kp: 600, kd: 120}
            theta_motor: {kp: 600, kd: 120}
            torso_lift_motor: {kp: 600, kd: 120}
            head_pan_motor: {kp: 100, kd: 20}
            head_tilt_motor: {kp: 100, kd: 20}
            r_upper_arm_roll_motor: {kp: 600, kd: 120}
            r_shoulder_pan_motor: {kp: 600, kd: 120}
            r_shoulder_lift_motor: {kp: 600, kd: 120}
            r_forearm_roll_motor: {kp: 400, kd: 80}
            r_elbow_flex_motor: {kp: 400, kd: 80}
            r_wrist_flex_motor: {kp: 200, kd: 40}
            r_wrist_roll_motor: {kp: 200, kd: 40}
            r_gripper_l_finger_motor: {kp: 100, kd: 20}
            l_upper_arm_roll_motor: {kp: 600, kd: 120}
            l_shoulder_pan_motor: {kp: 600, kd: 120}
            l_shoulder_lift_motor: {kp: 600, kd: 120}
            l_forearm_roll_motor: {kp: 400, kd: 80}
            l_elbow_flex_motor: {kp: 400, kd: 80}
            l_wrist_flex_motor: {kp: 200, kd: 40}
            l_wrist_roll_motor: {kp: 200, kd: 40}
            l_gripper_l_finger_motor: {kp: 100, kd: 20}
"""

scenario = LoadScenario(data=scenario_data)
station = MakeHardwareStation(scenario, meshcat)
simulator = Simulator(station)
context = simulator.get_mutable_context()

plant = station.GetSubsystemByName("plant")
pr2 = plant.GetModelInstanceByName("pr2")

mimic_joints = [
    "gripper_r_finger_joint",
    "gripper_l_finger_tip_joint",
    "gripper_r_finger_tip_joint",
    "gripper_r_finger_joint",
    "gripper_l_finger_tip_joint",
    "gripper_r_finger_tip_joint",
]
x0 = station.GetOutputPort("pr2.state_estimated").Eval(context)
x0_wout_mimics = []
for i, state_name in enumerate(plant.GetStateNames(pr2)):
    if all(mimic not in state_name for mimic in mimic_joints):
        x0_wout_mimics.append(x0[i])

station.GetInputPort("pr2.desired_state").FixValue(context, x0_wout_mimics)
simulator.AdvanceTo(0.1)
```

--------------------------------

### Setup Hardware Station Simulation (Python)

Source: https://github.com/russtedrake/manipulation/blob/master/book/mobile/spot.ipynb

Creates a hardware station simulation for the Spot robot using a scenario file. It loads the scenario, configures the station with Meshcat and remote callbacks, and initializes a simulator. It sets an initial state and advances the simulation briefly.

```python
scenario = LoadScenario(
    filename=FindResource(
        "models/spot/spot_with_arm_and_floating_base_actuators.scenario.yaml"
    )
)
station = MakeHardwareStation(
    scenario,
    meshcat,
    parser_preload_callback=lambda parser: AddSpotRemote(parser.package_map()),
)
simulator = Simulator(station)
context = simulator.get_mutable_context()
x0 = station.GetOutputPort("spot.state_estimated").Eval(context)
station.GetInputPort("spot.desired_state").FixValue(context, x0)
simulator.AdvanceTo(0.1);
```

--------------------------------

### Import Drake Libraries and Setup

Source: https://github.com/russtedrake/manipulation/blob/master/book/pick/pick.ipynb

Imports necessary modules from Drake and related libraries for robotics manipulation tasks. Includes setup for running within a Jupyter notebook environment.

```python
import mpld3
import numpy as np
from matplotlib import pyplot as plt
from pydrake.all import (
    AddMultibodyPlantSceneGraph,
    AngleAxis,
    DiagramBuilder,
    Integrator,
    JacobianWrtVariable,
    LeafSystem,
    MeshcatVisualizer,
    MultibodyPlant,
    MultibodyPositionToGeometryPose,
    Parser,
    PiecewisePolynomial,
    PiecewisePose,
    Quaternion,
    Rgba,
    RigidTransform,
    RotationMatrix,
    SceneGraph,
    Simulator,
    StartMeshcat,
    TrajectorySource,
)

from manipulation import running_as_notebook
from manipulation.station import LoadScenario, MakeHardwareStation
from manipulation.utils import RenderDiagram

if running_as_notebook:
    mpld3.enable_notebook()
```

--------------------------------

### Clear Meshcat Scene

Source: https://github.com/russtedrake/manipulation/blob/master/book/force/exercises/04_peg_in_hole.ipynb

Clears the Meshcat visualization environment, preparing it for a new simulation or visualization setup. This is a common setup step before starting a new manipulation task.

```python
# Clear meshcat
```

--------------------------------

### Create Camera Simulation Example (Python)

Source: https://github.com/russtedrake/manipulation/blob/master/book/pose.html

Demonstrates setting up a scene with a single object and an RgbdSensor in Drake. The code allows for evaluating output ports to obtain color and depth images.

```python
document.write(notebook_link('pose', notebook="camera_sim"))
```

--------------------------------

### Scene Setup: Create Directories

Source: https://github.com/russtedrake/manipulation/blob/master/book/trajectories/exercises/5_plan_place_initials.ipynb

Sets up the necessary directories for storing scenario files and assets. Ensures that the 'scenarios' and 'assets' directories exist, creating them if they don't. This is a prerequisite for loading and saving scene configurations.

```python
outdir = "scenarios"
assets_dir = "assets"
os.makedirs(outdir, exist_ok=True)
os.makedirs(assets_dir, exist_ok=True)
```

--------------------------------

### Initialize Robot and Gripper Setup (Placeholders)

Source: https://github.com/russtedrake/manipulation/blob/master/book/trajectories/exercises/kinematic_trajopt_around_shelf_iiwa.ipynb

These are placeholder comments indicating where the code for setting up the diagram builder, MultibodyPlant, SceneGraph, adding the Kuka Iiwa robot with collision model, and attaching the WSG gripper should be implemented.

```python
# TODO: create your diagram builder

# TODO: create your plant and scene graph with a time step of 0.001

# TODO: add an Iiwa robot to your plant and set collision model to be "with_box_collision"

# TODO: add the WSG gripper to your iiwa, set roll to 0.0 and weld it
```

--------------------------------

### Plot Trajectory - Python

Source: https://github.com/russtedrake/manipulation/blob/master/book/trajectories/gcs_simple_2d_cspace.ipynb

Visualizes a planned trajectory on a 2D plot. It shows the start and end points, the path itself, and overlays obstacles. The plot is configured with axis limits and ticks matching the environment setup.

```python
from pydrake.trajectories import PiecewisePolynomial

def plot_trajectory(traj):
    plt.figure(figsize=(6, 6))

    # Assuming 'obstacles' is a globally defined list of polygons
    # for O in obstacles:
    #     plt.fill(*O.T, fc="lightcoral", ec="k", zorder=4)

    plt.plot(*traj.value(traj.start_time()), "kx")
    plt.plot(*traj.value(traj.end_time()), "kx")
    times = np.linspace(traj.start_time(), traj.end_time(), 1000)
    waypoints = traj.vector_values(times)
    plt.plot(*waypoints, "b", zorder=5)

    plt.axis("square")
    plt.xlim([x_min[0], x_max[0]])
    plt.ylim([x_min[1], x_max[1]])
    plt.xticks(range(6))
    plt.yticks(range(6))
    plt.grid(1)
```

--------------------------------

### Setup and Run Simulation with Drake

Source: https://github.com/russtedrake/manipulation/blob/master/book/trajectories/exercises/5_plan_place_initials.ipynb

This Python code snippet outlines the steps to set up and run a Drake simulation for a robot arm and gripper. It involves loading a scenario, creating a diagram builder, adding trajectory sources, connecting them to robot components, and initializing/advancing the simulation. Key dependencies include Drake's DiagramBuilder and Simulator.

```python
# TODO: Load the scenario_base_file scenario

# TODO: Create a diagram builder

# TODO: Make a hardware station with the scenario and our meshcat.

# TODO: Add the trajectories to the diagram as TrajectorySources

# TODO: Connect the trajectorie source output ports to the arm and gripper
# position input ports, which you can get as "iiwa.position" and "wsg.position"


# TODO: Uncomment the following lines:
# diagram = builder.Build()
# simulation = Simulator(diagram)

# if running_as_notebook:
#     simulation.set_target_realtime_rate(1.0)

# ctx = simulation.get_mutable_context()
# diagram.ForcedPublish(ctx)

# meshcat.StartRecording()
# simulation.Initialize()
# simulation.AdvanceTo(
#     max(traj_q.end_time(), traj_wsg.end_time()) if running_as_notebook else 0.1
# )
# meshcat.StopRecording()
# meshcat.PublishRecording()

```

--------------------------------

### ICP Local Minima Example

Source: https://github.com/russtedrake/manipulation/blob/master/book/pose/icp.ipynb

Illustrates a scenario where the Iterative Closest Point algorithm can get stuck in a local minimum. This example uses a specific configuration of point clouds that causes ICP to converge to a suboptimal solution, highlighting the importance of understanding its limitations and potential for suboptimal convergence from various initial conditions.

```python
p_Om, p_s, X_O = MakeRectangleModelAndScenePoints(
    num_viewable_points=9,
    yaw_O=0.2,
    p_O=[1, 2],
)
IterativeClosestPoint(p_Om, p_s, X_O);
```

--------------------------------

### Test and Simulate Bimanual IIWA Setup

Source: https://github.com/russtedrake/manipulation/blob/master/book/intro/exercises/04_hardwarestation_and_scenarios.ipynb

These snippets are for testing and simulating the bimanual IIWA14 setup. The first part uses `Grader` to validate the `create_bimanual_IIWA14_with_hardware_station` function. The second part provides instructions for simulating the setup, including creating a context and simulator, setting the real-time rate, and running the simulation for a specified duration, with a reminder to check the Meshcat window.

```python
# Use this to test your implementation if you want!
Grader.grade_output([TestHardwareStationBimanual], [locals()], "results.json")
Grader.print_test_results("results.json")

```

```python
diagram = create_bimanual_IIWA14_with_hardware_station()

```

```python
print(f"Check your Meshcat window ({meshcat.web_url()}) to see the robot simulation!")

# Let us also simulate the simple IIWA14 setup to make sure it works as expected!
# TODO: Create a context for the station
# TODO: Create a simulator
# TODO: Set real-time rate to 1.0
# TODO: Run simulation for 15 seconds

```

--------------------------------

### Setup and Visualize Contact Geometry with Python

Source: https://github.com/russtedrake/manipulation/blob/master/book/clutter/contact_inspector.ipynb

Configures a 2D simulation environment to demonstrate contact physics between two specified shapes. It utilizes Drake's MultibodyPlant, SceneGraph, and Meshcat for visualization. The setup includes defining shapes, adding them to the simulation, and configuring contact visualization parameters.

```python
shapes = {
    "Point": Sphere(0.01),
    "Sphere": Sphere(1.0),
    "Cylinder": Cylinder(1.0, 2.0),
    "Box": Box(1.0, 2.0, 3.0),
    "Capsule": Capsule(1.0, 2.0),
    "Ellipsoid": Ellipsoid(1.0, 2.0, 3.0),
}

def contact_inspector(shape_name_A, shape_name_B):
    builder = DiagramBuilder()

    plant, scene_graph = AddMultibodyPlantSceneGraph(builder, time_step=0.0)
    AddShape(
        plant,
        shapes[shape_name_A],
        "A",
        mass=1,
        mu=1,
        color=[0.9, 0.5, 0.5, 0.5],
    )
    plant.WeldFrames(plant.world_frame(), plant.GetFrameByName("A"))

    AddShape(
        plant,
        shapes[shape_name_B],
        "B",
        mass=1,
        mu=1,
        color=[0.5, 0.5, 0.9, 0.5],
    )
    frame = plant.AddFrame(
        FixedOffsetFrame(
            "planar_joint_frame",
            plant.world_frame(),
            RigidTransform(RotationMatrix.MakeXRotation(np.pi / 2)),
        )
    )
    plant.AddJoint(
        PlanarJoint("B", frame, plant.GetFrameByName("B"), damping=[0, 0, 0])
    )

    plant.Finalize()

    meshcat.Delete()
    meshcat.DeleteAddedControls()
    MeshcatVisualizer.AddToBuilder(builder, scene_graph, meshcat)
    meshcat.Set2dRenderMode(xmin=-3.0, xmax=3.0, ymin=-0.2, ymax=3.0)

    cparams = ContactVisualizerParams()
    cparams.force_threshold = 1e-6
    cparams.newtons_per_meter = 1.0
    cparams.radius = 0.002
    contact_visualizer = ContactVisualizer.AddToBuilder(
        builder, plant, meshcat, cparams
    )

    print_contact_results = builder.AddSystem(PrintContactResults())
    builder.Connect(
        plant.get_contact_results_output_port(),
        print_contact_results.get_input_port(),
    )

    lower_limit = [-3, -3, -np.pi / 2.0]
    upper_limit = [3, 3, np.pi / 2.0]
    q0 = [1.2, 1.2, 0.0]

    default_interactive_timeout = None if running_as_notebook else 1.0
    sliders = builder.AddSystem(
        JointSliders(
            meshcat,
            plant,
            initial_value=q0,
            lower_limit=lower_limit,
            upper_limit=upper_limit,
        )
    )
    diagram = builder.Build()
    sliders.Run(diagram, default_interactive_timeout)
    meshcat.DeleteAddedControls()


contact_inspector("Box", "Sphere")
```

--------------------------------

### Choose Simulation Environment (Python)

Source: https://github.com/russtedrake/manipulation/blob/master/book/trajectories/exercises/5_plan_place_initials.ipynb

Configures the simulation environment by loading a specified scenario file and setting up the necessary components like the station, plant, and scene graph. It establishes communication ports for collision queries and initializes the simulation context. This method prepares the simulation for running path planning algorithms.

```python
def choose_sim(
        self,
        scenario_file: str,
        q_iiwa: tuple | None = None,
        gripper_setpoint: float = 0.1,
    ) -> None:

        self.clear_meshcat()

        self.scenario = LoadScenario(filename=scenario_file)
        builder = DiagramBuilder()
        self.station = builder.AddSystem(
            MakeHardwareStation(self.scenario, meshcat=meshcat)
        )

        self.plant = self.station.GetSubsystemByName("plant")

        self.scene_graph = self.station.GetSubsystemByName("scene_graph")

        # scene graph query output port.
        self.query_output_port = self.scene_graph.GetOutputPort("query")

        self.diagram = builder.Build()

        # contexts
        self.context_diagram = self.diagram.CreateDefaultContext()
        self.context_station = self.diagram.GetSubsystemContext(
            self.station, self.context_diagram
        )
        self.station.GetInputPort("iiwa.position").FixValue(
            self.context_station, np.zeros(7)
        )
        self.station.GetInputPort("wsg.position").FixValue(self.context_station, [0.1])
        self.context_scene_graph = self.station.GetSubsystemContext(
            self.scene_graph, self.context_station
        )
        self.context_plant = self.station.GetMutableSubsystemContext(
            self.plant, self.context_station
        )

        # mark initial configuration
        self.gripper_setpoint = gripper_setpoint
        if q_iiwa is None:
            self.q0 = self.plant.GetPositions(
                self.context_plant, self.plant.GetModelInstanceByName("iiwa")
            )
        else:
            self.q0 = q_iiwa
            self.SetStationConfiguration(q_iiwa, gripper_setpoint)

        self.DrawStation(self.q0, 0.1)
        query_object = self.query_output_port.Eval(self.context_scene_graph)
        self.okay_collisions = len(query_object.ComputePointPairPenetration())
```

--------------------------------

### Setup and Run Robot Simulation

Source: https://github.com/russtedrake/manipulation/blob/master/book/clutter/exercises/6_grasp_initials.ipynb

This code sets up the Drake simulator, initializes its context, and sets the initial integral value for the robot's position. It then advances the simulation to the end time of the generated trajectory and records the process for visualization.

```python
# Define the simulator.
simulator = Simulator(diagram)
context = simulator.get_mutable_context()
station_context = station.GetMyContextFromRoot(context)
integrator.set_integral_value(
    integrator.GetMyContextFromRoot(context),
    plant.GetPositions(
        plant.GetMyContextFromRoot(context),
        plant.GetModelInstanceByName("iiwa"),
    ),
)
diagram.ForcedPublish(context)
print(f"sanity check, simulation will run for {traj_V_G.end_time()} seconds")

# run simulation!
meshcat.StartRecording()
if running_as_notebook:
    simulator.set_target_realtime_rate(1.0)
simulator.AdvanceTo(traj_V_G.end_time())
meshcat.StopRecording()
meshcat.PublishRecording()

```

--------------------------------

### Import Drake Libraries (Python)

Source: https://github.com/russtedrake/manipulation/blob/master/book/robot/gymnasium_robotics.ipynb

Imports necessary libraries from Drake and auxiliary manipulation modules for robot control and simulation. Assumes Drake and manipulation packages are installed.

```python
import numpy as np
from pydrake.all import PackageMap, Parser, RobotDiagramBuilder, Simulator, StartMeshcat

from manipulation import running_as_notebook
from manipulation.make_drake_compatible_model import MakeDrakeCompatibleModel
from manipulation.remotes import AddGymnasiumRobotics
from manipulation.utils import ApplyDefaultVisualization
```

--------------------------------

### Link to Notebook Example (JavaScript)

Source: https://github.com/russtedrake/manipulation/blob/master/book/force.html

Generates a link to a specific notebook that demonstrates force control principles. This is used to provide users with runnable examples and further exploration of the concepts discussed.

```javascript
function notebook_link(name, notebook) {
  // Implementation to generate a link to the specified notebook
  return "<a href=\"" + notebook + ".ipynb\">" + name + " notebook</a>"; // Example implementation
}

document.write(notebook_link('force', notebook = "point_finger"));
```

--------------------------------

### Start Meshcat for Visualization

Source: https://github.com/russtedrake/manipulation/blob/master/book/intro/exercises/04_hardwarestation_and_scenarios.ipynb

Initializes the Meshcat visualizer, which is essential for observing the 3D robot simulations. This code snippet starts the Meshcat server and provides a link for the user to open the visualization in their browser.

```python
# Start meshcat for visualization
meshcat = StartMeshcat()
print("Click the link above to open Meshcat in your browser!")

```

--------------------------------

### Import Drake Libraries (Python)

Source: https://github.com/russtedrake/manipulation/blob/master/book/clutter/contact_wrench.ipynb

Imports necessary modules from the Drake library and NumPy for general-purpose numerical operations. This setup is common for Drake-based robotics simulations and manipulations.

```python
import numpy as np
from IPython.display import clear_output
from pydrake.all import (
    AbstractValue,
    AddMultibodyPlantSceneGraph,
    Box,
    ContactModel,
    ContactResults,
    ContactVisualizer,
    ContactVisualizerParams,
    DiagramBuilder,
    FixedOffsetFrame,
    JointSliders,
    LeafSystem,
    MeshcatCone,
    MeshcatVisualizer,
    PointCloud,
    PrismaticJoint,
    Rgba,
    RigidTransform,
    RotationMatrix,
    SpatialInertia,
    Sphere,
    StartMeshcat,
    UnitInertia,
    VectorToSkewSymmetric,
)

from manipulation import running_as_notebook
from manipulation.scenarios import AddShape

```

--------------------------------

### Import Drake MultibodyPlant and Parser (Python)

Source: https://github.com/russtedrake/manipulation/blob/master/book/pick/qdot_vs_v.ipynb

Imports necessary components from the pydrake library. This is a common starting point for using Drake's simulation and modeling capabilities.

```python
from pydrake.all import MultibodyPlant, Parser
```

--------------------------------

### Initialize Meshcat Visualizer and Temporary Directory (Python)

Source: https://github.com/russtedrake/manipulation/blob/master/book/mobile/scene_synthesizer.ipynb

Initializes the Meshcat visualizer for 3D rendering and creates a temporary directory for storing generated files. This setup is common for visualization and intermediate file storage.

```python
# Start the visualizer.
meshcat = StartMeshcat()
tmpdir = tempfile.TemporaryDirectory()
```

--------------------------------

### Python: Apply Shortcutting to Example Paths

Source: https://github.com/russtedrake/manipulation/blob/master/book/trajectories/exercises/5_plan_place_initials.ipynb

Applies the `shortcut_path` function to pre-defined paths for picking, placing, and resetting actions within a simulation environment. This demonstrates the practical use of the smoothing algorithm.

```python
sim.choose_sim(scenario_base_file, q_iiwa=q_initial)
sim.choose_sim(scenario_base_file, q_iiwa=q_initial)
short_path_pick = shortcut_path(sim, path_pick, passes=200, min_separation=2)

sim.choose_sim(scenario_grasp_file, q_iiwa=q_approach)
short_path_place = shortcut_path(sim, path_place, passes=200, min_separation=2)

sim.choose_sim(scenario_base_file, q_iiwa=q_goal)
short_path_reset = shortcut_path(sim, path_reset, passes=200, min_separation=2)
```

--------------------------------

### Setup Multibody Plant and Scene Graph for Manipulation

Source: https://github.com/russtedrake/manipulation/blob/master/book/force/point_finger.ipynb

Initializes the Drake simulation environment, including the MultibodyPlant, SceneGraph, and adds planar objects like a bin and a point finger. It configures contact approximations and visualizers.

```python
builder = DiagramBuilder()

plant, scene_graph = AddMultibodyPlantSceneGraph(builder, time_step=0.005)
plant.set_discrete_contact_approximation(DiscreteContactApproximation.kSap)
box = AddPlanarBinAndCracker(plant)
finger = AddPointFinger(plant)
plant.GetJointByName("finger_x").set_default_translation(0.15)
plant.GetJointByName("finger_z").set_default_translation(0.025)
plant.Finalize()

vis = MeshcatVisualizer.AddToBuilder(builder, scene_graph, meshcat)
contact_vis = ContactVisualizer.AddToBuilder(
    builder,
    plant,
    meshcat,
    ContactVisualizerParams(radius=0.005, newtons_per_meter=40.0),
)
```

--------------------------------

### Initialize Manipulation Station Simulation (Python)

Source: https://github.com/russtedrake/manipulation/blob/master/book/trajectories/exercises/5_plan_place_initials.ipynb

Initializes the `ManipulationStationSim` class, setting up the simulation environment. It loads a scenario file, configures the robot's initial state (joint positions and gripper setpoint), and performs an initial collision check. Dependencies include libraries for robotics simulation (e.g., Drake) and visualization (e.g., Meshcat).

```python
class ManipulationStationSim:
    def __init__(
        self,
        scenario_file: str | None = None,
        q_iiwa: tuple | None = None,
        gripper_setpoint: float = 0.1,
    ) -> None:

        self.scenario = None
        self.station = None
        self.plant = None
        self.scene_graph = None
        self.query_output_port = None
        self.diagram = None

        # contexts
        self.context_diagram = None
        self.context_station = None
        self.context_scene_graph = None
        self.context_plant = None

        # mark initial configuration
        self.q0 = None

        self.okay_collisions = None
        self.gripper_setpoint = gripper_setpoint

        if scenario_file is not None:
            self.choose_sim(scenario_file, q_iiwa, gripper_setpoint)
```

--------------------------------

### Plot Environment with Start/Goal and Obstacles - Python

Source: https://github.com/russtedrake/manipulation/blob/master/book/trajectories/gcs_simple_2d_cspace.ipynb

Renders the environment with safety regions, start and goal points, and optionally obstacles. This snippet is used for visualizing the initial state and constraints before trajectory planning.

```python
from pydrake.geometry import Point

# Assuming 'obstacles' is defined elsewhere
# environment_setup()
# 
# for O in obstacles:
#     plt.fill(*O.T, fc="lightcoral", ec="k", zorder=4)
# 
# plt.plot(*x_start, "kx")
# plt.plot(*x_goal, "kx")
# 
# plt.text(0.2, 0.35, "$q_0$", ha="center", va="bottom")
# plt.text(4.8, 4.65, "$q_T$", ha="center", va="top");
```

--------------------------------

### Import Drake Libraries and Initialize Meshcat (Python)

Source: https://github.com/russtedrake/manipulation/blob/master/book/trajectories/gcs_simple_2d_cspace.ipynb

This snippet imports various modules from the Drake library, including visualization, optimization, and planning tools. It also initializes Meshcat, a web-based visualization tool, for use in the current environment. This setup is crucial for visualizing robotic systems and planning trajectories.

```python
import mpld3
import numpy as np
from matplotlib import pyplot as plt
from pydrake.all import (
    AddDefaultVisualization,
    ConnectPlanarSceneGraphVisualizer,
    IrisFromCliqueCoverOptions,
    IrisInConfigurationSpaceFromCliqueCover,
    RandomGenerator,
    RigidTransform,
    RobotDiagramBuilder,
    SceneGraphCollisionChecker,
    StartMeshcat,
    VPolytope,
)
from pydrake.geometry.optimization import GraphOfConvexSetsOptions, HPolyhedron, Point
from pydrake.planning import GcsTrajectoryOptimization
from scipy.spatial import ConvexHull

from manipulation import running_as_notebook
from manipulation.utils import ConfigureParser

if running_as_notebook:
    mpld3.enable_notebook()

meshcat = StartMeshcat()

```

--------------------------------

### Simulate Diagram with Meshcat Visualization and Recording

Source: https://github.com/russtedrake/manipulation/blob/master/book/robot/simulation.ipynb

Sets up a simulator for the diagram, configures real-time rate, advances the simulation, and enables Meshcat recording for animation playback. This function encapsulates the entire simulation and visualization setup.

```python
def animation_demo():
    builder = DiagramBuilder()

    # Adds both MultibodyPlant and the SceneGraph, and wires them together.
    plant, scene_graph = AddMultibodyPlantSceneGraph(builder, time_step=1e-4)
    # Note that we parse into both the plant and the scene_graph here.
    Parser(plant, scene_graph).AddModelsFromUrl(
        "package://drake_models/iiwa_description/sdf/iiwa14_no_collision.sdf"
    )
    plant.WeldFrames(plant.world_frame(), plant.GetFrameByName("iiwa_link_0"))
    plant.Finalize()

    # Adds the MeshcatVisualizer and wires it to the SceneGraph.
    MeshcatVisualizer.AddToBuilder(builder, scene_graph, meshcat)

    diagram = builder.Build()
    context = diagram.CreateDefaultContext()
    plant_context = plant.GetMyMutableContextFromRoot(context)
    plant.SetPositions(plant_context, [-1.57, 0.1, 0, -1.2, 0, 1.6, 0])
    plant.get_actuation_input_port().FixValue(plant_context, np.zeros(7))

    simulator = Simulator(diagram, context)
    simulator.set_target_realtime_rate(1.0)

    meshcat.StartRecording()
    simulator.AdvanceTo(5.0 if running_as_notebook else 0.1)
    meshcat.StopRecording()
    meshcat.PublishRecording()


animation_demo()
```

--------------------------------

### Gamepad Teleoperation Setup (Python)

Source: https://github.com/russtedrake/manipulation/blob/master/book/intro/intro.ipynb

Sets up a gamepad teleoperation system for robot control. It initializes systems, loads a scenario, configures a hardware station with an IIWA arm and WSG gripper, and connects gamepad inputs to robot control ports. Dependencies include `DiagramBuilder`, `LoadScenario`, `MakeHardwareStation`, `GamepadDiffIK`, and `Simulator`.

```python
def gamepad_teleop():
    if meshcat.GetGamepad().index == None:
        print(
            "Press a button on the gamepad with the meshcat window in focus to activate gamepad support (then run this cell again)."
        )
        return

    builder = DiagramBuilder()

    scenario = LoadScenario(data=scenario_data)
    station = builder.AddSystem(MakeHardwareStation(scenario, meshcat))
    meshcat.ResetRenderMode()
    meshcat.DeleteAddedControls()

    diff_ik_plant = MakeMultibodyPlant(scenario, model_instance_names=["iiwa"])
    frame = diff_ik_plant.GetFrameByName("iiwa_link_7")
    gamepad = builder.AddSystem(GamepadDiffIK(meshcat, diff_ik_plant, frame))
    builder.Connect(
        gamepad.GetOutputPort("iiwa.position"),
        station.GetInputPort("iiwa.position"),
    )
    builder.Connect(
        gamepad.GetOutputPort("wsg.position"),
        station.GetInputPort("wsg.position"),
    )
    builder.Connect(
        station.GetOutputPort("iiwa.state_estimated"),
        gamepad.GetInputPort("robot_state"),
    )
    builder.AddSystem(StopButton(meshcat))

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

    simulator.set_target_realtime_rate(1.0)
    simulator.AdvanceTo(np.inf)


gamepad_teleop()
```

--------------------------------

### Integrator and Position Control Setup (Python)

Source: https://github.com/russtedrake/manipulation/blob/master/book/pick/pick.ipynb

Connects an integrator to the controller's output and then to the robot's position input. It also feeds the measured robot position back to the controller, establishing a closed-loop control system.

```python
integrator = builder.AddSystem(Integrator(7))
integrator.set_name("integrator")
builder.Connect(controller.get_output_port(), integrator.get_input_port())
builder.Connect(integrator.get_output_port(), station.GetInputPort("iiwa.position"))
builder.Connect(
    station.GetOutputPort("iiwa.position_measured"),
    controller.GetInputPort("iiwa.position"),
)
```

--------------------------------

### Build and Connect Manipulation System

Source: https://github.com/russtedrake/manipulation/blob/master/book/clutter/clutter_clearing.ipynb

This snippet demonstrates building a system using a DiagramBuilder, connecting output ports of a planner to input ports of a switch, and connecting the switch's output to a station's input. It also shows adding a StopButton system and building the final diagram.

```python
builder.Connect(
        planner.GetOutputPort("iiwa_position_command"),
        switch.DeclareInputPort("position"),
    )
    builder.Connect(switch.get_output_port(), station.GetInputPort("iiwa.position"))
    builder.Connect(
        planner.GetOutputPort("control_mode"),
        switch.get_port_selector_input_port(),
    )

    builder.AddSystem(StopButton(meshcat))

    diagram = builder.Build()
```