Add joint position command

### What change is being made

Add joint position command, e.g. `cmd.joint_pos.arm`
The command provides the list of joints in the metadata.

### Why this change is being made

Support policies which command joint positions (e.g. manipulation)

### Tested

Covered by unit and integration tests.

GitOrigin-RevId: fbd2158274d35d5bcfd930a6ba21c7cfcff521e2

Author: awoll-bdai

control/include/exploy/command_interface.hpp

@@ -144,6 +144,33 @@ class CommandInterface {
     LOG_STREAM(ERROR, "floatValue() not implemented for command: " << command_name);
     return std::nullopt;
   }
+  /**
+   * @brief Initialize data source of a commanded joint position.
+   *
+   * Called once per joint during initialization (usually non real-time).
+   *
+   * @param command_name The name of the command.
+   * @param joint_name The name of the joint.
+   * @return True if initialization succeeded, false otherwise.
+   */
+  virtual bool initJointPosition(const std::string& command_name, const std::string& joint_name) {
+    LOG_STREAM(ERROR, "initJointPosition() not implemented for command: "
+                          << command_name << ", joint: " << joint_name);
+    return false;
+  }
+  /**
+   * @brief Get the commanded position for a single joint.
+   *
+   * @param command_name The name of the command.
+   * @param joint_name The name of the joint.
+   * @return The commanded joint position, or std::nullopt if unavailable.
+   */
+  virtual std::optional<float> jointPosition(const std::string& command_name,
+                                             const std::string& joint_name) const {
+    LOG_STREAM(ERROR, "jointPosition() not implemented for command: " << command_name
+                                                                      << ", joint: " << joint_name);
+    return std::nullopt;
+  }
 };
 
 }  // namespace exploy::control

control/include/exploy/components.hpp

@@ -485,6 +485,34 @@ class CommandBooleanInput : public Input {
   std::string command_name_;  ///< Boolean command name.
 };
 
+/**
+ * @brief Input component that reads commanded joint positions.
+ *
+ * Reads the commanded position for each joint specified in the metadata by calling
+ * CommandInterface::jointPosition() once per joint, then copies the values to the
+ * ONNX input buffer in the order defined by the metadata joint names.
+ */
+class CommandJointPositionInput : public Input {
+ public:
+  /**
+   * @brief Construct a joint position command input component.
+   *
+   * @param key ONNX input tensor name (e.g., "cmd.joint_pos.arm").
+   * @param command_name Name of the joint position command to read.
+   * @param metadata Metadata specifying the ordered list of joint names.
+   */
+  CommandJointPositionInput(const std::string& key, const std::string& command_name,
+                            const metadata::JointPositionCommandMetadata& metadata);
+
+  bool init(RobotStateInterface& state, CommandInterface& command) override;
+  bool read(OnnxRuntime& runtime, RobotStateInterface& state, CommandInterface& command) override;
+
+ private:
+  std::string key_;                                  ///< ONNX input tensor name.
+  std::string command_name_;                         ///< Joint position command name.
+  metadata::JointPositionCommandMetadata metadata_;  ///< Command configuration.
+};
+
 /**
  * @brief Input component that reads a floating-point command value.
  *

control/include/exploy/matcher.hpp

@@ -337,6 +337,19 @@ class CommandFloatMatcher : public Matcher {
   bool matches(const Match& maybe_match) override;
   std::vector<std::unique_ptr<Input>> createInputs() const override;
 };
+
+/**
+ * @brief Matcher for joint position command input tensors.
+ *
+ * Matches patterns like "cmd.joint_pos.{name}" and creates CommandJointPositionInput
+ * components. The joint names are read from the tensor's JSON metadata field
+ * ("joint_names" array).
+ */
+class CommandJointPositionMatcher : public Matcher {
+ public:
+  bool matches(const Match& maybe_match) override;
+  std::vector<std::unique_ptr<Input>> createInputs() const override;
+};
 // ---------------------------------------------------------------
 
 // ---------------  Body matchers --------------------------------

control/include/exploy/metadata.hpp

@@ -265,6 +265,15 @@ struct JointMetadata {
   std::vector<std::string> names{};  ///< Joint names.
 };
 
