diff --git a/LICENSE b/LICENSE
index c2b7de3..cd7dbc7 100644
--- a/LICENSE
+++ b/LICENSE
@@ -186,7 +186,7 @@
same "printed page" as the copyright notice for easier
identification within third-party archives.
- Copyright 2025 Thomason Zhou
+ Copyright 2025 Enactic, Inc.
Licensed under the Apache License, Version 2.0 (the "License");
you may not use this file except in compliance with the License.
diff --git a/openarm_bimanual.mjcf.xml b/openarm_bimanual.mjcf.xml
index 74085ce..aa09e14 100644
--- a/openarm_bimanual.mjcf.xml
+++ b/openarm_bimanual.mjcf.xml
@@ -3,7 +3,7 @@
-
+
@@ -14,6 +14,8 @@
+
+
@@ -46,48 +48,48 @@
-
+
-
+
-
+
-
+
-
+
-
+
-
+
-
+
-
+
@@ -108,48 +110,48 @@
-
+
-
+
-
+
-
+
-
+
-
+
-
+
-
+
-
+
@@ -190,23 +192,43 @@
-
-
+
+
-
+
-
+
-
+
-
-
+
+
-
+
-
+
-
+
+
+
+
+
diff --git a/openarm_bimanual_position.mjcf.xml b/openarm_bimanual_position.mjcf.xml
new file mode 100644
index 0000000..ef82e87
--- /dev/null
+++ b/openarm_bimanual_position.mjcf.xml
@@ -0,0 +1,251 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/openarm_mujoco_hardware/CMakeLists.txt b/openarm_mujoco_hardware/CMakeLists.txt
new file mode 100644
index 0000000..6ab9460
--- /dev/null
+++ b/openarm_mujoco_hardware/CMakeLists.txt
@@ -0,0 +1,98 @@
+# Copyright 2025 Enactic, Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+# http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+cmake_minimum_required(VERSION 3.22)
+project(openarm_mujoco_hardware)
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+ add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+# find dependencies
+find_package(ament_cmake REQUIRED)
+find_package(hardware_interface REQUIRED)
+find_package(pluginlib REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(rclcpp_lifecycle REQUIRED)
+find_package(Boost REQUIRED)
+find_package(nlohmann_json REQUIRED)
+
+add_library(${PROJECT_NAME} SHARED
+ src/openarm_mujoco_hardware.cpp
+)
+
+target_include_directories(
+ ${PROJECT_NAME}
+ PRIVATE
+ include
+)
+
+ament_target_dependencies(${PROJECT_NAME} PUBLIC
+ hardware_interface
+ pluginlib
+ rclcpp
+ rclcpp_lifecycle
+)
+
+pluginlib_export_plugin_description_file(hardware_interface openarm_mujoco_hardware.xml)
+
+target_link_libraries(${PROJECT_NAME} PRIVATE Boost::headers nlohmann_json::nlohmann_json)
+
+
+install(
+ TARGETS ${PROJECT_NAME}
+ DESTINATION lib
+ )
+
+install(DIRECTORY include/
+DESTINATION include
+)
+
+
+if(BUILD_TESTING)
+ find_package(ament_lint_auto REQUIRED)
+ find_package(ament_cmake_gmock REQUIRED)
+ find_package(hardware_interface REQUIRED)
+ find_package(ros2_control_test_assets REQUIRED)
+
+ ament_add_gmock(test_openarm_mujoco_hardware
+ test/test_load_openarm_mujoco_hardware.cpp
+ )
+ target_link_libraries(test_openarm_mujoco_hardware
+ ${PROJECT_NAME}
+ )
+ ament_target_dependencies(test_openarm_mujoco_hardware
+ hardware_interface
+ ros2_control_test_assets
+ )
+
+ set(ament_cmake_copyright_FOUND TRUE)
+ set(ament_cmake_cpplint_FOUND TRUE)
+ ament_lint_auto_find_test_dependencies()
+endif()
+
+ament_export_include_directories(
+ include
+)
+
+ament_export_libraries(
+ ${PROJECT_NAME}
+)
+ament_export_dependencies(
+ hardware_interface
+ pluginlib
+ rclcpp
+)
+
+ament_package()
diff --git a/openarm_mujoco_hardware/README.md b/openarm_mujoco_hardware/README.md
new file mode 100644
index 0000000..cfb1836
--- /dev/null
+++ b/openarm_mujoco_hardware/README.md
@@ -0,0 +1,42 @@
+# OpenArm MuJoCo Hardware Interface
+
+This package provides a ros2_control hardware interface for simulating OpenArm using the MuJoCo physics engine in place of physical hardware. It connects to a WebAssembly instance of MuJoCo through WebSockets.
+
+## Usage
+
+Certain OpenArm packages have been configured to use this interface by specifying the `hardware_type` flag.
+This flag defaults to `real`, which assumes the use of physical hardware.
+Setting the flag to `mock` uses an interface that writes commands to states directly (with some delays).
+Setting the flag to `sim` uses the MuJoCo hardware interface.
+
+For example, to use MoveIt2 with simulated bimanual hardware, first run MuJoCo by visiting:
+
+[github.com/thomasonzhou/mujoco_anywhere](https://github.com/thomasonzhou/mujoco_anywhere)
+
+Then run the original command with the `hardware_type` flag:
+```sh
+ros2 launch -d openarm_bimanual_moveit_config demo.launch.py hardware_type:=sim
+```
+
+*It may be necessary to install the nlohmann-json-dev library before building*
+
+Please note that running multiple instances of the website will cause conflicting signals. Future configurations will allow for multiple instances to run simultaneously.
+
+## Configuration
+
+### Hardware Plugin Config
+
+The hardware plugin is specified in `openarm_description/openarm.ros2_control.xacro` as follows:
+
+```xml
+
+
+ openarm_mujoco_hardware/MujocoHardware
+ left_
+ 1337
+
+
+
+```
+
+When using OpenArm in a bimanual configuration, the WebSocket ports default to 1337 for right arm and 1338 for left arm commands. However, in practice commands can be sent and states can be received through any connected port.
diff --git a/openarm_mujoco_hardware/include/openarm_mujoco_hardware/openarm_mujoco_hardware.hpp b/openarm_mujoco_hardware/include/openarm_mujoco_hardware/openarm_mujoco_hardware.hpp
new file mode 100644
index 0000000..7cc6629
--- /dev/null
+++ b/openarm_mujoco_hardware/include/openarm_mujoco_hardware/openarm_mujoco_hardware.hpp
@@ -0,0 +1,123 @@
+// Copyright 2025 Enactic, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#pragma once
+
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+
+#include "hardware_interface/system_interface.hpp"
+#include "hardware_interface/types/hardware_interface_type_values.hpp"
+
+namespace openarm_mujoco_hardware {
+
+class WebSocketSession;
+
+class MujocoHardware : public hardware_interface::SystemInterface {
+ public:
+ MujocoHardware() = default;
+
+ hardware_interface::CallbackReturn on_init(
+ const hardware_interface::HardwareInfo& info) override;
+ hardware_interface::CallbackReturn on_configure(
+ const rclcpp_lifecycle::State& /*previous_state*/) override;
+ hardware_interface::CallbackReturn on_cleanup(
+ const rclcpp_lifecycle::State& /*previous_state*/) override;
+ hardware_interface::CallbackReturn on_shutdown(
+ const rclcpp_lifecycle::State& /*previous_state*/) override;
+ hardware_interface::CallbackReturn on_activate(
+ const rclcpp_lifecycle::State& /*previous_state*/) override;
+ hardware_interface::CallbackReturn on_deactivate(
+ const rclcpp_lifecycle::State& /*previous_state*/) override;
+ hardware_interface::CallbackReturn on_error(
+ const rclcpp_lifecycle::State& /*previous_state*/) override;
+
+ std::vector export_state_interfaces()
+ override;
+ std::vector export_command_interfaces()
+ override;
+ hardware_interface::return_type read(
+ const rclcpp::Time& /*time*/,
+ const rclcpp::Duration& /*period*/) override;
+ hardware_interface::return_type write(
+ const rclcpp::Time& /*time*/,
+ const rclcpp::Duration& /*period*/) override;
+
+ friend class WebSocketSession;
+
+ private:
+ static constexpr size_t TOTAL_DOF =
+ 8; // Total degrees of freedom, including gripper
+ inline static constexpr std::array KP_ = {
+ 180.0, 180.0, 140.0, 155.0, 115.0, 115.0, 115.0, 25.0};
+ inline static constexpr std::array KD_ = {
+ 0.01, 0.01, 0.01, 0.01, 0.01, 0.01, 0.01, 0.01};
+ static constexpr double MAX_MOTOR_TORQUE = 100.0;
+
+ std::vector qpos_;
+ std::vector qvel_;
+ std::vector qtau_;
+
+ std::vector cmd_qpos_;
+ std::vector cmd_qvel_;
+ std::vector cmd_qtau_ff_;
+
+ std::mutex state_mutex_;
+
+ void clear_cmd_torque();
+
+ // websocket connection to mujoco
+ boost::asio::ip::tcp::endpoint endpoint_;
+ boost::asio::ip::address address_;
+ static constexpr double kDefaultWebsocketPort = 1337;
+ unsigned short websocket_port_;
+ boost::asio::io_context ioc_{};
+ boost::asio::ip::tcp::acceptor acceptor_{ioc_};
+ std::thread ioc_thread_;
+
+ std::shared_ptr ws_session_;
+ void start_accept();
+
+ const std::string kMuJoCoWebSocketURL_ =
+ "https://thomasonzhou.github.io/mujoco_anywhere/";
+};
+
+class WebSocketSession : public std::enable_shared_from_this {
+ public:
+ static std::shared_ptr create(
+ boost::asio::ip::tcp::socket socket, MujocoHardware* hw);
+ void run();
+ WebSocketSession(boost::asio::ip::tcp::socket socket, MujocoHardware* hw);
+
+ void send_json(const nlohmann::json& j);
+
+ private:
+ void flush();
+ void do_handshake();
+ void on_accept(boost::beast::error_code ec);
+ void do_read();
+ void on_read(boost::beast::error_code ec, std::size_t);
+
+ boost::beast::websocket::stream ws_;
+ boost::beast::flat_buffer buffer_;
+ MujocoHardware* hw_;
+ std::deque> send_queue_;
+ bool write_in_progress_;
+};
+}; // namespace openarm_mujoco_hardware
diff --git a/openarm_mujoco_hardware/openarm_mujoco_hardware.xml b/openarm_mujoco_hardware/openarm_mujoco_hardware.xml
new file mode 100644
index 0000000..97ae025
--- /dev/null
+++ b/openarm_mujoco_hardware/openarm_mujoco_hardware.xml
@@ -0,0 +1,25 @@
+
+
+
+
+
+ ros2_control hardware interface.
+
+
+
diff --git a/openarm_mujoco_hardware/package.xml b/openarm_mujoco_hardware/package.xml
new file mode 100644
index 0000000..43b085f
--- /dev/null
+++ b/openarm_mujoco_hardware/package.xml
@@ -0,0 +1,39 @@
+
+
+
+
+ openarm_mujoco_hardware
+ 1.0.0
+ MuJoCo hardware interface for ros2_control
+ Thomason Zhou
+ Apache-2.0
+
+ ament_cmake
+
+ nlohmann-json-dev
+ rclcpp
+ hardware_interface
+ pluginlib
+
+ ament_lint_auto
+ ament_lint_common
+ ros2_control_test_assets
+
+
+ ament_cmake
+
+
diff --git a/openarm_mujoco_hardware/src/openarm_mujoco_hardware.cpp b/openarm_mujoco_hardware/src/openarm_mujoco_hardware.cpp
new file mode 100644
index 0000000..b314a77
--- /dev/null
+++ b/openarm_mujoco_hardware/src/openarm_mujoco_hardware.cpp
@@ -0,0 +1,322 @@
+// Copyright 2025 Enactic, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include
+
+namespace openarm_mujoco_hardware {
+
+hardware_interface::CallbackReturn MujocoHardware::on_init(
+ const hardware_interface::HardwareInfo& info) {
+ if (hardware_interface::SystemInterface::on_init(info) !=
+ CallbackReturn::SUCCESS) {
+ return CallbackReturn::ERROR;
+ }
+
+ if (info.hardware_parameters.find("websocket_port") !=
+ info.hardware_parameters.end()) {
+ const double val = std::stoi(info.hardware_parameters.at("websocket_port"));
+ if (val <= 0) {
+ return hardware_interface::CallbackReturn::FAILURE;
+ }
+ websocket_port_ = val;
+ } else {
+ websocket_port_ = kDefaultWebsocketPort;
+ }
+ std::cerr << "websocket port: " << websocket_port_ << std::endl;
+ address_ = boost::asio::ip::make_address("0.0.0.0");
+ endpoint_ = boost::asio::ip::tcp::endpoint(address_, websocket_port_);
+
+ // allocate space for joint states
+ const size_t DOF = info_.joints.size();
+
+ qpos_.resize(DOF, 0.0);
+ qvel_.resize(DOF, 0.0);
+ qtau_.resize(DOF, 0.0);
+
+ cmd_qpos_.resize(DOF, 0.0);
+ cmd_qvel_.resize(DOF, 0.0);
+ cmd_qtau_ff_.resize(DOF, 0.0);
+
+ return hardware_interface::CallbackReturn::SUCCESS;
+}
+
+void MujocoHardware::start_accept() {
+ acceptor_.async_accept([this](boost::beast::error_code ec,
+ boost::asio::ip::tcp::socket socket) {
+ if (ec) {
+ std::cerr << "error accepting connection: " << ec.message() << std::endl;
+ }
+ std::cout << "new connection accepted." << std::endl;
+ ws_session_ = WebSocketSession::create(std::move(socket), this);
+ ws_session_->run();
+ this->start_accept();
+ });
+}
+
+hardware_interface::CallbackReturn MujocoHardware::on_configure(
+ const rclcpp_lifecycle::State& /*previous_state*/) {
+ boost::beast::error_code ec;
+
+ if (acceptor_.is_open()) {
+ return hardware_interface::CallbackReturn::FAILURE;
+ } else {
+ acceptor_.open(endpoint_.protocol(), ec);
+ if (ec) {
+ throw std::runtime_error("open error: " + ec.message());
+ }
+ }
+
+ acceptor_.set_option(boost::asio::socket_base::reuse_address(true), ec);
+ if (ec) {
+ throw std::runtime_error("enable address reuse error: " + ec.message());
+ }
+
+ acceptor_.bind(endpoint_, ec);
+ if (ec) {
+ throw std::runtime_error("bind error: " + ec.message());
+ }
+
+ acceptor_.listen(boost::asio::socket_base::max_listen_connections, ec);
+ if (ec) {
+ throw std::runtime_error("listen error: " + ec.message());
+ }
+
+ start_accept();
+
+ ioc_thread_ = std::thread([this]() {
+ try {
+ ioc_.run();
+ } catch (const std::exception& e) {
+ std::cerr << "error in io_context thread: " << e.what() << std::endl;
+ }
+ });
+
+ return hardware_interface::CallbackReturn::SUCCESS;
+}
+
+hardware_interface::CallbackReturn MujocoHardware::on_cleanup(
+ const rclcpp_lifecycle::State& /*previous_state*/) {
+ return hardware_interface::CallbackReturn::SUCCESS;
+}
+
+hardware_interface::CallbackReturn MujocoHardware::on_shutdown(
+ const rclcpp_lifecycle::State& /*previous_state*/) {
+ clear_cmd_torque();
+ ioc_.stop();
+ if (ioc_thread_.joinable()) ioc_thread_.join();
+ return hardware_interface::CallbackReturn::SUCCESS;
+}
+
+hardware_interface::CallbackReturn MujocoHardware::on_activate(
+ const rclcpp_lifecycle::State& /*previous_state*/) {
+ return hardware_interface::CallbackReturn::SUCCESS;
+}
+
+void MujocoHardware::clear_cmd_torque() {
+ nlohmann::json cmd_msg;
+ nlohmann::json& cmd = cmd_msg["cmd"];
+ for (size_t i = 0; i < cmd_qpos_.size(); ++i) {
+ cmd[info_.joints[i].name] = 0.0;
+ }
+ if (ws_session_) {
+ ws_session_->send_json(cmd_msg);
+ }
+}
+
+hardware_interface::CallbackReturn MujocoHardware::on_deactivate(
+ const rclcpp_lifecycle::State& /*previous_state*/) {
+ clear_cmd_torque();
+ return hardware_interface::CallbackReturn::SUCCESS;
+}
+
+hardware_interface::CallbackReturn MujocoHardware::on_error(
+ const rclcpp_lifecycle::State& /*previous_state*/) {
+ clear_cmd_torque();
+ return hardware_interface::CallbackReturn::SUCCESS;
+}
+std::vector
+MujocoHardware::export_state_interfaces() {
+ std::vector state_interfaces;
+ for (size_t i = 0; i < qpos_.size(); ++i) {
+ state_interfaces.emplace_back(hardware_interface::StateInterface(
+ info_.joints.at(i).name, hardware_interface::HW_IF_POSITION,
+ &qpos_[i]));
+ state_interfaces.emplace_back(hardware_interface::StateInterface(
+ info_.joints.at(i).name, hardware_interface::HW_IF_VELOCITY,
+ &qvel_[i]));
+ // state_interfaces.emplace_back(hardware_interface::StateInterface(info_.joints.at(i).name,
+ // hardware_interface::HW_IF_EFFORT, &qtau_[i]));
+ }
+ return state_interfaces;
+}
+std::vector
+MujocoHardware::export_command_interfaces() {
+ std::vector command_interfaces;
+ for (size_t i = 0; i < qpos_.size(); ++i) {
+ command_interfaces.emplace_back(hardware_interface::CommandInterface(
+ info_.joints.at(i).name, hardware_interface::HW_IF_POSITION,
+ &cmd_qpos_[i]));
+ command_interfaces.emplace_back(hardware_interface::CommandInterface(
+ info_.joints.at(i).name, hardware_interface::HW_IF_VELOCITY,
+ &cmd_qvel_[i]));
+ command_interfaces.emplace_back(hardware_interface::CommandInterface(
+ info_.joints.at(i).name, hardware_interface::HW_IF_EFFORT,
+ &cmd_qtau_ff_[i]));
+ }
+ return command_interfaces;
+}
+hardware_interface::return_type MujocoHardware::read(
+ const rclcpp::Time& /*time*/, const rclcpp::Duration& /*period*/) {
+ // Read state from the last recieived websocket message
+ // Why decouple this? The simulation might be paused, and we want to read the
+ // last state
+
+ // right now this is optional as state is updated by listening to messages
+ // from MuJoCo
+ return hardware_interface::return_type::OK;
+}
+hardware_interface::return_type MujocoHardware::write(
+ const rclcpp::Time& /*time*/, const rclcpp::Duration& /*period*/) {
+ // send a websocket message
+
+ nlohmann::json cmd_msg;
+ nlohmann::json& cmd = cmd_msg["cmd"];
+
+ for (size_t i = 0; i < cmd_qpos_.size(); ++i) {
+ const double qpos_error = cmd_qpos_[i] - qpos_[i];
+ const double qvel_error = cmd_qvel_[i] - qvel_[i];
+ const double qtau_ff = cmd_qtau_ff_[i];
+
+ double cmd_torque = KP_[i] * qpos_error + KD_[i] * qvel_error + qtau_ff;
+
+ // if (cmd_torque > MAX_MOTOR_TORQUE) {
+ // cmd_torque = MAX_MOTOR_TORQUE;
+ // } else if (cmd_torque < -MAX_MOTOR_TORQUE) {
+ // cmd_torque = -MAX_MOTOR_TORQUE;
+ // }
+ cmd[info_.joints[i].name] = cmd_torque;
+ }
+ if (ws_session_) {
+ ws_session_->send_json(cmd_msg);
+ } else {
+ std::cerr << "MuJoCo WebSocket session is not active, please connect at "
+ << kMuJoCoWebSocketURL_ << std::endl;
+ }
+
+ return hardware_interface::return_type::OK;
+}
+
+WebSocketSession::WebSocketSession(boost::asio::ip::tcp::socket socket,
+ MujocoHardware* hw)
+ : ws_(std::move(socket)), hw_(hw), write_in_progress_(false) {}
+
+std::shared_ptr WebSocketSession::create(
+ boost::asio::ip::tcp::socket socket, MujocoHardware* hw) {
+ return std::make_shared(std::move(socket), hw);
+}
+
+void WebSocketSession::send_json(const nlohmann::json& j) {
+ std::shared_ptr msg = std::make_shared(j.dump());
+ boost::asio::post(ws_.get_executor(), [self = shared_from_this(), msg] {
+ self->send_queue_.push_back(msg);
+ if (!self->write_in_progress_) {
+ self->flush();
+ }
+ });
+}
+
+void WebSocketSession::flush() {
+ if (send_queue_.empty()) {
+ write_in_progress_ = false;
+ return;
+ }
+
+ write_in_progress_ = true;
+ auto msg = send_queue_.front();
+ send_queue_.pop_front();
+
+ ws_.async_write(boost::asio::buffer(*msg),
+ [self = shared_from_this(), msg](boost::beast::error_code ec,
+ std::size_t) {
+ if (ec) {
+ std::cerr << "send error: " << ec.message() << std::endl;
+ }
+ self->write_in_progress_ = false;
+ self->flush();
+ });
+}
+
+void WebSocketSession::run() { do_handshake(); }
+
+void WebSocketSession::do_handshake() {
+ ws_.set_option(boost::beast::websocket::stream_base::timeout::suggested(
+ boost::beast::role_type::server));
+ ws_.async_accept(boost::beast::bind_front_handler(
+ &WebSocketSession::on_accept, shared_from_this()));
+}
+
+void WebSocketSession::on_accept(boost::beast::error_code ec) {
+ if (ec) {
+ std::cerr << "handshake failed: " << ec.message() << std::endl;
+ }
+ do_read();
+}
+
+void WebSocketSession::do_read() {
+ ws_.async_read(buffer_, boost::beast::bind_front_handler(
+ &WebSocketSession::on_read, shared_from_this()));
+}
+
+void WebSocketSession::on_read(boost::beast::error_code ec,
+ std::size_t bytes_transferred) {
+ if (ec) {
+ std::cerr << "read error: " << ec.message() << std::endl;
+ return;
+ }
+ std::string data = boost::beast::buffers_to_string(buffer_.data());
+ {
+ std::lock_guard(hw_->state_mutex_);
+ try {
+ nlohmann::json j = nlohmann::json::parse(data);
+ // if "state" key exists, update the hardware state
+ if (j.contains("state")) {
+ const nlohmann::json& state = j["state"];
+ for (size_t i = 0; i < hw_->info_.joints.size(); ++i) {
+ const auto& joint = hw_->info_.joints[i];
+ if (state.contains(joint.name)) {
+ const nlohmann::json& joint_data = state.at(joint.name);
+ if (joint_data.contains("qpos")) {
+ hw_->qpos_[i] = joint_data.at("qpos").get();
+ }
+ if (joint_data.contains("qvel")) {
+ hw_->qvel_[i] = joint_data.at("qvel").get();
+ }
+ }
+ }
+ }
+ } catch (const nlohmann::json::parse_error& e) {
+ std::cerr << "json parse error: " << e.what() << std::endl;
+ }
+ };
+
+ buffer_.consume(bytes_transferred);
+ do_read();
+}
+
+}; // namespace openarm_mujoco_hardware
+
+#include "pluginlib/class_list_macros.hpp"
+
+PLUGINLIB_EXPORT_CLASS(openarm_mujoco_hardware::MujocoHardware,
+ hardware_interface::SystemInterface)
\ No newline at end of file
diff --git a/openarm_mujoco_hardware/test/test_load_openarm_mujoco_hardware.cpp b/openarm_mujoco_hardware/test/test_load_openarm_mujoco_hardware.cpp
new file mode 100644
index 0000000..56e7c2b
--- /dev/null
+++ b/openarm_mujoco_hardware/test/test_load_openarm_mujoco_hardware.cpp
@@ -0,0 +1,31 @@
+// Copyright 2025 Enactic, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include
+
+#include
+
+#include "hardware_interface/system_interface.hpp"
+
+static constexpr const char* kPluginName =
+ "openarm_mujoco_hardware/MujocoHardware";
+
+TEST(TestLoadMujocoOpenarmHardware, can_load_plugin) {
+ pluginlib::ClassLoader loader(
+ "openarm_mujoco_hardware", "hardware_interface::SystemInterface");
+ std::shared_ptr instance;
+
+ ASSERT_NO_THROW(instance = loader.createSharedInstance(kPluginName));
+ EXPECT_NE(instance, nullptr);
+}