如何运行自定义滑块

Question

我正在使用分段常数曲率（PCC）假设对软机器人进行模拟，并使用增强的刚体模型（ARBM）代表每个PCC段。为此，我首先想实现一个用于曲率控制的手动滑块，即，一个滑块，我可以在其中输入两个曲率参数（theta和phi），然后通过一些已知的映射m（theta，theta， phi）在meshcat中展示机器人。

我已经在显示ARBM，但是，正在努力使滑块运行。结果，我想拥有类似于让您从操纵课程中获得机器人。因此，理想情况下，一个运动学模拟可以在其中设置曲线以显示所得的ARBM配置。

截至目前，我的方法是：

• 创建一个基于叶片系统的自定义滑块类，该类别创建两个幻灯片创建
• 基于应用映射m（Theta，phi）的向量系统创建转换系统对于滑块的输出，产生其输出上ARBM关节状态的输出
• 将机器人关节设置为该状态（这是我在努力）的

问题。问题似乎是我无法直接连接所需的输出以与关节滑块相同的方式到达关节位置。有没有办法做到这一点，还是我应该遵循其他方法？

有关各个实例的代码，请参见下文：

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

这里有几个活动部件。首先，您说的是“运动学模拟”，但是“让我们让您的机器人”中的演示不会模拟（物理学），它只会可视化滑块设置的运动学。假设这足以满足您的目标，则可以将回调传回您的sliders.run method（就像我在笔记本中所做的那样，对应于本章），我相信，如果您在该回调中调用plant.setpositions，它应该有效吗？

Answer 2

最终，我使用interslider.lun（...）的重新完成自定义滑块。对我来说，以下是足够的（两个平面恒定曲率段，每个曲线段，每个曲线为4个刚性接头）：

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)

然后只需使用构建器添加它们并调用Run（）函数即可。