frankaemika/franka_ros

Access model library from multiple ROS nodes at the same time

Closed this issue · 5 comments

kmerckae commented

I would like to create a planning-navigation-control pipeline with multiple ROS nodes running at different frequencies for each of the layers in the pipeline. The navigation layer relies on the predicted/simulated forward kinematics and dynamics over a fixed time horizon of the Panda robot, i.e. it needs to receive the mass matrix, Coriolis vector, gravity vector for some specific joint angles and velocities. How to do this?

  • I tried to make standard ROS nodes and included the franka_hw::FrankaModelInterface in the node where I wanted to get access to the franka model library and its functions, but this does not work. Why is this not possible?

  • I only seem to get access to the model library when making a controller class (similar to the franka_example_controllers) with

    public controller_interface::MultiInterfaceController<
                           franka_hw::FrankaModelInterface,
                           franka_hw::FrankaStateInterface>

    But the update rate of these controller classes is 1kHz and my navigation layer and planning layer should run at a lower frequency, e.g. 100Hz and 10Hz, respectively. Where to change this update rate?

  • Another solution I thought about is to use libfranka to get access to the model library in the navigation layer. I noticed that when I made multiple ROS nodes with the libfranka includes that I could access in all the nodes the kinematic and dynamic model parameters. However, the moment I also launched the franka_control node, I got a franka::NetworkException error.

    |571x312

What is the best way to get access to the franka model library in multiple ROS nodes?

Thanks in advance!

Kind regards,

Kelly

gollth commented

Hi @kmerckae

thanks for your interest in our robot and the FCI.

I tried to make standard ROS nodes and included the franka_hw::FrankaModelInterface in the node where I wanted to get access to the franka model library and its functions, but this does not work. Why is this not possible?

This has architectural reasons. We "abusing" a bit the ROS control hardware interfaces to offer the users access to allow model calculations in realtime. Without accessing the model via ROS control, we cannot guarantee that performance.

Where to change this update rate?

Short answer: You can't! Long answer: This is again because of the realtime constraint as described in the first point.

the moment I also launched the franka_control node, I got a franka::NetworkException error.

This is a bit hard to trace what the exact problem is. Looking at the error message it seems that you are not fulfilling a real-time constraint. Are you sure your control loop is sending commands every 1ms? Is something blocking your control loop or overloading your network? Maybe you can share some launch file we can better understand what is going on.

What is the best way to get access to the franka model library in multiple ROS nodes?

My suggestion would be to write a custom "read-only" controller inheriting from the FrankaModelInterface extracting the information you need and offering them via a ROS topic/service/action to your nodes. This way you can also reduce the frequency

kmerckae commented

Hi @gollth,

Thanks for your answer!

This is a bit hard to trace what the exact problem is. Looking at the error message it seems that you are not fulfilling a real-time constraint. Are you sure your control loop is sending commands every 1ms? Is something blocking your control loop or overloading your network? Maybe you can share some launch file we can better understand what is going on.

The problem was that I launched multiple nodes that accessed the robot state and model via libfranka, whereafter I launched a franka control node for sending commands to the robot. Once the franka control node was launched, I got that franka::NetworkException error. Apparently this problem appeared because only a single connection per robot can be made and in my case, the franka_control node "noticed" another connection.

My suggestion would be to write a custom "read-only" controller inheriting from the FrankaModelInterface extracting the information you need and offering them via a ROS topic/service/action to your nodes. This way you can also reduce the frequency

With that "read-only" controller, you mean a basic ROS node instead of a franka_control node that inherits from the 'FrankaModelInterface"? With franka_ros or pure libfranka?

kmerckae commented

In the meantime I also received an answer from Christian of the Franka support team.
He said the following.

You can also get the model via libfranka.
My colleagues had the following idea as a quick hack to achieve this:
Quickly connect to libfranka to get the model and then use the model in ROS.

Below the more elaborated answer.

