RobotLocomotion / drake

Model-based design and verification for robotics.
https://drake.mit.edu
Other
3.35k stars 1.27k forks source link

Sliders in meshcat force coarse precision on degrees of freedom #19955

Open DamrongGuoy opened 1 year ago

DamrongGuoy commented 1 year ago

What happened?

Using Blender to create assets for Drake, users specified a pose of a smaller box to align with a corner of a bigger box in Blender; however, when users loaded the SDFormat file into Drake and visualized in Meshcat, the two boxes were off a little.

I was able to simplify and reproduce the problem with this Python code:

prudential_coordinate_test_2 = """<?xml version="1.0"?>
<sdf version="1.7" xmlns:drake="[drake.mit.edu](http://drake.mit.edu/)">
  <model name="prudentail_coordinate_test_2">
    <link name='bottom_cube'>
      <pose>-0.3075 -0.65 -0.05952 0.0 -0.0 0.0</pose>
      <visual name='bottom_cube'>
        <pose>0 0 0 0 0 0</pose>
        <geometry>
          <box>
            <size>0.1 0.1 0.1</size>
          </box>
        </geometry>
        <material>
          <diffuse>0.44 0.134 0.98</diffuse>
        </material>
      </visual>
    </link>
    <link name='table_top'>
      <pose>0.0 0.0 -0.0 0.0 0.0 0.0</pose>
      <visual name='table_top'>
        <pose>0 0 0 0 0 0</pose>
        <geometry>
          <box>
            <size>0.715 1.4 0.01905</size>
          </box>
        </geometry>
        <material>
          <diffuse>0.134 0.56 0.22</diffuse>
        </material>
      </visual>
    </link>
  </model>
</sdf>
"""

from pydrake.geometry import StartMeshcat
meshcat = StartMeshcat()

from pydrake.visualization import ModelVisualizer
visualizer = ModelVisualizer(meshcat=meshcat, visualize_frames=False)
visualizer.parser().AddModelsFromString(prudential_coordinate_test_2, "sdf")
visualizer.Run(loop_once=True)

The smaller box bottom_cube is specified with <pose>-0.3075 -0.65 -0.05952 0.0 -0.0 0.0</pose> and <size>0.1 0.1 0.1</size>, so Meshcat should draw its min x at -0.3075 - 0.1 / 2 = -0.3575.

The bigger box table_top is specified with <pose>0 0 0 0 0 0</pose> and <size>0.715 1.4 0.01905</size>, so Meshcat should draw its min x at -0.715 / 2 = -0.3575, which is the same as the smaller box.

However, instead of being aligned at the corner, they look off a little in Meshcat in the following pictures:

image

image

Thanks to @xuchenhan-tri for volunteering to check it.

CC: @rcory @hongkai-dai @CodySimpson-ctr-TRI

Version

No response

What operating system are you using?

Ubuntu 22.04

What installation option are you using?

compiled from source code using Bazel

Relevant log output

No response

xuchenhan-tri commented 1 year ago

@joemasterjohn tracked down the cause of this. Thanks Joe! This is purely a visualizer issue. The positions parsed from sdformat are not corrupted. In ModelVisualizer, the position/pose of a body is fed by JointSlider. JointSlider defaults to a 1cm step, so anything higher resolution can't be resolved.

IMO, here are the steps towards resolving this issue:

  1. We should not temper with the prescribed value in sdformat in the visualizer. That probably means that the initial/default value in the joint slider should faithfully reflect that in the plant.
  2. The default step size should probably be smaller. I wouldn't be surprised if people care about precision at mm or sub-mm level.

cc @trowell-tri for more insights.

jwnimmer-tri commented 1 year ago

That probably means that the initial/default value in the joint slider should faithfully reflect that in the plant.

The problem is that the slider cannot denote that value. The floating point number in the slider display on the screen will be always be rounded to the nearest step. I don't think we should lie to the user by showing one number on the screen, while posturing the MbP using a different number.

On the other hand, it is quite unfortunate that the default posture isn't shown precisely upon boot-up.

I wonder how difficult it would be to teach Meshcat::AddSlider to offer a detent at the initial value, so that it can always represent that value.

SeanCurtis-TRI commented 1 year ago

Probably worth changing the title of this issue. Given the investigation, the original speculative title can be clarified to reflect the new understanding of the issue. It's not a meshcat issue, per se. For example, if you weren't using model_visualizer, the boxes would be shown in correct positions. It's merely the fact that model_visualizer uses sliders to control the dof that introduces the precision error. Possible title:

Sliders in meshcat force coarse precision on degrees of freedom (or some such thing).

xuchenhan-tri commented 1 year ago

I wonder how difficult it would be to teach Meshcat::AddSlider to offer a detent at the initial value, so that it can always represent that value.

dat.GUI currently doesn't support values other than multiples of the stepsize or the max and the min. https://github.com/dataarts/dat.gui/blob/f720c729deca5d5c79da8464f8a05500d38b140c/src/dat/controllers/NumberController.js#L62

I don't think we should depend on upstream (of meshcat) accommodating off-grid values to resolve this issue. WDYT @jwnimmer-tri?

jwnimmer-tri commented 1 year ago

To me, the question of "how" is not the most important one. If we need to, it would not be that difficult to patch dat.gui. To me, the most important question is what we want the UX to be for our users.

I've asked @trowell-tri to weigh in here.

In the meantime, my thoughts so far are: (1) The slider value displayed on the screen should exactly match the value on the output port. (2) If the user drags the slider away from its initial value, they should still be able to either type in or slide back to the initial value to "reset" it without reloading the model. (3) The boot-up posture of model_visualizer should match the MbP's default context.

If the only defect we want to fix for now is free joints (as in the example above), then I think the simplest possible code fix would be to take our arbitrary [-10, 10] range for the free joint translation sliders, and instead change them to be [initial_value - 10, initial_value + 10] so that the slider is centered around the initial position. That would not work for joints with limits, but it would at least fix this free joint problem.

xuchenhan-tri commented 1 year ago

If the only defect we want to fix for now is free joints (as in the example above), then I think the simplest possible code fix would be to take our arbitrary [-10, 10] range for the free joint translation sliders, and instead change them to be [initial_value - 10, initial_value + 10] so that the slider is centered around the initial position. That would not work for joints with limits, but it would at least fix this free joint problem.

Thanks. That's exactly what I had in mind too.

Re: joint with limits, do you think (3) should still holds even if the default position is outside the joint limits? I can see arguments either way.

jwnimmer-tri commented 1 year ago

My intuition on (3) is that yes we should extend the slider range to admit the initial position, so it can be set to the default value initially. That violates a putative "(4) The sliders should span exactly the joint limits" but that seems like the least important invariant, and we're anyway already breaking it when we invent "10" as a default limit.

xuchenhan-tri commented 1 year ago

Then it seems like we can use the range

[max(min(initial_value, lower_limit), initial_value-10), min(max(initial_value, upper_limit), initial_value+10)]

to handle joints with limits too.