[GenericSystem] Add velocity-only mode in fake_components #540

livanov93 · 2021-09-21T18:01:33Z

PR info:

Add disable_command flag to be able to simulate disconnected driver
Add prepare_command_mode_switch and perform_command_mode_switch for switching between velocity-only and position-only controllers
Add first-order integration of velocity for position calculation in velocity mode

destogl

As per call.

destogl · 2021-09-24T07:57:12Z

hardware_interface/include/fake_components/generic_system.hpp

+enum StoppingInterface
+{
+  NONE,
+  STOP_POSITION,
+  STOP_VELOCITY
+};
+


Suggested change

enum StoppingInterface

{

NONE,

STOP_POSITION,

STOP_VELOCITY

};

destogl · 2021-09-24T08:05:28Z

hardware_interface/src/fake_components/generic_system.cpp

+  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);
+      }
+    }


Suggested change

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);

}

}

hardware_interface::return_type ret_val = hardware_interface::return_type::OK;

// Currently we don't accept ACCELERATION-only and EFFORT-only interfaces

for (const auto & key : start_interfaces)

{

for (auto i = 0ul; i < info_.joints.size(); i++)

{

if (key == info_.joints[i].name + "/" + hardware_interface::HW_IF_ACCELERATION ||

key == info_.joints[i].name + "/" + hardware_interface::HW_IF_EFFORT)

{

ret_val = false;

}

}

return ret_val;

Also not allowed if velocity is missing and there are only ACC and POS interfaces.

destogl · 2021-09-24T08:19:33Z

hardware_interface/src/fake_components/generic_system.cpp

+  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;


Suggested change

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;

hardware_interface::return_type ret_val = hardware_interface::return_type::OK;

// class variable

std::vector<std::vector<bool>> used_interface_per_joint_[interface_type][joint];

// set everything to false first

for (const auto & interface : start_interfaces)

{

joint_name = split (interface.split("/")[0])

interface_name = split (interface.split("/")[1])

auto found_it = std::find(standard_interfaces_.begin(), standard_interfaces_.end(), interface_name)

if (found_it != standard_interfaces_.end())

{

auto pos = std::dist(standard_interfaces_.begin(), found_it);

auto found_joint = std::find_if(info_.joints.being(), info_.joints.end(), [&](const auto & joint) { return joint.name == joint_name; });

auto joint_pos = std::dist(info_.joints.begin(), found_joint);

used_interface_per_joint_[dist][joint_pos] = true;

}

return true;

destogl · 2021-10-25T18:44:19Z

hardware_interface/include/fake_components/generic_system.hpp

+  std::vector<uint> stop_modes_;
+  std::vector<std::string> start_modes_;


I don't understand the nomenclature here exactly. Why do we need this?

destogl · 2021-10-25T18:46:35Z

hardware_interface/include/fake_components/generic_system.hpp

+  bool position_controller_running_;
+  bool velocity_controller_running_;


Why not to use one variable and use indexes defined in the above constants (line 73 and 74)?

Suggested change

bool position_controller_running_;

bool velocity_controller_running_;

size_t controller_running_;

destogl · 2021-10-25T18:48:23Z

hardware_interface/include/fake_components/generic_system.hpp

+  std::vector<std::string> start_modes_;
+  bool position_controller_running_;
+  bool velocity_controller_running_;
+  std::chrono::system_clock::time_point begin;


Class member:

Suggested change

std::chrono::system_clock::time_point begin;

std::chrono::system_clock::time_point begin_;

hardware_interface/src/fake_components/generic_system.cpp

destogl · 2021-10-25T18:52:28Z

hardware_interface/src/fake_components/generic_system.cpp

@@ -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_);
+


Suggested change

destogl · 2021-10-25T18:56:10Z

hardware_interface/src/fake_components/generic_system.cpp

+  const std::vector<std::string> & /*start_interfaces*/,
+  const std::vector<std::string> & /*stop_interfaces*/)


This probably shouldn't be commented out and then we dont' have to store interfaces for starting and stopping.

destogl · 2021-10-25T18:57:42Z

hardware_interface/src/fake_components/generic_system.cpp

+
+  // 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


Comment needs update

mergify · 2022-02-11T08:14:39Z

This pull request is in conflict. Could you fix it @livanov93?

mergify · 2022-09-24T06:44:26Z

This pull request is in conflict. Could you fix it @livanov93?

mergify · 2023-03-20T11:40:50Z

This pull request is in conflict. Could you fix it @livanov93?

livanov93 added 8 commits September 21, 2021 13:10

Add disabling command propagation option.

a217ad4

Make velocity work.

1035cae

Remove debugg stuff.

c9dc3dd

Additional changes for velocity ctrl.

65216c9

Set initial velocity to zero.

f671675

Test with parenthesis.

c57bdbc

Adapt fake component to use velocity control.

f0788f4

Merge with resolved changes.

a47424b

livanov93 mentioned this pull request Sep 21, 2021

Extending demo tests ros-controls/ros2_control_demos#136

Closed

Add option to use velocity and position mode at the same time.

a21d522

destogl reviewed Sep 24, 2021

View reviewed changes

destogl reviewed Nov 12, 2021

View reviewed changes

destogl mentioned this pull request Jan 19, 2023

Mocking: map command interface to state interface of different types #885

Closed

This was referenced May 17, 2023

[MockHardware] Enable disabling of command to simulate HW failures. #1027

Merged

[MockHardware] Added dynamic simulation functionality. #1028

Merged

bmagyar closed this May 26, 2023

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

[GenericSystem] Add velocity-only mode in fake_components #540

[GenericSystem] Add velocity-only mode in fake_components #540

livanov93 commented Sep 21, 2021

destogl left a comment

destogl Sep 24, 2021

destogl Sep 24, 2021

destogl Sep 24, 2021

destogl Sep 24, 2021

destogl Oct 25, 2021

destogl Oct 25, 2021

destogl Oct 25, 2021

destogl Oct 25, 2021

destogl Oct 25, 2021

destogl Oct 25, 2021

mergify bot commented Feb 11, 2022

mergify bot commented Sep 24, 2022

mergify bot commented Mar 20, 2023

		std::vector<uint> stop_modes_;
		std::vector<std::string> start_modes_;

		bool position_controller_running_;
		bool velocity_controller_running_;

	bool position_controller_running_;
	bool velocity_controller_running_;
	size_t controller_running_;

	std::chrono::system_clock::time_point begin;
	std::chrono::system_clock::time_point begin_;

		@@ -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_);

		const std::vector<std::string> & /start_interfaces/,
		const std::vector<std::string> & /stop_interfaces/)

[GenericSystem] Add velocity-only mode in fake_components #540

[GenericSystem] Add velocity-only mode in fake_components #540

Conversation

livanov93 commented Sep 21, 2021

destogl left a comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

mergify bot commented Feb 11, 2022

mergify bot commented Sep 24, 2022

mergify bot commented Mar 20, 2023