How to run a custom Slider - drake

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.