+/**
+ * @brief Metadata for joint position commands.
+ *
+ * Specifies the ordered list of joint names whose positions are provided by the command.
+ */
+struct JointPositionCommandMetadata {
+  std::vector<std::string> joint_names{};  ///< Ordered joint names.
+};
+
 /**
  * @brief Parse JointMetadata from JSON.
  *
@@ -275,6 +284,16 @@ inline void from_json(const json& j, JointMetadata& jm) {
   j.at("joint_names").get_to(jm.names);
 }
 
+/**
+ * @brief Parse JointPositionCommandMetadata from JSON.
+ *
+ * @param j JSON object containing a "joint_names" array.
+ * @param cmd JointPositionCommandMetadata object to populate.
+ */
+inline void from_json(const json& j, JointPositionCommandMetadata& cmd) {
+  j.at("joint_names").get_to(cmd.joint_names);
+}
+
 /**
  * @brief Parsed version (MAJOR.MINOR).
  */

control/src/components.cpp

@@ -485,6 +485,43 @@ bool CommandBooleanInput::read(OnnxRuntime& runtime, RobotStateInterface& /*stat
   return true;
 }
 
+// Implementation of CommandJointPositionInput methods
+CommandJointPositionInput::CommandJointPositionInput(
+    const std::string& key, const std::string& command_name,
+    const metadata::JointPositionCommandMetadata& metadata)
+    : key_(key), command_name_(command_name), metadata_(metadata) {}
+
+bool CommandJointPositionInput::init(RobotStateInterface& /*state*/, CommandInterface& command) {
+  if (metadata_.joint_names.empty()) {
+    LOG_STREAM(ERROR,
+               "initJointPosition() called with empty joint_names for command: " << command_name_);
+    return false;
+  }
+  for (const auto& joint_name : metadata_.joint_names) {
+    if (!command.initJointPosition(command_name_, joint_name)) return false;
+  }
+  return true;
+}
+
+bool CommandJointPositionInput::read(OnnxRuntime& runtime, RobotStateInterface& /*state*/,
+                                     CommandInterface& command) {
+  auto maybe_buffer = runtime.inputBuffer<float>(key_);
+  if (!maybe_buffer.has_value()) return false;
+  auto buffer = maybe_buffer.value();
+  if (buffer.size() != metadata_.joint_names.size()) {
+    LOG_STREAM(ERROR, "Buffer size " << buffer.size() << " does not match joint_names size "
+                                     << metadata_.joint_names.size()
+                                     << " for command: " << command_name_);
+    return false;
+  }
+  for (std::size_t i = 0; i < metadata_.joint_names.size(); ++i) {
+    auto maybe_pos = command.jointPosition(command_name_, metadata_.joint_names[i]);
+    if (!maybe_pos.has_value()) return false;
+    buffer[i] = maybe_pos.value();
+  }
+  return true;
+}
+
 // Implementation of CommandFloatInput methods
 CommandFloatInput::CommandFloatInput(const std::string& key, const std::string& command_name,
                                      const metadata::FloatCommandMetadata& metadata)

control/src/controller.cpp

