plant.SetPositions does not update visualization

user_17326484 0
Viewed 88

My code (using Russ Tedrake's manipulation module to make shapes): https://gist.github.com/richardrl/b71a1b6ed9e30dfc9ac252cc99193634

The logic is in line 147 is completely ignored with respect to the visualizer. No matter what positions I set it, the visualization is the same.

Video here: https://www.dropbox.com/s/86werg0fxmbqebs/ball_horizontal_line.mp4?dl=0

The spheres pop out of the origin position (which they start off in from AddShape) and move outwards in a horizontal line, regardless of what configuration I set in line 147.

I am not positive whether this is a problem of, the visualization is disconnected from my plant, or something else. But what I do know is my code is setting the plant positions properly (console outputs):

ln145 before setting positions
[ 1.  0.  0.  0.  0.  0.  0.  1.  0.  0.  0.  0.  0.  0.  1.  0.  0.  0.
  0.  0.  0.]
ln156 after setting positions
[ 0.          0.          0.3         0.          0.          0.          0.
  0.6         0.          0.3         0.          0.          0.          0.
  0.3         0.          0.81961524  0.          0.          0.          0.        ]
ln163 after simulation
[ -2.48258262e-04   0.00000000e+00   2.99999898e-01   0.00000000e+00
   9.32950273e-04   0.00000000e+00   2.98500000e-01  -2.25902431e-01
   0.00000000e+00  -6.31659664e-01   0.00000000e+00  -1.30262016e+00
   0.00000000e+00   2.98500000e-01  -7.89982679e-01   0.00000000e+00
  -3.71132816e-01   0.00000000e+00   1.28836582e+00   0.00000000e+00
   2.98500000e-01]

EDIT: Here is a self-contained file that reproduces the error: https://gist.github.com/richardrl/6013299b63100c585851ed1803406cfa

drake
1 Answers

Here's what jumps out at me.

I believe you've got a fundamental problem. It looks like AddShape returns a model instance index and not a body index. But you're using it as a body index (more or less). This is causing you to slice into the generalized coordinates badly. So, when you think you're setting position, you're actually writing to the quaternion.

For example, in STACKED_TOUCHING touching configuration, sphere 1's position should be at [0, 0, 0.3]. And you see the first three state variables have been come [0, 0, 0.3], however those three values are the first three values of what used to be the unit quaternion [1, 0, 0, 0].

Generally, trying to directly interpret state can be a dangerous proposition. So, rather than trying to manipulate it, I'd recommend using the API where the actions you're taking are more easily interpretiable:

for idx, sphere in enumerate(spheres):
    # Note: we're ignoring the model instance index entirely and
    # simply acquiring bodies based on their *globally* unique body names.
    sphere_name = f"sphere_{idx}"
    sphere_body = plant.GetBodyByName(sphere_name)
    plant.SetFreeBodyPose(plant_context, sphere_body,
                          RigidTransform(p=guesses, idx))

Some further random notes:

  1. You've got multiple instances of DiagramBuilder and a diagram (lines 49-58 are no-ops because they are 100% replaced with the following definition.
  2. The invocation of scene_graph.Publish(sg_context) on line 160 is redundant; the previous line where you publish the diagram should be sufficient.
  3. Looking at your video, it seems like the spheres "organically" move out of collision (with each other and with the ground"). This makes some sense as they start in total collision (only arbitrarily rotated), so they have to move out of collision. If you look at your final position values. The 7th, 14th, and 21st values are all, basically, 0.3; the spheres have risen up to be radius height above the ground plane.
user_76862560
Related