[GenericSystem] Add velocity-only mode in fake_components

hardware_interface/include/fake_components/generic_system.hpp

@@ -17,6 +17,7 @@
 #ifndef FAKE_COMPONENTS__GENERIC_SYSTEM_HPP_
 #define FAKE_COMPONENTS__GENERIC_SYSTEM_HPP_
 
+#include <chrono>
 #include <string>
 #include <vector>
 
@@ -32,6 +33,13 @@ using hardware_interface::return_type;
 
 namespace fake_components
 {
+enum StoppingInterface
+{
+  NONE,
+  STOP_POSITION,
+  STOP_VELOCITY
+};
+
 class HARDWARE_INTERFACE_PUBLIC GenericSystem
 : public hardware_interface::BaseInterface<hardware_interface::SystemInterface>
 {
@@ -58,6 +66,14 @@ class HARDWARE_INTERFACE_PUBLIC GenericSystem
 
   return_type write() override { return return_type::OK; }
 
+  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;
+
 protected:
   /// Use standard interfaces for joints because they are relevant for dynamic behaviour
   /**
@@ -71,6 +87,7 @@ class HARDWARE_INTERFACE_PUBLIC GenericSystem
     hardware_interface::HW_IF_ACCELERATION, hardware_interface::HW_IF_EFFORT};
 
   const size_t POSITION_INTERFACE_INDEX = 0;
+  const size_t VELOCITY_INTERFACE_INDEX = 1;
 
   struct MimicJoint
   {
@@ -109,6 +126,16 @@ class HARDWARE_INTERFACE_PUBLIC GenericSystem
   double position_state_following_offset_;
   std::string custom_interface_with_following_offset_;
   size_t index_custom_interface_with_following_offset_;
+  bool command_propagation_disabled_;
+
+  // resources switching aux vars
+  std::vector<uint> stop_modes_;
+  std::vector<std::string> start_modes_;
+  bool position_controller_running_;
+  bool velocity_controller_running_;
+  std::chrono::system_clock::time_point begin;
+  // for velocity control, store last position command
+  std::vector<double> joint_pos_commands_old_;
 };
 
 typedef GenericSystem GenericRobot;

hardware_interface/src/fake_components/generic_system.cpp

@@ -17,6 +17,7 @@
 #include "fake_components/generic_system.hpp"
 
 #include <algorithm>
+#include <chrono>
 #include <cmath>
 #include <iterator>
 #include <limits>
@@ -47,6 +48,18 @@ return_type GenericSystem::configure(const hardware_interface::HardwareInfo & in
     fake_sensor_command_interfaces_ = false;
   }
 
+  // check if there is parameter that disables commands
+  // this way we simulate disconnected driver
+  it = info_.hardware_parameters.find("disable_commands");
+  if (it != info.hardware_parameters.end())
+  {
+    command_propagation_disabled_ = it->second == "true" || it->second == "True";
+  }
+  else
+  {
+    command_propagation_disabled_ = false;
+  }
+
   // process parameters about state following
   position_state_following_offset_ = 0.0;
   custom_interface_with_following_offset_ = "";
@@ -66,6 +79,7 @@ return_type GenericSystem::configure(const hardware_interface::HardwareInfo & in
 
   // Initialize storage for standard interfaces
   initialize_storage_vectors(joint_commands_, joint_states_, standard_interfaces_);
+
   // set all values without initial values to 0
   for (auto i = 0u; i < info_.joints.size(); i++)
   {
@@ -78,6 +92,16 @@ return_type GenericSystem::configure(const hardware_interface::HardwareInfo & in
     }
   }
 
+  // set memory position vector to initial value
+  joint_pos_commands_old_.resize(joint_commands_[POSITION_INTERFACE_INDEX].size());
+  joint_pos_commands_old_ = joint_commands_[POSITION_INTERFACE_INDEX];
+
+  // joint velocity commands to zero
+  for (auto i = 0u; i < joint_commands_[VELOCITY_INTERFACE_INDEX].size(); ++i)
+  {
+    joint_commands_[VELOCITY_INTERFACE_INDEX][i] = 0.0;
+  }
+
   // Search for mimic joints
   for (auto i = 0u; i < info_.joints.size(); ++i)
   {
@@ -180,6 +204,15 @@ return_type GenericSystem::configure(const hardware_interface::HardwareInfo & in
   }
   initialize_storage_vectors(sensor_fake_commands_, sensor_states_, sensor_interfaces_);
 
+  stop_modes_ = {StoppingInterface::NONE, StoppingInterface::NONE, StoppingInterface::NONE,
+                 StoppingInterface::NONE, StoppingInterface::NONE, StoppingInterface::NONE};
+  start_modes_ = {hardware_interface::HW_IF_POSITION, hardware_interface::HW_IF_POSITION,
+                  hardware_interface::HW_IF_POSITION, hardware_interface::HW_IF_POSITION,
+                  hardware_interface::HW_IF_POSITION, hardware_interface::HW_IF_POSITION};
+  position_controller_running_ = false;
+  velocity_controller_running_ = false;
+  begin = std::chrono::system_clock::now();
+
   status_ = hardware_interface::status::CONFIGURED;
   return hardware_interface::return_type::OK;
 }
@@ -278,20 +311,134 @@ std::vector<hardware_interface::CommandInterface> GenericSystem::export_command_
   return command_interfaces;
 }
 
+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;
+
+  start_modes_.clear();
+  stop_modes_.clear();
+
+  // Starting interfaces
+  // add start interface per joint in tmp var for later check
+  for (const auto & key : start_interfaces)
+  {
+    for (auto i = 0u; i < info_.joints.size(); i++)
+    {
+      if (key == info_.joints[i].name + "/" + hardware_interface::HW_IF_POSITION)
+      {
+        start_modes_.push_back(hardware_interface::HW_IF_POSITION);
+      }
+      if (key == info_.joints[i].name + "/" + hardware_interface::HW_IF_VELOCITY)
+      {
+        start_modes_.push_back(hardware_interface::HW_IF_VELOCITY);
+      }
+    }
+  }
+
+  // Stopping interfaces
+  // add stop interface per joint in tmp var for later check
+  for (const auto & key : stop_interfaces)
+  {
+    for (auto i = 0u; i < info_.joints.size(); i++)
+    {
+      if (key == info_.joints[i].name + "/" + hardware_interface::HW_IF_POSITION)
+      {
+        stop_modes_.push_back(StoppingInterface::STOP_POSITION);
+      }
+      if (key == info_.joints[i].name + "/" + hardware_interface::HW_IF_VELOCITY)
+      {
+        stop_modes_.push_back(StoppingInterface::STOP_VELOCITY);
+      }
+    }
+  }
+
+  return ret_val;
+}
+
+return_type GenericSystem::perform_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;
+
+  position_controller_running_ = false;
+  velocity_controller_running_ = false;
+
+  if (
+    start_modes_.size() != 0 &&
+    std::find(start_modes_.begin(), start_modes_.end(), hardware_interface::HW_IF_POSITION) !=
+      start_modes_.end())
+  {
+    for (size_t i = 0; i < joint_commands_[POSITION_INTERFACE_INDEX].size(); ++i)
+    {
+      joint_commands_[POSITION_INTERFACE_INDEX][i] = joint_states_[POSITION_INTERFACE_INDEX][i];
+    }
+    position_controller_running_ = true;
+  }
+
+  if (
+    start_modes_.size() != 0 &&
+    std::find(start_modes_.begin(), start_modes_.end(), hardware_interface::HW_IF_VELOCITY) !=
+      start_modes_.end())
+  {
+    for (size_t i = 0; i < joint_commands_[VELOCITY_INTERFACE_INDEX].size(); ++i)
+    {
+      joint_commands_[VELOCITY_INTERFACE_INDEX][i] = 0.0;
+    }
+    velocity_controller_running_ = true;
+  }
+  return ret_val;
+}
+
 return_type GenericSystem::read()
 {
+  std::chrono::system_clock::time_point begin_last = begin;
+  begin = std::chrono::system_clock::now();
+  double period =
+    std::chrono::duration_cast<std::chrono::milliseconds>(begin - begin_last).count() / 1000.0;
+
   // 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 (
+      !std::isnan(joint_commands_[POSITION_INTERFACE_INDEX][j]) && !command_propagation_disabled_ &&
+      position_controller_running_)
     {
       joint_states_[POSITION_INTERFACE_INDEX][j] =
         joint_commands_[POSITION_INTERFACE_INDEX][j] +
         (custom_interface_with_following_offset_.empty() ? position_state_following_offset_ : 0.0);
+
+      if (info_.joints[j].state_interfaces.size() > 1)
+        joint_states_[VELOCITY_INTERFACE_INDEX][j] =
+          (joint_commands_[POSITION_INTERFACE_INDEX][j] - joint_pos_commands_old_[j]) / period;
     }
   }
+
+  // velocity
+  for (size_t j = 0; j < joint_commands_[VELOCITY_INTERFACE_INDEX].size(); ++j)
+  {
+    if (
+      !std::isnan(joint_commands_[VELOCITY_INTERFACE_INDEX][j]) && !command_propagation_disabled_ &&
+      velocity_controller_running_)
+    {
+      if (!position_controller_running_)
+      {
+        joint_states_[POSITION_INTERFACE_INDEX][j] +=
+          joint_commands_[VELOCITY_INTERFACE_INDEX][j] * period;
+        joint_commands_[POSITION_INTERFACE_INDEX][j] = joint_states_[POSITION_INTERFACE_INDEX][j];
+      }
+
+      joint_states_[VELOCITY_INTERFACE_INDEX][j] = joint_commands_[VELOCITY_INTERFACE_INDEX][j];
+    }
+  }
+
+  // remember old value of position
+  joint_pos_commands_old_ = joint_commands_[POSITION_INTERFACE_INDEX];
+
   // do loopback on all other interfaces - starts from 1 because 0 index is position interface
-  for (size_t i = 1; i < joint_states_.size(); ++i)
+  for (size_t i = 2; i < joint_states_.size(); ++i)
   {
     for (size_t j = 0; j < joint_states_[i].size(); ++j)
     {