@@ -29,6 +29,7 @@ OnnxRLController::OnnxRLController(RobotStateInterface& state, CommandInterface&
   context_.registerMatcher(std::make_unique<CommandSE2VelocityMatcher>());
   context_.registerMatcher(std::make_unique<CommandBooleanMatcher>());
   context_.registerMatcher(std::make_unique<CommandFloatMatcher>());
+  context_.registerMatcher(std::make_unique<CommandJointPositionMatcher>());
   context_.registerMatcher(std::make_unique<StepCountMatcher>());
   // Register all group matchers
   context_.registerGroupMatcher(std::make_unique<JointMatcher>());

control/src/matcher.cpp

@@ -434,6 +434,30 @@ std::vector<std::unique_ptr<Input>> CommandFloatMatcher::createInputs() const {
   return inputs;
 }
 
+bool CommandJointPositionMatcher::matches(const Match& maybe_match) {
+  std::smatch match;
+  std::regex pattern = std::regex(fmt::format("cmd\\.joint_pos\\.({})", kAlphanumeric));
+  if (std::regex_match(maybe_match.name, match, pattern)) {
+    found_matches_[match[1].str()] = maybe_match;
+    return true;
+  }
+  return false;
+}
+
+std::vector<std::unique_ptr<Input>> CommandJointPositionMatcher::createInputs() const {
+  std::vector<std::unique_ptr<Input>> inputs;
+  for (const auto& [name, match] : found_matches_) {
+    if (!match.metadata.has_value()) continue;
+    auto maybe_metadata =
+        metadata::safe_json_get<metadata::JointPositionCommandMetadata>(match.metadata.value());
+    if (!maybe_metadata.has_value()) continue;
+    if (maybe_metadata.value().joint_names.empty()) continue;
+    inputs.push_back(
+        std::make_unique<CommandJointPositionInput>(match.name, name, maybe_metadata.value()));
+  }
+  return inputs;
+}
+
 bool CommandSE2VelocityMatcher::matches(const Match& maybe_match) {
   std::smatch match;
   std::regex pattern = std::regex(fmt::format("cmd\\.se2_velocity\\.({})", kAlphanumeric));

control/test/components_test.cpp

@@ -212,6 +212,51 @@ TEST_F(OnnxComponentsTest, MemoryOutput_WithRealRuntime) {
   EXPECT_TRUE(memory_was_overwritten) << "Memory should be overwritten with new output data";
 }
 
+TEST_F(OnnxComponentsTest, CommandJointPositionInput_InitAndRead) {
+  metadata::JointPositionCommandMetadata meta;
+  meta.joint_names = {"j1", "j2"};
+  CommandJointPositionInput input("cmd.joint_pos.arm", "arm", meta);
+
+  EXPECT_CALL(command_mock_, initJointPosition("arm", "j1")).WillOnce(Return(true));
+  EXPECT_CALL(command_mock_, initJointPosition("arm", "j2")).WillOnce(Return(true));
+  EXPECT_TRUE(input.init(state_mock_, command_mock_));
+
+  EXPECT_CALL(command_mock_, jointPosition("arm", "j1")).WillOnce(Return(std::make_optional(0.1f)));
+  EXPECT_CALL(command_mock_, jointPosition("arm", "j2")).WillOnce(Return(std::make_optional(0.2f)));
+  EXPECT_TRUE(input.read(runtime, state_mock_, command_mock_));
+
+  auto buffer = runtime.inputBuffer<float>("cmd.joint_pos.arm");
+  ASSERT_TRUE(buffer.has_value());
+  ASSERT_EQ(buffer->size(), 2u);
+  EXPECT_FLOAT_EQ((*buffer)[0], 0.1f);
+  EXPECT_FLOAT_EQ((*buffer)[1], 0.2f);
+}
+
+TEST_F(OnnxComponentsTest, CommandJointPositionInput_InitFailsWhenJointNamesEmpty) {
+  metadata::JointPositionCommandMetadata meta;  // joint_names left empty
+  CommandJointPositionInput input("cmd.joint_pos.arm", "arm", meta);
+  EXPECT_FALSE(input.init(state_mock_, command_mock_));
+}
+
+TEST_F(OnnxComponentsTest, CommandJointPositionInput_InitFailsWhenOneJointFails) {
+  metadata::JointPositionCommandMetadata meta;
+  meta.joint_names = {"j1", "j2"};
+  CommandJointPositionInput input("cmd.joint_pos.arm", "arm", meta);
+
+  EXPECT_CALL(command_mock_, initJointPosition("arm", "j1")).WillOnce(Return(false));
+  EXPECT_FALSE(input.init(state_mock_, command_mock_));
+}
+
+TEST_F(OnnxComponentsTest, CommandJointPositionInput_ReadFailsWhenJointUnavailable) {
+  metadata::JointPositionCommandMetadata meta;
+  meta.joint_names = {"j1", "j2"};
+  CommandJointPositionInput input("cmd.joint_pos.arm", "arm", meta);
+
+  EXPECT_CALL(command_mock_, jointPosition("arm", "j1")).WillOnce(Return(std::make_optional(0.1f)));
+  EXPECT_CALL(command_mock_, jointPosition("arm", "j2")).WillOnce(Return(std::nullopt));
+  EXPECT_FALSE(input.read(runtime, state_mock_, command_mock_));
+}
+
 // ---------------  Matcher tests for default metadata --------------------------------
 
 TEST(CommandFloatMatcherTest, CreatesInputWithoutMetadata) {
@@ -257,4 +302,61 @@ TEST(CommandSE2VelocityMatcherTest, CreatesInputWithMetadata) {
   ASSERT_EQ(inputs.size(), 1u);
 }
 
+TEST(CommandJointPositionMatcherTest, DoesNotMatchOtherPatterns) {
+  CommandJointPositionMatcher matcher;
+  EXPECT_FALSE(matcher.matches({.name = "cmd.float.gain"}));
+  EXPECT_FALSE(matcher.matches({.name = "cmd.joint_vel.arm"}));
+  EXPECT_FALSE(matcher.matches({.name = "cmd.joint_pos"}));
+}
+
+TEST(CommandJointPositionMatcherTest, SkipsInputWithoutMetadata) {
+  CommandJointPositionMatcher matcher;
+  ASSERT_TRUE(matcher.matches({.name = "cmd.joint_pos.arm"}));
+
+  auto inputs = matcher.createInputs();
+  ASSERT_EQ(inputs.size(), 0u);
+}
+
+TEST(CommandJointPositionMatcherTest, SkipsInputWithEmptyJointNames) {
+  CommandJointPositionMatcher matcher;
+  ASSERT_TRUE(matcher.matches({.name = "cmd.joint_pos.arm", .metadata = R"({"joint_names": []})"}));
+
+  auto inputs = matcher.createInputs();
+  ASSERT_EQ(inputs.size(), 0u);
+}
+
+TEST(CommandJointPositionMatcherTest, CreatesInputWithMetadata) {
+  CommandJointPositionMatcher matcher;
+  Match m{
+      .name = "cmd.joint_pos.arm",
+      .metadata = R"({"joint_names": ["j1", "j2"]})",
+  };
+  ASSERT_TRUE(matcher.matches(m));
+
+  auto inputs = matcher.createInputs();
+  ASSERT_EQ(inputs.size(), 1u);
+}
+
+TEST(CommandJointPositionMatcherTest, DoesNotMatchSameNameTwice) {
+  CommandJointPositionMatcher matcher;
+  ASSERT_TRUE(matcher.matches({.name = "cmd.joint_pos.arm"}));
+  // Second match with the same name overwrites, still produces one entry (but 0 inputs without
+  // metadata)
+  ASSERT_TRUE(matcher.matches({.name = "cmd.joint_pos.arm"}));
+
+  auto inputs = matcher.createInputs();
+  ASSERT_EQ(inputs.size(), 0u);
+}
+
+TEST(CommandJointPositionMatcherTest, MatchesMultipleCommands) {
+  CommandJointPositionMatcher matcher;
+  ASSERT_TRUE(matcher.matches(
+      {.name = "cmd.joint_pos.arm", .metadata = R"({"joint_names": ["j1", "j2"]})"}));
+  ASSERT_TRUE(matcher.matches(
+      {.name = "cmd.joint_pos.leg", .metadata = R"({"joint_names": ["j3", "j4"]})"}));
+
+  auto inputs = matcher.createInputs();
+  ASSERT_EQ(inputs.size(), 2u);
+}
+
 }  // namespace exploy::control

control/test/controller_test.cpp

@@ -225,6 +225,8 @@ class OnnxControllerTest : public ::testing::Test {
     EXPECT_CALL(command_mock_, initSe3Pose("pose", _)).WillOnce(Return(true));
     EXPECT_CALL(command_mock_, initBooleanSelector("selector", _)).WillOnce(Return(true));
     EXPECT_CALL(command_mock_, initFloatValue("value", _)).WillOnce(Return(true));
+    EXPECT_CALL(command_mock_, initJointPosition("arm", "j1")).WillOnce(Return(true));
+    EXPECT_CALL(command_mock_, initJointPosition("arm", "j2")).WillOnce(Return(true));
   }
 
   void ExpectInitCustom() {
@@ -283,6 +285,10 @@ class OnnxControllerTest : public ::testing::Test {
         .WillRepeatedly(Return(std::make_optional(true)));
     EXPECT_CALL(command_mock_, floatValue("value"))
         .WillRepeatedly(Return(std::make_optional(1.23f)));
+    EXPECT_CALL(command_mock_, jointPosition("arm", "j1"))
+        .WillRepeatedly(Return(std::make_optional(0.0f)));
+    EXPECT_CALL(command_mock_, jointPosition("arm", "j2"))
+        .WillRepeatedly(Return(std::make_optional(0.0f)));
   }
 
   void ExpectReadCustom() {

control/test/mock_command_interface.hpp

@@ -24,6 +24,10 @@ class MockCommandInterface : public CommandInterface {
               (const std::string& command_name, const FloatScalarConfig& config), (override));
   MOCK_METHOD(std::optional<float>, floatValue, (const std::string& command_name),
               (const override));
+  MOCK_METHOD(bool, initJointPosition,
+              (const std::string& command_name, const std::string& joint_name), (override));
+  MOCK_METHOD(std::optional<float>, jointPosition,
+              (const std::string& command_name, const std::string& joint_name), (const, override));
 };
 
 }  // namespace exploy::control