Reading and writing to the same hardware interface

[jcarpinelli-bdai]

To implement a joint position controller (which accepts a topic input), we need to be able to read the current state of the robot. To do this, I added the "/position" interface to the configure_state_interfaces method, as shown here. When I do so, I get the following error.

If we cannot read and write to the same hardware interfaces, I think we can add one extra topic input to the controller which accepts the published robot state. Still, I think the optimal solution is reading the state directly from the hardware interface. Is this supported?

Launch Output

$ ros2 launch franka_bringup joint_position_example_controller.launch.py robot_ip:=${IP}

[spawner-5] [INFO] [1711125035.965050119] [spawner_franka_robot_state_broadcaster]: Configured and activated franka_robot_state_broadcaster
[spawner-7] [INFO] [1711125035.969229936] [spawner_joint_position_example_controller]: Configured and activated joint_position_example_controller
[spawner-4] [INFO] [1711125035.971297980] [spawner_joint_state_broadcaster]: Configured and activated joint_state_broadcaster
[ros2_control_node-3] terminate called after throwing an instance of 'franka::ControlException'
[ros2_control_node-3]   what():  libfranka: Move command aborted: motion aborted by reflex! ["joint_motion_generator_acceleration_discontinuity"]
[ros2_control_node-3] Stack trace (most recent call last) in thread 845:
[ros2_control_node-3] #19   Object "", at 0xffffffffffffffff, in 
[ros2_control_node-3] #18   Object "/usr/lib/x86_64-linux-gnu/libc.so.6", at 0x7ff0ad13f84f, in 
[ros2_control_node-3] #17   Object "/usr/lib/x86_64-linux-gnu/libc.so.6", at 0x7ff0ad0adac2, in 
[ros2_control_node-3] #16   Object "/usr/lib/x86_64-linux-gnu/libstdc++.so.6.0.30", at 0x7ff0ad33e252, in 
[ros2_control_node-3] #15   Object "/opt/ros/humble/lib/controller_manager/ros2_control_node", at 0x55c0f40f7800, in 
[ros2_control_node-3] #14   Object "/opt/ros/humble/lib/libcontroller_manager.so", at 0x7ff0ad74a2d4, in controller_manager::ControllerManager::read(rclcpp::Time const&, rclcpp::Duration const&)
[ros2_control_node-3] #13   Object "/opt/ros/humble/lib/libhardware_interface.so", at 0x7ff0acf4e69c, in hardware_interface::ResourceManager::read(rclcpp::Time const&, rclcpp::Duration const&)
[ros2_control_node-3] #12   Object "/opt/ros/humble/lib/libhardware_interface.so", at 0x7ff0acf6be8f, in hardware_interface::System::read(rclcpp::Time const&, rclcpp::Duration const&)
[ros2_control_node-3] #11   Object "/workspaces/bdai/_build/build/franka_hardware/libfranka_hardware.so", at 0x7ff092f27cb8, in franka_hardware::FrankaHardwareInterface::read(rclcpp::Time const&, rclcpp::Duration const&)
[ros2_control_node-3] #10   Object "/workspaces/bdai/_build/build/franka_hardware/libfranka_hardware.so", at 0x7ff092f441a4, in franka_hardware::Robot::readOnce()
[ros2_control_node-3] #9    Object "/workspaces/bdai/_build/build/franka_hardware/libfranka_hardware.so", at 0x7ff092f4557f, in franka_hardware::Robot::readOnceActiveControl()
[ros2_control_node-3] #8    Object "/usr/lib/libfranka.so.0.13.2", at 0x7ff092c91527, in franka::ActiveControl::readOnce()
[ros2_control_node-3] #7    Object "/usr/lib/libfranka.so.0.13.2", at 0x7ff092c8fde2, in franka::Robot::Impl::throwOnMotionError(franka::RobotState const&, unsigned int) [clone .cold]
[ros2_control_node-3] #6    Object "/usr/lib/x86_64-linux-gnu/libstdc++.so.6.0.30", at 0x7ff0ad3104d7, in __cxa_throw
[ros2_control_node-3] #5    Object "/usr/lib/x86_64-linux-gnu/libstdc++.so.6.0.30", at 0x7ff0ad310276, in std::terminate()
[ros2_control_node-3] #4    Object "/usr/lib/x86_64-linux-gnu/libstdc++.so.6.0.30", at 0x7ff0ad31020b, in 
[ros2_control_node-3] #3    Object "/usr/lib/x86_64-linux-gnu/libstdc++.so.6.0.30", at 0x7ff0ad304b9d, in 
[ros2_control_node-3] #2    Object "/usr/lib/x86_64-linux-gnu/libc.so.6", at 0x7ff0ad0417f2, in abort
[ros2_control_node-3] #1    Object "/usr/lib/x86_64-linux-gnu/libc.so.6", at 0x7ff0ad05b475, in raise
[ros2_control_node-3] #0    Object "/usr/lib/x86_64-linux-gnu/libc.so.6", at 0x7ff0ad0af9fc, in pthread_kill
[ros2_control_node-3] Aborted (Signal sent by tkill() 750 770001474)

[BarisYazici]

