Skip to content

feat(sim-control): expose sim.get_joint_state so callers can read back the pose - #8

Merged
mrgh97 merged 1 commit into
PhyAgentOS:masterfrom
mrgh97:feat/get-joint-state-query
Nov 18, 2026
Merged

mrgh97 merged 1 commit into
PhyAgentOS:masterfrom
mrgh97:feat/get-joint-state-query

Conversation

@mrgh97

@mrgh97 mrgh97 commented Nov 17, 2026

Copy link
Copy Markdown
Contributor

Problem

sim.control exposed get_robot_info (static descriptor) and reset only. Nothing in the node subscribed to mujoco/proprio_state, so there was no way to read back the measured joint state through the Tool API: after an aborted motion, a timeout, or a reset, a caller could not tell where the arm actually is, and had to infer the pose from the next motion or from simulator logs.

Change

Item Detail
New dora input joint_state (mujoco/proprio_state), cached with arrival timestamp in state.py (JointStateCache)
New operation sim.control/get_joint_state (query)
Outputs name, position, velocity, effort, age_ms
Staleness NO_FRESH_ROBOT_STATE when no sample arrived or the newest is older than joint_state_timeout_ms
New config key joint_state_timeout_ms (default 200 ms, matching the 50 Hz publish rate)
Docs TOOL_CONTRACTS.md operations table + operation section, README.md dora wiring

Design notes:

  • the cache is dependency-free and bounded (one sample), so the node stays a light info/reset service;
  • freshness is explicit (age_ms plus a stale error) instead of returning whatever was last seen, because a stale pose read as current is worse than no pose;
  • the operation is a query (no side effects, concurrency 1) so it can be polled between actions.

Tests

packages/.../tests — 74 passed:

  • test_joint_state_query_returns_the_cached_sample (fresh sample + age_ms);
  • test_joint_state_query_rejects_a_stale_sample → NO_FRESH_ROBOT_STATE;
  • test_joint_state_query_without_any_sample_is_not_fresh;
  • test_joint_state_query_flows_through_binding (full envelope round-trip);
  • test_joint_state_input_updates_the_cache (dora input dispatch);
  • test_joint_state_timeout_defaults_and_validation (config parsing, rejects 0/-1/True/"200");
  • descriptor test updated to assert three operations.

Compatibility

  • existing operations and their schemas are unchanged;
  • the node works without the joint_state input wired: the query then answers NO_FRESH_ROBOT_STATE instead of failing at startup;
  • no wire/schema change in forge_msgs.

…k the pose

The endpoint only offered `get_robot_info` (static descriptor) and `reset`. A
caller that aborts a motion, or that wants to verify where the arm actually is,
had no way to read the measured joint state -- the simulator publishes
`JointState` on `mujoco/proprio_state`, but nothing in this node subscribed to
it, so failures were only diagnosable by inferring from the next motion.

* new `joint_state` dora input caches the newest `JointState` (`state.py`,
  `JointStateCache`) with its arrival timestamp;
* new `sim.control/get_joint_state` **query** operation returns `name`,
  `position`, `velocity`, `effort` and `age_ms`;
* the query fails with `NO_FRESH_ROBOT_STATE` when no sample arrived or the
  newest is older than `joint_state_timeout_ms` (new config key, default 200 ms,
  matching the 50 Hz publish rate), so a stale pose is never reported as current;
* `TOOL_CONTRACTS.md` / `README.md` document the new operation and dora port.

Tests: cache freshness (fresh, stale, empty), the query through the endpoint,
the same query through the dora binding, the `joint_state` input path, and
config parsing. 74 tests pass for the package.
@mrgh97
mrgh97 merged commit 05f5ed3 into PhyAgentOS:master Nov 18, 2026
2 of 3 checks passed
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment

Labels

None yet

Projects

None yet

Development

Successfully merging this pull request may close these issues.

1 participant