From 00309079a6148b7f908cd9f6827d9bd5e948d126 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Wed, 17 May 2023 02:31:01 +0200 Subject: [PATCH] Added dynamic simulation functionality. --- .../mock_components/generic_system.hpp | 17 +- .../src/mock_components/generic_system.cpp | 227 ++++++++++++++++- .../mock_components/test_generic_system.cpp | 232 +++++++++++++++++- 3 files changed, 464 insertions(+), 12 deletions(-) diff --git a/hardware_interface/include/mock_components/generic_system.hpp b/hardware_interface/include/mock_components/generic_system.hpp index 78ae1488a6..dacf71c461 100644 --- a/hardware_interface/include/mock_components/generic_system.hpp +++ b/hardware_interface/include/mock_components/generic_system.hpp @@ -32,6 +32,10 @@ namespace mock_components { using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; +static constexpr size_t POSITION_INTERFACE_INDEX = 0; +static constexpr size_t VELOCITY_INTERFACE_INDEX = 1; +static constexpr size_t ACCELERATION_INTERFACE_INDEX = 2; + class HARDWARE_INTERFACE_PUBLIC GenericSystem : public hardware_interface::SystemInterface { public: @@ -41,6 +45,14 @@ class HARDWARE_INTERFACE_PUBLIC GenericSystem : public hardware_interface::Syste std::vector export_command_interfaces() override; + return_type prepare_command_mode_switch( + const std::vector & start_interfaces, + const std::vector & stop_interfaces) override; + + return_type perform_command_mode_switch( + const std::vector & start_interfaces, + const std::vector & stop_interfaces) override; + return_type read(const rclcpp::Time & time, const rclcpp::Duration & period) override; return_type write(const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) override @@ -60,8 +72,6 @@ class HARDWARE_INTERFACE_PUBLIC GenericSystem : public hardware_interface::Syste hardware_interface::HW_IF_POSITION, hardware_interface::HW_IF_VELOCITY, hardware_interface::HW_IF_ACCELERATION, hardware_interface::HW_IF_EFFORT}; - const size_t POSITION_INTERFACE_INDEX = 0; - struct MimicJoint { std::size_t joint_index; @@ -114,6 +124,9 @@ class HARDWARE_INTERFACE_PUBLIC GenericSystem : public hardware_interface::Syste double position_state_following_offset_; std::string custom_interface_with_following_offset_; size_t index_custom_interface_with_following_offset_; + + bool calculate_dynamics_; + std::vector joint_control_mode_; }; typedef GenericSystem GenericRobot; diff --git a/hardware_interface/src/mock_components/generic_system.cpp b/hardware_interface/src/mock_components/generic_system.cpp index d2c2e56510..f63f122ebd 100644 --- a/hardware_interface/src/mock_components/generic_system.cpp +++ b/hardware_interface/src/mock_components/generic_system.cpp @@ -106,6 +106,18 @@ CallbackReturn GenericSystem::on_init(const hardware_interface::HardwareInfo & i use_fake_gpio_command_interfaces_ = false; } + // check if there is parameter that enables dynamic calculation + // this way we simulate disconnected driver + it = info_.hardware_parameters.find("calculate_dynamics"); + if (it != info.hardware_parameters.end()) + { + calculate_dynamics_ = hardware_interface::parse_bool(it->second); + } + else + { + calculate_dynamics_ = false; + } + // process parameters about state following position_state_following_offset_ = 0.0; custom_interface_with_following_offset_ = ""; @@ -287,7 +299,7 @@ std::vector GenericSystem::export_command_ std::vector command_interfaces; // Joints' state interfaces - for (auto i = 0u; i < info_.joints.size(); i++) + for (size_t i = 0; i < info_.joints.size(); ++i) { const auto & joint = info_.joints[i]; for (const auto & interface : joint.command_interfaces) @@ -308,6 +320,8 @@ std::vector GenericSystem::export_command_ } } } + // Set position control mode per default + joint_control_mode_.resize(info_.joints.size(), POSITION_INTERFACE_INDEX); // Fake sensor command interfaces if (use_mock_sensor_command_interfaces_) @@ -344,7 +358,124 @@ std::vector GenericSystem::export_command_ return command_interfaces; } -return_type GenericSystem::read(const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) +return_type GenericSystem::prepare_command_mode_switch( + const std::vector & start_interfaces, + const std::vector & /*stop_interfaces*/) +{ + hardware_interface::return_type ret_val = hardware_interface::return_type::OK; + + if (!calculate_dynamics_) + { + return ret_val; + } + + const size_t FOUND_ONCE_FLAG = 1000000; + + std::vector joint_found_in_x_requests_; + joint_found_in_x_requests_.resize(info_.joints.size(), 0); + + for (const auto & key : start_interfaces) + { + // check if interface is joint + auto joint_it_found = std::find_if( + info_.joints.begin(), info_.joints.end(), + [key](const auto & joint) { return (key.find(joint.name) != std::string::npos); }); + + if (joint_it_found != info_.joints.end()) + { + const size_t joint_index = std::distance(info_.joints.begin(), joint_it_found); + if (joint_found_in_x_requests_[joint_index] == 0) + { + joint_found_in_x_requests_[joint_index] = FOUND_ONCE_FLAG; + } + + if (key == info_.joints[joint_index].name + "/" + hardware_interface::HW_IF_POSITION) + { + joint_found_in_x_requests_[joint_index] += 1; + } + if (key == info_.joints[joint_index].name + "/" + hardware_interface::HW_IF_VELOCITY) + { + joint_found_in_x_requests_[joint_index] += 1; + } + if (key == info_.joints[joint_index].name + "/" + hardware_interface::HW_IF_ACCELERATION) + { + joint_found_in_x_requests_[joint_index] += 1; + } + } + else + { + RCUTILS_LOG_DEBUG_NAMED( + "mock_generic_system", "Got interface '%s' that is not joint - nothing to do!", + key.c_str()); + } + } + + for (size_t i = 0; i < info_.joints.size(); ++i) + { + // There has to always be at least one control mode from the above three set + if (joint_found_in_x_requests_[i] == FOUND_ONCE_FLAG) + { + RCUTILS_LOG_ERROR_NAMED( + "mock_generic_system", "Joint '%s' has to have '%s', '%s', or '%s' interface!", + info_.joints[i].name.c_str(), hardware_interface::HW_IF_POSITION, + hardware_interface::HW_IF_VELOCITY, hardware_interface::HW_IF_ACCELERATION); + ret_val = hardware_interface::return_type::ERROR; + } + + // Currently we don't support multiple interface request + if (joint_found_in_x_requests_[i] > (FOUND_ONCE_FLAG + 1)) + { + RCUTILS_LOG_ERROR_NAMED( + "mock_generic_system", + "Got multiple (%zu) starting interfaces for joint '%s' - this is not " + "supported!", + joint_found_in_x_requests_[i] - FOUND_ONCE_FLAG, info_.joints[i].name.c_str()); + ret_val = hardware_interface::return_type::ERROR; + } + } + + return ret_val; +} + +return_type GenericSystem::perform_command_mode_switch( + const std::vector & start_interfaces, + const std::vector & /*stop_interfaces*/) +{ + if (!calculate_dynamics_) + { + return hardware_interface::return_type::OK; + } + + for (const auto & key : start_interfaces) + { + // check if interface is joint + auto joint_it_found = std::find_if( + info_.joints.begin(), info_.joints.end(), + [key](const auto & joint) { return (key.find(joint.name) != std::string::npos); }); + + if (joint_it_found != info_.joints.end()) + { + const size_t joint_index = std::distance(info_.joints.begin(), joint_it_found); + + if (key == info_.joints[joint_index].name + "/" + hardware_interface::HW_IF_POSITION) + { + joint_control_mode_[joint_index] = POSITION_INTERFACE_INDEX; + } + if (key == info_.joints[joint_index].name + "/" + hardware_interface::HW_IF_VELOCITY) + { + joint_control_mode_[joint_index] = VELOCITY_INTERFACE_INDEX; + } + if (key == info_.joints[joint_index].name + "/" + hardware_interface::HW_IF_ACCELERATION) + { + joint_control_mode_[joint_index] = ACCELERATION_INTERFACE_INDEX; + } + } + } + + return hardware_interface::return_type::OK; +} + +return_type GenericSystem::read(const rclcpp::Time & /*time*/, const rclcpp::Duration & period) { auto mirror_command_to_state = [](auto & states_, auto commands_, size_t start_index = 0) { @@ -360,19 +491,97 @@ return_type GenericSystem::read(const rclcpp::Time & /*time*/, const rclcpp::Dur } }; - // apply offset to positions only for (size_t j = 0; j < joint_states_[POSITION_INTERFACE_INDEX].size(); ++j) { - if (!std::isnan(joint_commands_[POSITION_INTERFACE_INDEX][j])) + if (calculate_dynamics_) { - joint_states_[POSITION_INTERFACE_INDEX][j] = - joint_commands_[POSITION_INTERFACE_INDEX][j] + - (custom_interface_with_following_offset_.empty() ? position_state_following_offset_ : 0.0); + switch (joint_control_mode_[j]) + { + case ACCELERATION_INTERFACE_INDEX: + { + // currently we do backward integration + joint_states_[POSITION_INTERFACE_INDEX][j] += // apply offset to positions only + joint_states_[VELOCITY_INTERFACE_INDEX][j] * period.seconds() + + (custom_interface_with_following_offset_.empty() ? position_state_following_offset_ + : 0.0); + + joint_states_[VELOCITY_INTERFACE_INDEX][j] += + joint_states_[ACCELERATION_INTERFACE_INDEX][j] * period.seconds(); + + if (!std::isnan(joint_commands_[ACCELERATION_INTERFACE_INDEX][j])) + { + joint_states_[ACCELERATION_INTERFACE_INDEX][j] = + joint_commands_[ACCELERATION_INTERFACE_INDEX][j]; + } + break; + } + case VELOCITY_INTERFACE_INDEX: + { + // currently we do backward integration + joint_states_[POSITION_INTERFACE_INDEX][j] += // apply offset to positions only + joint_states_[VELOCITY_INTERFACE_INDEX][j] * period.seconds() + + (custom_interface_with_following_offset_.empty() ? position_state_following_offset_ + : 0.0); + + if (!std::isnan(joint_commands_[VELOCITY_INTERFACE_INDEX][j])) + { + const double old_velocity = joint_states_[VELOCITY_INTERFACE_INDEX][j]; + + joint_states_[VELOCITY_INTERFACE_INDEX][j] = + joint_commands_[VELOCITY_INTERFACE_INDEX][j]; + + joint_states_[ACCELERATION_INTERFACE_INDEX][j] = + (joint_states_[VELOCITY_INTERFACE_INDEX][j] - old_velocity) / period.seconds(); + } + break; + } + case POSITION_INTERFACE_INDEX: + { + if (!std::isnan(joint_commands_[POSITION_INTERFACE_INDEX][j])) + { + const double old_position = joint_states_[POSITION_INTERFACE_INDEX][j]; + const double old_velocity = joint_states_[VELOCITY_INTERFACE_INDEX][j]; + + joint_states_[POSITION_INTERFACE_INDEX][j] = // apply offset to positions only + joint_commands_[POSITION_INTERFACE_INDEX][j] + + (custom_interface_with_following_offset_.empty() ? position_state_following_offset_ + : 0.0); + + joint_states_[VELOCITY_INTERFACE_INDEX][j] = + (joint_states_[POSITION_INTERFACE_INDEX][j] - old_position) / period.seconds(); + + joint_states_[ACCELERATION_INTERFACE_INDEX][j] = + (joint_states_[VELOCITY_INTERFACE_INDEX][j] - old_velocity) / period.seconds(); + } + break; + } + } + } + else + { + for (size_t j = 0; j < joint_states_[POSITION_INTERFACE_INDEX].size(); ++j) + { + if (!std::isnan(joint_commands_[POSITION_INTERFACE_INDEX][j])) + { + joint_states_[POSITION_INTERFACE_INDEX][j] = // apply offset to positions only + joint_commands_[POSITION_INTERFACE_INDEX][j] + + (custom_interface_with_following_offset_.empty() ? position_state_following_offset_ + : 0.0); + } + } } } - // do loopback on all other interfaces - starts from 1 because 0 index is position interface - mirror_command_to_state(joint_states_, joint_commands_, 1); + // do loopback on all other interfaces - starts from 1 or 3 because 0, 1, 3 are position, + // velocity, and acceleration interface + if (calculate_dynamics_) + { + mirror_command_to_state(joint_states_, joint_commands_, 3); + } + else + { + mirror_command_to_state(joint_states_, joint_commands_, 1); + } for (const auto & mimic_joint : mimic_joints_) { diff --git a/hardware_interface/test/mock_components/test_generic_system.cpp b/hardware_interface/test/mock_components/test_generic_system.cpp index 9fb1aa4988..52642a130a 100644 --- a/hardware_interface/test/mock_components/test_generic_system.cpp +++ b/hardware_interface/test/mock_components/test_generic_system.cpp @@ -33,7 +33,8 @@ namespace { const auto TIME = rclcpp::Time(0); -const auto PERIOD = rclcpp::Duration::from_seconds(0.01); +const auto PERIOD = rclcpp::Duration::from_seconds(0.1); // 0.1 seconds for easier math +const auto COMPARE_DELTA = 0.0001; } // namespace class TestGenericSystem : public ::testing::Test @@ -448,6 +449,39 @@ class TestGenericSystem : public ::testing::Test )"; + + hardware_system_2dof_standard_interfaces_with_different_controli_modes_ = + R"( + + + mock_components/GenericSystem + true + + + + + + 3.45 + + + + + + + + + 2.78 + + + + + + + + + + +)"; } std::string hardware_robot_2dof_; @@ -467,6 +501,7 @@ class TestGenericSystem : public ::testing::Test std::string valid_urdf_ros2_control_system_robot_with_gpio_fake_command_; std::string sensor_with_initial_value_; std::string gpio_with_initial_value_; + std::string hardware_system_2dof_standard_interfaces_with_different_controli_modes_; }; // Forward declaration @@ -1553,3 +1588,198 @@ TEST_F(TestGenericSystem, gpio_with_initial_value_) ASSERT_EQ(1, state.get_value()); } + +TEST_F(TestGenericSystem, simple_dynamics_pos_vel_acc_control_modes_interfaces) +{ + auto urdf = ros2_control_test_assets::urdf_head + + hardware_system_2dof_standard_interfaces_with_different_controli_modes_ + + ros2_control_test_assets::urdf_tail; + TestableResourceManager rm(urdf); + // Activate components to get all interfaces available + activate_components(rm); + + // Check interfaces + EXPECT_EQ(1u, rm.system_components_size()); + ASSERT_EQ(7u, rm.state_interface_keys().size()); + EXPECT_TRUE(rm.state_interface_exists("joint1/position")); + EXPECT_TRUE(rm.state_interface_exists("joint1/velocity")); + EXPECT_TRUE(rm.state_interface_exists("joint1/acceleration")); + EXPECT_TRUE(rm.state_interface_exists("joint2/velocity")); + EXPECT_TRUE(rm.state_interface_exists("joint2/velocity")); + EXPECT_TRUE(rm.state_interface_exists("joint2/acceleration")); + EXPECT_TRUE(rm.state_interface_exists("flange_vacuum/vacuum")); + + ASSERT_EQ(5u, rm.command_interface_keys().size()); + EXPECT_TRUE(rm.command_interface_exists("joint1/position")); + EXPECT_TRUE(rm.command_interface_exists("joint1/velocity")); + EXPECT_TRUE(rm.command_interface_exists("joint2/velocity")); + EXPECT_TRUE(rm.command_interface_exists("joint2/acceleration")); + EXPECT_TRUE(rm.command_interface_exists("flange_vacuum/vacuum")); + + // Check initial values + hardware_interface::LoanedStateInterface j1p_s = rm.claim_state_interface("joint1/position"); + hardware_interface::LoanedStateInterface j1v_s = rm.claim_state_interface("joint1/velocity"); + hardware_interface::LoanedStateInterface j1a_s = rm.claim_state_interface("joint1/acceleration"); + hardware_interface::LoanedStateInterface j2p_s = rm.claim_state_interface("joint2/position"); + hardware_interface::LoanedStateInterface j2v_s = rm.claim_state_interface("joint2/velocity"); + hardware_interface::LoanedStateInterface j2a_s = rm.claim_state_interface("joint2/acceleration"); + hardware_interface::LoanedCommandInterface j1p_c = rm.claim_command_interface("joint1/position"); + hardware_interface::LoanedCommandInterface j1v_c = rm.claim_command_interface("joint1/velocity"); + hardware_interface::LoanedCommandInterface j2v_c = rm.claim_command_interface("joint2/velocity"); + hardware_interface::LoanedCommandInterface j2a_c = + rm.claim_command_interface("joint2/acceleration"); + + EXPECT_EQ(3.45, j1p_s.get_value()); + EXPECT_EQ(0.0, j1v_s.get_value()); + EXPECT_EQ(0.0, j1a_s.get_value()); + EXPECT_EQ(2.78, j2p_s.get_value()); + EXPECT_EQ(0.0, j2v_s.get_value()); + EXPECT_EQ(0.0, j2a_s.get_value()); + ASSERT_TRUE(std::isnan(j1p_c.get_value())); + ASSERT_TRUE(std::isnan(j1v_c.get_value())); + ASSERT_TRUE(std::isnan(j2v_c.get_value())); + ASSERT_TRUE(std::isnan(j2a_c.get_value())); + + // Test error management in prepare mode switch + ASSERT_EQ( // joint2 has non 'position', 'velocity', or 'acceleration' interface + rm.prepare_command_mode_switch({"joint1/position", "joint2/effort"}, {}), false); + ASSERT_EQ( // joint1 has two interfaces + rm.prepare_command_mode_switch({"joint1/position", "joint1/acceleration"}, {}), false); + + // switch controller mode as controller manager is doing - gpio itf 'vacuum' will be ignored + ASSERT_EQ( + rm.prepare_command_mode_switch( + {"joint1/position", "joint2/acceleration", "flange_vacuum/vacuum"}, {}), + true); + ASSERT_EQ( + rm.perform_command_mode_switch( + {"joint1/position", "joint2/acceleration", "flange_vacuum/vacuum"}, {}), + true); + + // set some new values in commands + j1p_c.set_value(0.11); + j2a_c.set_value(3.5); + + // State values should not be changed + EXPECT_EQ(3.45, j1p_s.get_value()); + EXPECT_EQ(0.0, j1v_s.get_value()); + EXPECT_EQ(0.0, j1a_s.get_value()); + EXPECT_EQ(2.78, j2p_s.get_value()); + EXPECT_EQ(0.0, j2v_s.get_value()); + EXPECT_EQ(0.0, j2a_s.get_value()); + ASSERT_EQ(0.11, j1p_c.get_value()); + ASSERT_TRUE(std::isnan(j1v_c.get_value())); + ASSERT_TRUE(std::isnan(j2v_c.get_value())); + ASSERT_EQ(3.5, j2a_c.get_value()); + + // write() does not change values + rm.write(TIME, PERIOD); + EXPECT_EQ(3.45, j1p_s.get_value()); + EXPECT_EQ(0.0, j1v_s.get_value()); + EXPECT_EQ(0.0, j1a_s.get_value()); + EXPECT_EQ(2.78, j2p_s.get_value()); + EXPECT_EQ(0.0, j2v_s.get_value()); + EXPECT_EQ(0.0, j2a_s.get_value()); + ASSERT_EQ(0.11, j1p_c.get_value()); + ASSERT_TRUE(std::isnan(j1v_c.get_value())); + ASSERT_TRUE(std::isnan(j2v_c.get_value())); + ASSERT_EQ(3.5, j2a_c.get_value()); + + // read() mirrors commands to states and calculate dynamics + rm.read(TIME, PERIOD); + EXPECT_EQ(0.11, j1p_s.get_value()); + EXPECT_EQ(-33.4, j1v_s.get_value()); + EXPECT_NEAR(-334.0, j1a_s.get_value(), COMPARE_DELTA); + EXPECT_EQ(2.78, j2p_s.get_value()); + EXPECT_EQ(0.0, j2v_s.get_value()); + EXPECT_EQ(3.5, j2a_s.get_value()); + ASSERT_EQ(0.11, j1p_c.get_value()); + ASSERT_TRUE(std::isnan(j1v_c.get_value())); + ASSERT_TRUE(std::isnan(j2v_c.get_value())); + ASSERT_EQ(3.5, j2a_c.get_value()); + + // read() mirrors commands to states and calculate dynamics + rm.read(TIME, PERIOD); + EXPECT_EQ(0.11, j1p_s.get_value()); + EXPECT_EQ(0.0, j1v_s.get_value()); + EXPECT_NEAR(334.0, j1a_s.get_value(), COMPARE_DELTA); + EXPECT_EQ(2.78, j2p_s.get_value()); + EXPECT_NEAR(0.35, j2v_s.get_value(), COMPARE_DELTA); + EXPECT_EQ(3.5, j2a_s.get_value()); + ASSERT_EQ(0.11, j1p_c.get_value()); + ASSERT_TRUE(std::isnan(j1v_c.get_value())); + ASSERT_TRUE(std::isnan(j2v_c.get_value())); + ASSERT_EQ(3.5, j2a_c.get_value()); + + // read() mirrors commands to states and calculate dynamics + rm.read(TIME, PERIOD); + EXPECT_EQ(0.11, j1p_s.get_value()); + EXPECT_EQ(0.0, j1v_s.get_value()); + EXPECT_EQ(0.0, j1a_s.get_value()); + EXPECT_EQ(2.815, j2p_s.get_value()); + EXPECT_NEAR(0.7, j2v_s.get_value(), COMPARE_DELTA); + EXPECT_EQ(3.5, j2a_s.get_value()); + ASSERT_EQ(0.11, j1p_c.get_value()); + ASSERT_TRUE(std::isnan(j1v_c.get_value())); + ASSERT_TRUE(std::isnan(j2v_c.get_value())); + ASSERT_EQ(3.5, j2a_c.get_value()); + + // switch controller mode as controller manager is doing + ASSERT_EQ(rm.prepare_command_mode_switch({"joint1/velocity", "joint2/velocity"}, {}), true); + ASSERT_EQ(rm.perform_command_mode_switch({"joint1/velocity", "joint2/velocity"}, {}), true); + + // set some new values in commands + j1v_c.set_value(0.5); + j2v_c.set_value(2.0); + + // State values should not be changed + EXPECT_EQ(0.11, j1p_s.get_value()); + EXPECT_EQ(0.0, j1v_s.get_value()); + EXPECT_EQ(0.0, j1a_s.get_value()); + EXPECT_EQ(2.815, j2p_s.get_value()); + EXPECT_NEAR(0.7, j2v_s.get_value(), COMPARE_DELTA); + EXPECT_EQ(3.5, j2a_s.get_value()); + ASSERT_EQ(0.11, j1p_c.get_value()); + ASSERT_EQ(0.5, j1v_c.get_value()); + ASSERT_EQ(2.0, j2v_c.get_value()); + ASSERT_EQ(3.5, j2a_c.get_value()); + + // write() does not change values + rm.write(TIME, PERIOD); + EXPECT_EQ(0.11, j1p_s.get_value()); + EXPECT_EQ(0.0, j1v_s.get_value()); + EXPECT_EQ(0.0, j1a_s.get_value()); + EXPECT_EQ(2.815, j2p_s.get_value()); + EXPECT_NEAR(0.7, j2v_s.get_value(), COMPARE_DELTA); + EXPECT_EQ(3.5, j2a_s.get_value()); + ASSERT_EQ(0.11, j1p_c.get_value()); + ASSERT_EQ(0.5, j1v_c.get_value()); + ASSERT_EQ(2.0, j2v_c.get_value()); + ASSERT_EQ(3.5, j2a_c.get_value()); + + // read() mirrors commands to states and calculate dynamics (both velocity mode) + rm.read(TIME, PERIOD); + EXPECT_EQ(0.11, j1p_s.get_value()); + EXPECT_EQ(0.5, j1v_s.get_value()); + EXPECT_EQ(5.0, j1a_s.get_value()); + EXPECT_EQ(2.885, j2p_s.get_value()); + EXPECT_EQ(2.0, j2v_s.get_value()); + EXPECT_NEAR(13.0, j2a_s.get_value(), COMPARE_DELTA); + ASSERT_EQ(0.11, j1p_c.get_value()); + ASSERT_EQ(0.5, j1v_c.get_value()); + ASSERT_EQ(2.0, j2v_c.get_value()); + ASSERT_EQ(3.5, j2a_c.get_value()); + + // read() mirrors commands to states and calculate dynamics (both velocity mode) + rm.read(TIME, PERIOD); + EXPECT_EQ(0.16, j1p_s.get_value()); + EXPECT_EQ(0.5, j1v_s.get_value()); + EXPECT_EQ(0.0, j1a_s.get_value()); + EXPECT_EQ(3.085, j2p_s.get_value()); + EXPECT_EQ(2.0, j2v_s.get_value()); + EXPECT_EQ(0.0, j2a_s.get_value()); + ASSERT_EQ(0.11, j1p_c.get_value()); + ASSERT_EQ(0.5, j1v_c.get_value()); + ASSERT_EQ(2.0, j2v_c.get_value()); + ASSERT_EQ(3.5, j2a_c.get_value()); +}