libfranka was built to use one model instance per robot. So what you are trying to achieve is not the typical use case. Nonetheless, we hope we can help you a bit further with the following information:

  1. What do you mean with "quickly connect to libranka"? Do you mean the code franka::Robot robot(<robot_ip>)? How programming-wise do this "quickly"?
  2. I can get a Franka model instance via franka::Model model(robot.loadModel()). Is it possible to save this instance globally? If so, how to do this in the main function of a generic ROS node?

    About point 1) and 2), you got the right idea. About how to use this in a ROS node, we do not have a specific recommendation on what is the best approach to this. Perhaps you write a separate node for that.

  3. How to disconnect from libfranka programming-wise? Which libfranka function to call?

    You can simply disconnect by letting the object "robot" go out of scope.

  4. Is it possible to use the pose, mass, gravity, and coriolis functions from the franka::Model Class when disconnected from libfranka on the saved Franka model instance?

    As long as the model object is "alive" you can access these parameters.

  5. If I understand it well, you propose a solution where I first launch some ROS nodes that receive the Franka model via libfranka. Once they have the model, they all disconnect from libfranka. When they are all disconnected from libfranka, it is possible to launch a franka_control node to send commands to the robot.

    Exactly.

I have tried this and I don't get any errors anymore, so this seems a good solution!

gollth commented

Hi @kmerckae,

glad that Chris could help you. The solutions he suggested is the standard way of using the FCI (i.e. one franka::Robot instance per connection). Since you opened this issue in in franka_ros and not libfranka, let me add how you could achieve the same in franka_ros:

With that "read-only" controller, you mean a basic ROS node instead of a franka_control node that inherits from the 'FrankaModelInterface"? With franka_ros or pure libfranka?

Not exactly a "basic ROS node" but rather a ROS controller which is a plugin dynamically loaded into the controller manager node (a.k.a. franka_control).

To create such "read-only" controller do something like this:

#include <memory>
#include <string>

#include <controller_interface/controller_base.h>
#include <controller_interface/multi_interface_controller.h>
#include <hardware_interface/robot_hw.h>
#include <pluginlib/class_list_macros.h>
#include <ros/node_handle.h>
#include <ros/time.h>
#include <std_srvs/Trigger.h>

#include <franka_hw/franka_model_interface.h>
#include <franka_hw/franka_state_interface.h>

class MyReadOnlyController
    : public controller_interface::MultiInterfaceController<franka_hw::FrankaStateInterface,
                                                            franka_hw::FrankaModelInterface> {
  // Hold the state & model handle as members of your controller
  std::unique_ptr<franka_hw::FrankaStateHandle> fsh;
  std::unique_ptr<franka_hw::FrankaModelHandle> fmh;

  ros::ServiceServer service;

  bool init(hardware_interface::RobotHW* robot_hw,
            ros::NodeHandle& root_node_handle,
            ros::NodeHandle& controller_node_handle) override {
    auto fsi = robot_hw->get<franka_hw::FrankaStateInterface>();
    auto fmi = robot_hw->get<franka_hw::FrankaModelInterface>();

    // Error handling omitted for brevity
    fsh = std::make_unique<franka_hw::FrankaStateHandle>(fsi->getHandle("panda_robot"));
    fmh = std::make_unique<franka_hw::FrankaModelHandle>(fmi->getHandle("panda_model"));

    // Offer the functionality you need to the outside world, e.g. via a service:
    service =
        root_node_handle.advertiseService<std_srvs::Trigger::Request, std_srvs::Trigger::Response>(
            "my_model_function", [&](auto& request, auto& response) {
              // Do something here with the model library, e.g. getting the mass matrix.
              // Probably you also want your own service type
              auto M = fmh->getMass();
              response.message = "M[0,0] = " + std::to_string(M.at(0));
              response.success = true;
              return true;
            });

    return true;
  }

  void update(const ros::Time& time, const ros::Duration& period) override {
    // This is the realtime loop of your controller. Since its read-only, you can simply do nothing
    // here. Note though, that you want to avoid blocking calls here, since this can trigger
    // communication_constraints_reflex if the update loop is too slow
  }
};

PLUGINLIB_EXPORT_CLASS(MyReadOnlyController, controller_interface::ControllerBase)

Don't treat this as perfect code please, it should just give you an idea to get started. Behind the scenes a very similar thing happens to the libfranka solution. With robot_hw->get<franka_hw::FrankaModelInterface>() you also claim the model library and hold it in memory (member of the controller), to offer its functionality to other parts of your system (e.g. via a ROS service).

Make sure to properly register your controller to pluginlib.

See also ModelExampleController for more details.

I hope this helps

  • 👍1
gollth commented

I have tried this and I don't get any errors anymore, so this seems a good solution!

Great to hear. I will close this ticket then. Feel free to re-open anytime if issue persists