RSDK-14379: world state store service -> motion service - #6285
RSDK-14379: world state store service -> motion service#6285Devin T. Currie (DTCurrie) wants to merge 6 commits into
Conversation
viambot
added
the
safe to test
Devin T. Currie (DTCurrie)
changed the title
…rld state params with dependent world state from store
viambot
added
safe to test
viambot
added
safe to test
viambot
added
safe to test
…rld-state-store-service-to-motion-service
viambot
added
safe to test
There was a problem hiding this comment.
delete this before merge
There was a problem hiding this comment.
delete this before merge
There was a problem hiding this comment.
delete this before merge
Dan Gottlieb (dgottlieb)
changed the title
| func (ms *builtIn) plan(ctx context.Context, req motion.MoveReq, logger logging.Logger) (motionplan.Plan, error) { | ||
| // Merge any request-supplied world state with the configured store's so a Move avoids both sets of | ||
| // obstacles; conflicts are logged, not fatal. | ||
| storeObstacles, storeTransforms := ms.storeWorldStateParts(ctx) |
There was a problem hiding this comment.
This looks exactly as I hoped!
|
|
||
| // normalizeTransformGeometry fills in an identity geometry center when the store leaves it unset, which | ||
| // the geometry converter requires. It clones so the store's proto is never mutated. | ||
| func normalizeTransformGeometry(tf *commonpb.Transform) *commonpb.Transform { |
There was a problem hiding this comment.
All these other helpers in builtin.go -- we can keep in the motion service package, but please move them to a separate (world_state.go?) file.
I think I actually want these in the referenceframe/worldstate.go. But I also think there's some refactoring to be had. And it's not right for me to ask you to do that here. I don't know exactly what I want without doing some tinkering myself. An end state where we just have some file that I can delete as I move functionality over is all I need to see here.
| return err | ||
| } | ||
|
|
||
| t.Run("obstacle in a known frame plans", func(t *testing.T) { |
There was a problem hiding this comment.
These tests are fine. But the test I really want to see is that adding a huge obstacle via the world state store can make planning fail. That will give me the negative acknowledgment I need that the world state store is being consumed.
|
|
||
| // DepthCameraWorld simulates a depth camera detecting three colored blocks on a table and reporting them | ||
| // into the store, refreshed each capture cycle. No real camera or vision service is involved. | ||
| type DepthCameraWorld struct { |
There was a problem hiding this comment.
Is this and the lidar world stuff just helpers for simulation? Presumably for (sim_test?) testing?
3 participants
What
Wire a configured
world_state_storeservice into the builtin motion service so that aMoveautomatically plans around the store's dynamic obstacles, without the caller re-passing a
WorldStateon every call. Also make the
fakeworld state store an exemplary, self-contained detection simulatorthat a real (module) store can be modeled after.
This is additive and opt-in: when the new motion attribute is unset, behavior is unchanged.
Why
Viam keeps no persistent world representation between motion planning calls, so today an application has
to be the source of truth for the world and re-pass a
WorldStateon everyMove. The world statestore docs already describe the service as an input source for the motion service, but nothing consumed
it. This change implements that consume path for the
Move(cartesian) flow.Static obstacles (configured component geometries) were already auto-included by the planner via the
frame system. The gap this closes is dynamic obstacles, for example objects detected at runtime by a
depth camera or lidar.
What changed
Motion service (
services/motion/builtin/builtin.go):world_state_store_service_name(a single service name).Validatereturns it asa required dependency;
Reconfigureresolves it onto the service.Move, the store is read fresh and its transforms are converted into planner obstacles andsupplemental transforms, then merged with any request-supplied
WorldState(merge, not override).the frame system are warned and skipped, transforms whose name collides with an existing frame are
warned and skipped, and duplicate names or UUIDs across the request and the store are disambiguated so
both are kept.
the store contributed.
World state store fake, made an ideal example (
services/worldstatestore/fake/):depth_cameramode: three colored blocks (red, green, blue) built through the motion-toolsdrawAPIso color metadata and geometry render correctly in the visualizer and participate in collision.
lidarmode: a single, fixed point cloud representing a ring of surface hits around the arm, withper-scan range jitter for realism (it does not spin). The cloud serializes to an octree, so it is a
real collision obstacle. The ring is positioned so the arm starts clear of every surface.
subscribers receive the full current world as ADDED, then every change.
Tests:
(returns arrive as collision-capable octrees), and the fake simulators.
No
go.viam.com/apichanges are required. The motion service consumes the store through the existingListUUIDsandGetTransformRPCs.How to test
A manual harness lives in
_local_wss_test/. See_local_wss_test/README.mdfor the full walkthrough.It configures a
simulatedarm (so moves play out as visible motion rather than snapping), a statictableobstacle under the arm, a fakeworld_state_store, and abuiltinmotion service pointed at it.Setup:
componentsandservicesfrom_local_wss_test/config.jsoninto the machine's raw JSON config.save it outside the repo, for example
~/machine.json.go run ./web/cmd/server -config ~/machine.json.You should see
world state store "worldstate" wired into this motion serviceon startup.viamrobotics/visualization) and connect to the machine, using themachine address and API key from the downloaded config.
Manual check (Move Frame plugin):
arm, confirm the Motion service dropdown isbuiltin, drag the gizmo, and Execute. Moves intoa block or below the table are rejected with a constraint violation; moves to clear space plan normally.
Scripted play-through (good for recording a demo):
With the server and visualizer up, in another terminal run the demo client, which authenticates with
the machine API key and plays a single path, then exits:
-mode depth_camerasweeps the three blocks and refuses to enter them.-mode lidarthreads thepoint-cloud ring and refuses its surfaces. Set the store's
input_sensor_typeto match the mode yourun. The script prints how many transforms the store serves (3 for depth_camera, 1 for lidar) so you
can confirm the match before recording.
Note: the motion service must be named
builtinso the visualizer's default target is the store-awareone, otherwise RDK's auto-created default
builtinshadows it. This is called out in the README.Coordination
This changes the motion planner's input path, so it should be reviewed with the motion planning team
before merge. It is behind an opt-in attribute and default behavior is unchanged when the attribute is
unset.
Screen.Recording.2026-08-03.at.1.52.47.PM.mov
Screen.Recording.2026-08-03.at.1.54.19.PM.mov