How to run a custom Slider

Question

I'm working on a simulation of a Soft Robot using the Piecewise Constant Curvature (PCC) assumption and representing each PCC segment with an Augmented Rigid Body Model (ARBM). For this I first would like to implement a manual slider for curvature control, i.e., a slider where I can input the two curvature parameters (Theta and Phi) and after mapping it to the ARBM via some known map m(Theta, Phi) showcase the robot in meshcat.

I'm already displaying the ARBM, however, am struggling to get the slider to run. As a result, I'd like to have something similar to the Let's get you a Robot notebook from the Manipulation course. So ideally a kinematic simulation in which I can set the curvatures to show the resulting ARBM configuration.

As of right now, my approach is the following:

• Create a Custom Slider Class that is based on LeafSystem that creates two slides
• Create a Transformation System based on a VectorSystem that applies the map m(Theta, Phi) to the output of the sliders yielding the ARBM joint states on its output
• Set the Robot Joints to this State (This is where I'm struggling)

The problem seems to be that I cannot connect the desired output directly to the Joint positions in the same way that, e.g., the Joint sliders are. Is there a way to do this or should I follow a different approach?

See below for the code of the individual instances:

CurvatureSlider.py:

from dataclasses import dataclass
import numpy as np

from pydrake.systems.framework import LeafSystem


class CurvatureSliders(LeafSystem):
    @dataclass
    class SliderDefault:
        """Default values for the meshcat sliders."""
        name: str
        """The name that is used to add / query values from."""
        default: float
        """The initial value of the slider."""

    _THETA = SliderDefault("Theta", 0.0)
    _PHI = SliderDefault("Phi", 0.0)

    def __init__(self, meshcat):
        """
        @param meshcat The already created pydrake.geometry.Meshcat instance.
        """

        LeafSystem.__init__(self)
        self.DeclareVectorOutputPort("theta_phi", 2, self.DoCalcOutput)
        self.meshcat = meshcat

        # Curvature Control Sliders
        self.meshcat.AddSlider(
            name=self._THETA.name, min=-2.0 * np.pi,
            max=2.0 * np.pi, step=0.01,
            value=self._THETA.default)
        self.meshcat.AddSlider(
            name=self._PHI.name, min=-2.0 * np.pi,
            max=2.0 * np.pi, step=0.01,
            value=self._PHI.default)

    def SetConfiguration(self, config: tuple):
        """
        @param pose Tuple of floats that is ordered (Theta, Phi)
        """
        assert(len(config) == 2)
        self.meshcat.SetSliderValue(self._THETA.name, config[0])
        self.meshcat.SetSliderValue(self._PHI.name, config[1])

    def DoCalcOutput(self, context, output):
        theta = self.meshcat.GetSliderValue(self._THETA.name)
        phi = self.meshcat.GetSliderValue(self._PHI.name)
        output.SetAtIndex(0, theta)
        output.SetAtIndex(1, phi)

CC2ARBM.py:

import numpy as np

from pydrake.systems.framework import VectorSystem


class CC2ARBM(VectorSystem):

    def __init__(self, L: float):
        """
        @param L The length of the segment.
        """
        VectorSystem.__init__(self, 2, 10)

        # Define length of the segment
        self._L = L

    def DoCalcVectorOutput(self, context, u, x, y):
        # Extract Input
        theta = u[0]
        phi = u[1]

        # Compute ARBM equivalent configuration
        b = 0.5 * self._L
        eta = 0
        if not theta == 0:
            b = self._L / theta * np.sqrt(1
                                          + 4 * np.sin(0.5 * theta) / theta
                                          * (np.sin(0.5 * theta) / theta)
                                          - np.cos(0.5 * theta))
            eta = np.arccos(1 / b * self._L / theta * np.sin(0.5 * theta))

        print("Computed ARBM Joint Position")
        # Aggreate Output
        y.SetAtIndex(0, phi)
        y.SetAtIndex(1, 0.5 * theta - eta)
        y.SetAtIndex(2, b)
        y.SetAtIndex(3, eta)
        y.SetAtIndex(4, - phi)
        y.SetAtIndex(5, phi)
        y.SetAtIndex(6, eta)
        y.SetAtIndex(7, b)
        y.SetAtIndex(8, 0.5 * theta - eta)
        y.SetAtIndex(9, - phi)

Main.py:

import sys
import time
import matplotlib.pyplot as plt
import numpy as np

from CurvatureSliders import CurvatureSliders
from CC2ARBM import CC2ARBM

from pydrake.geometry import (
    MeshcatVisualizerCpp,
    MeshcatVisualizerParams,
    Role,
    StartMeshcat
)

from pydrake.math import (
    RigidTransform,
    RotationMatrix
)

from pydrake.multibody.meshcat import JointSliders
from pydrake.multibody.tree import FixedOffsetFrame
from pydrake.multibody.parsing import Parser
from pydrake.multibody.plant import AddMultibodyPlantSceneGraph

from pydrake.systems.analysis import Simulator
from pydrake.systems.framework import DiagramBuilder
from pydrake.systems.primitives import LogVectorOutput


def do_main(teleop: bool,
            target_realtime_rate: float,
            simulation_time: float,
            max_time_step: float,
            description_path: str) -> None:

    # Start the visualizer.
    # The cell will output an HTTP link after the execution.
    # Click the link and a MeshCat tab should appear in your browser.
    meshcat = StartMeshcat()

    # Reset Meshcat Simulator
    meshcat.Delete()
    meshcat.DeleteAddedControls()

    # Init the Diagram Builder
    builder = DiagramBuilder()

    # Note: the time_step here is chosen arbitrarily.
    plant, scene_graph = AddMultibodyPlantSceneGraph(
        builder, time_step=max_time_step)

    # Load the files into the plant/scene_graph.
    parser = Parser(plant)
    # L - Mount
    mount = parser.AddModelFromFile("../mount.sdf")
    # Robot
    model = parser.AddModelFromFile(description_path)

    # Create an offset frame located half the link height above the origin
    base_frame = plant.GetFrameByName("mount_base")
    offset_frame = plant.AddFrame(
        frame=FixedOffsetFrame(
            name="offset_frame",
            P=plant.world_frame(),
            X_PF=RigidTransform(
                R=RotationMatrix.Identity(),
                p=np.array([0, 0, 0.5])
            ),
            model_instance=None)
    )
    # Weld the base link to the offset frame
    plant.WeldFrames(offset_frame, base_frame)

    # Weld the robot base to the L-Mount
    robot_base_frame = plant.GetFrameByName("robot_base")
    robot_mounting_frame = plant.GetFrameByName("robot_location")
    plant.WeldFrames(robot_mounting_frame, robot_base_frame)

    # Finalize Plant
    plant.Finalize()

    #############################################################
    #              Post Plant Finalization Code                 #
    #############################################################

    # Set the Default states of the Joints
    for i in plant.GetJointIndices(model):
        j = plant.get_joint(i)
        if j.num_positions() == 1:
            j.set_default_positions([-0.2])

    # Make the control inputs of the model an input to the diagram.
    builder.ExportInput(plant.get_actuation_input_port())

    # Add two visualizers, one to publish the "visual" geometry, and one to
    # publish the "collision" geometry.
    visual = MeshcatVisualizerCpp.AddToBuilder(
        builder, scene_graph, meshcat,
        MeshcatVisualizerParams(
            role=Role.kPerception, prefix="visual")
    )
    collision = MeshcatVisualizerCpp.AddToBuilder(
        builder, scene_graph, meshcat,
        MeshcatVisualizerParams(role=Role.kProximity, prefix="collision"))
    # Disable the collision geometry at the start; it can be enabled by the
    # checkbox in the meshcat controls.
    meshcat.SetProperty("collision", "visible", False)

    # Setup Loggers
    plant_logger = LogVectorOutput(plant.get_state_output_port(), builder)

   
    # Joint Sliders Work like this
    # sliders = builder.AddSystem(JointSliders(meshcat, plant))

    # Add Curvature Sliders
    curv_sliders = builder.AddSystem(CurvatureSliders(meshcat))
    cc2arbm = builder.AddSystem(CC2ARBM(0.2))

    # Connect the Sliders to the transformation block
    builder.Connect(curv_sliders.get_output_port(0),
                    cc2arbm.get_input_port(0))

    # Build the diagram
    diagram = builder.Build()

    # Start runnin the teleoperation
    # Start Running the Slider similar to the Joint slider 
    # e.g. sliders.Run(diagram)


if __name__ == '__main__':
    if len(sys.argv) <= 1:
        do_main(target_realtime_rate=1,
                simulation_time=10.0,
                max_time_step=1.0e-3,
                description_path="single_cc_segment.sdf")
    else:
        do_main(target_realtime_rate=float(sys.argv[1]),
                simulation_time=float(sys.argv[2]),
                max_time_step=float(sys.argv[3]),
                description_path=sys.argv[4])

Answer 1

There are a few moving parts here. First, you say "kinematic simulation", but the demonstration in "Let's get you a robot" does not simulate (physics), it only visualizes the kinematics as set by the sliders. Assuming that is sufficient for your goal, then you could pass a callback into your sliders.Run method (as I do in the notebook corresponding to this chapter), and I believe that if you call plant.SetPositions in that callback, it should work?

Answer 2

I eventually ran the custom slider using a reimplementation of JointSlider.Run(...). For me, the following was sufficient (Two planar constant curvature segments, represented by 4 rigid joints each):

from dataclasses import dataclass
import numpy as np
import functools
import operator
import logging
import time
from typing import List, Tuple
from pydrake.systems.framework import LeafSystem


class CurvatureSliders(LeafSystem):
    @dataclass
    class SliderDefault:
        """Default values for the meshcat sliders."""
        name: str
        """The name that is used to add / query values from."""
        default: float
        """The initial value of the slider."""

    _Q1 = SliderDefault("q1", 0.0)
    _Q2 = SliderDefault("q2", 0.0)

    def __init__(self, meshcat, plant, L=0.2):
        """
        @param meshcat The already created pydrake.geometry.Meshcat instance.
        @param plant The plant the sliders are connected to
        @param L the restlength of the segment
        """

        # Call Super Constructor
        LeafSystem.__init__(self)

        # Declare System Output
        output = self.DeclareVectorOutputPort(
            "q1_q2", 2, self.DoCalcOutput)
        output.disable_caching_by_default()

        # Init Class Variables
        self._meshcat = meshcat
        self._plant = plant
        self._L = L

        # Curvature Control Sliders
        self._meshcat.AddSlider(
            name=self._Q1.name, min=-2.0 * np.pi,
            max=2.0 * np.pi, step=0.1,
            value=self._Q1.default)
        self._meshcat.AddSlider(
            name=self._Q2.name, min=-2.0 * np.pi,
            max=2.0 * np.pi, step=0.1,
            value=self._Q2.default)

    def SetConfiguration(self, q: Tuple):
        """
        @param q configuration for each CC segment descriped by (q1, q2)
        """
        self._meshcat.SetSliderValue(self._Q1.name, q[0])
        self._meshcat.SetSliderValue(self._Q2.name, q[1])

    def CC2AGRB(self, q: Tuple) -> List[float]:

        # Extract input
        q1 = q[0]
        q2 = q[1]
        # Compute ARBM equivalent configuration
        config1 = [0, 0.5 * self._L, 0.5 * self._L, 0]
        config2 = [0, 0.5 * self._L, 0.5 * self._L, 0]

        if q1 != 0:
            config1 = [
                0.5 * q1,
                self._L * np.sin(0.5 * q1) / q1,
                self._L * np.sin(0.5 * q1) / q1,
                0.5 * q1
            ]
        if q2 != 0:
            config2 = [
                0.5 * q2,
                self._L * np.sin(0.5 * q2) / q2,
                self._L * np.sin(0.5 * q2) / q2,
                0.5 * q2
            ]

        return functools.reduce(operator.iconcat, [config1, config2], [])

    def DoCalcOutput(self, context, output):
        q1 = self._meshcat.GetSliderValue(self._Q1.name)
        q2 = self._meshcat.GetSliderValue(self._Q2.name)

        output.SetAtIndex(0, q1)
        output.SetAtIndex(1, q2)

    def Run(self, diagram, timeout=1e5):

        # Get all the contextes
        root_context = diagram.CreateDefaultContext()
        sliders_context = self.GetMyContextFromRoot(root_context)
        plant_context = self._plant.GetMyMutableContextFromRoot(root_context)

        # Add Stop Button
        kButtonName = "Stop Curvature Sliders"
        logging.info("Press the '{}' button in Meshcat to continue.",
                     kButtonName)
        self._meshcat.AddButton(kButtonName)

        # Greb current time to implement the timeout
        t0 = time.time()

        # Loop until the button is clicked, or
        # the timeout (when given) is reached.
        diagram.Publish(root_context)
        while self._meshcat.GetButtonClicks(kButtonName) < 1:
            # Break out of loop if timeout elapsed
            elapsed_t = time.time() - t0
            if elapsed_t >= timeout:
                break

            # If the sliders have not changed, avoid invalidating the context.
            old_positions = self._plant.GetPositions(plant_context)
            new_positions = self.CC2AGRB(
                self.get_output_port().Eval(sliders_context))
            if (np.abs(new_positions - old_positions) < 0.001).all():
                time.sleep(0.01)
                continue

            # Publish the new positions.
            self._plant.SetPositions(plant_context, new_positions)
            diagram.Publish(root_context)

And then just add them using the builder and calling the Run() function.