[MockHardware] Added dynamic simulation functionality.

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<hardware_interface::CommandInterface> export_command_interfaces() override;
 
+ return_type prepare_command_mode_switch(
+ const std::vector<std::string> & start_interfaces,
+ const std::vector<std::string> & stop_interfaces) override;
+
+ return_type perform_command_mode_switch(
+ const std::vector<std::string> & start_interfaces,
+ const std::vector<std::string> & 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;
@@ -115,6 +125,9 @@ class HARDWARE_INTERFACE_PUBLIC GenericSystem : public hardware_interface::Syste
  std::string custom_interface_with_following_offset_;
  size_t index_custom_interface_with_following_offset_;
 
+ bool calculate_dynamics_;
+ std::vector<size_t> joint_control_mode_;
+
  bool command_propagation_disabled_;
 };
 

hardware_interface/src/mock_components/generic_system.cpp

@@ -131,6 +131,17 @@ CallbackReturn GenericSystem::on_init(const hardware_interface::HardwareInfo & i
  command_propagation_disabled_ = false;
  }
 
+ // check if there is parameter that enables dynamic calculation
+ 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_ = "";
@@ -312,7 +323,7 @@ std::vector<hardware_interface::CommandInterface> GenericSystem::export_command_
  std::vector<hardware_interface::CommandInterface> 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)
@@ -333,6 +344,8 @@ std::vector<hardware_interface::CommandInterface> GenericSystem::export_command_
  }
  }
  }
+ // Set position control mode per default
+ joint_control_mode_.resize(info_.joints.size(), POSITION_INTERFACE_INDEX);
 
  // Mock sensor command interfaces
  if (use_mock_sensor_command_interfaces_)
@@ -369,7 +382,135 @@ std::vector<hardware_interface::CommandInterface> 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<std::string> & start_interfaces,
+ const std::vector<std::string> & /*stop_interfaces*/)
+{
+ hardware_interface::return_type ret_val = hardware_interface::return_type::OK;
+
+ const size_t FOUND_ONCE_FLAG = 1000000;
+
+ std::vector<size_t> 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)
+ {
+ if (!calculate_dynamics_)
+ {
+ RCUTILS_LOG_WARN_NAMED(
+ "mock_generic_system",
+ "Requested velocity mode for joint '%s' without dynamics calculation enabled - this "
+ "might lead to wrong feedback and unexpected behavior.",
+ info_.joints[joint_index].name.c_str());
+ }
+ joint_found_in_x_requests_[joint_index] += 1;
+ }
+ if (key == info_.joints[joint_index].name + "/" + hardware_interface::HW_IF_ACCELERATION)
+ {
+ if (!calculate_dynamics_)
+ {
+ RCUTILS_LOG_WARN_NAMED(
+ "mock_generic_system",
+ "Requested acceleration mode for joint '%s' without dynamics calculation enabled - "
+ "this might lead to wrong feedback and unexpected behavior.",
+ info_.joints[joint_index].name.c_str());
+ }
+ 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<std::string> & start_interfaces,
+ const std::vector<std::string> & /*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)
 {
  if (command_propagation_disabled_)
  {
@@ -392,19 +533,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_)
  {