when the controller first start, we calculate need to calculate the trajectory based on the initial_q that we get in the beginning. This is partially a bug from our side. You can see an example of this in the libfranka as well https://github.com/frankaemika/libfranka/blob/master/examples/generate_joint_position_motion.cpp#L51.

So you need to keep the initial_q for the first calculation for your trajectory. You could still add another state_interface which has the position interface.
Try this sample:

controller_interface::InterfaceConfiguration
JointPositionExampleController::state_interface_configuration() const {
  controller_interface::InterfaceConfiguration config;
  config.type = controller_interface::interface_configuration_type::INDIVIDUAL;
  for (int i = 1; i <= num_joints; ++i) {
    config.names.push_back(arm_id_ + "_joint" + std::to_string(i) + "/" + k_HW_IF_INITIAL_POSITION);
    config.names.push_back(arm_id_ + "_joint" + std::to_string(i) + "/position");
  }
  return config;
}

controller_interface::return_type JointPositionExampleController::update(
    const rclcpp::Time& /*time*/,
    const rclcpp::Duration& /*period*/) {
  if (initialization_flag_) {
    for (int i = 0; i < num_joints; ++i) {
      initial_q_.at(i) = state_interfaces_[2 * i].get_value();
    }
    initialization_flag_ = false;
  }
  for(int i =0; i< num_joints; ++i){
    q_.at(i) = state_interfaces_[2*i + 1].get_value();
  }

  elapsed_time_ = elapsed_time_ + trajectory_period;
  double delta_angle = M_PI / 16 * (1 - std::cos(M_PI / 5.0 * elapsed_time_)) * 0.2;

  for (int i = 0; i < num_joints; ++i) {
    if (i == 4) {
      command_interfaces_[i].set_value(initial_q_.at(i) - delta_angle);
    } else {
      command_interfaces_[i].set_value(initial_q_.at(i) + delta_angle);
    }
  }

  return controller_interface::return_type::OK;
}

[jcarpinelli-bdai]

Thanks for your replies & explanation! I've tried a few iterations of the code you sent above. You do have to read from state_interfaces_[2*i] before you read from state_interfaces_[2*i+1]; if you don't you get a discontinuity ControlError.

When writing to hardware interfaces in a ROS2 controller, I typically don't think about the motion generator running "under the hood" in the arm's embedded system. My understanding of ROS2 control is that all of the implementation details below the hardware interface should be abstracted away from the controller level. Is there any way for this behavior to change? As it stands, when writing to the position interface, it looks like every controller needs to read from a hardware interface it doesn't explicitly need to satisfy the embedded system running underneath the hardware interface. If you are looking for some open-source support in the form of PRs, please let me know.

[BarisYazici]

sorry I didn't it get it completely. Did what I send you satisfy what you needed?
We are always open for contributions :)

[jcarpinelli-bdai]

sorry I didn't it get it completely. Did what I send you satisfy what you needed?

The instruction to read from the initial_joint_position interface if you want to write to the position interface, even if you don't need the initial_joint_position in your controller, does fix the issue I was experiencing. I could be wrong of course, but I think that this behavior is a slightly incorrect implementation of the ROS2 hardware interface, though. I don't think controllers should have to read from a state interface they don't need; all command interfaces should be ready to be written to after on_configure.

[BarisYazici]

No you don't always need the initial_joint_position unless you command the robot. You can remove the commanding section and just read the state part.

controller_interface::InterfaceConfiguration
JointPositionExampleController::state_interface_configuration() const {
  controller_interface::InterfaceConfiguration config;
  config.type = controller_interface::interface_configuration_type::INDIVIDUAL;
  for (int i = 1; i <= num_joints; ++i) {
    config.names.push_back(arm_id_ + "_joint" + std::to_string(i) + "/position");
  }
  return config;
}

controller_interface::return_type JointPositionExampleController::update(
    const rclcpp::Time& /*time*/,
    const rclcpp::Duration& /*period*/) {

  for(int i =0; i< num_joints; ++i){
    q_.at(i) = state_interfaces_[i ].get_value();
  }


  return controller_interface::return_type::OK;
}

[jcarpinelli-bdai]

Yes agreed. I think being required to read from initial_joint_position if you want to write to position is an incorrect implementation of the ROS2 control / hardware interfaces.

[BarisYazici]

when the controller first start, we calculate need to calculate the trajectory based on the initial_q that we get in the beginning. This is partially a bug from our side

You are correct, I mentioned this here.

[jcarpinelli-bdai]

Sorry, I'm surprised this issue was closed. Does the team plan to fix the issue so that franka_ros2 complies with ROS2's controller and hardware interfaces? If so, should I make a separate issue for the interface bug?

[BarisYazici]

Sorry for not being clear. This bug is not on the franka_ros2 or in the FCI side. And this should not prevent you from writing your joint position controller. If you need more help, you could share your code. We can try to fix your issue.

[BarisYazici]

Btw you can always activate the rate limiter and low pass filter to avoid ["joint_motion_generator_acceleration_discontinuity"] errors ->

franka_ros2/franka_hardware/include/franka_hardware/robot.hpp

Line 316 in 40cf08b

bool joint_position_command_rate_limit_active_{false};