search

frankaemika / franka_ros

ROS integration for Franka research robots
https://frankaemika.github.io
Apache License 2.0
350 stars 307 forks source link

franka_control_node incorrectly handles controller starting #232

Open rhaschke opened 2 years ago

rhaschke commented 2 years ago
  1. Trying to start a ros_control controller when the robot is not in automatic mode (external activation device pushed down), franka_control_node correctly rejects the operation: [ERROR] [ros.franka_control]: libfranka: Move command rejected: command not possible in the current mode! But: the controller is reported as started, e.g. in rqt's controller manager plugin.
  2. If the robot is switched back to automatic mode (external activation device pulled up), starting a controller is still not possible:
    • franka_control_node doesn't report an error (anymore)
    • the controller is reported to be active in rqt, but the controller isn't actually running!
  3. To enable starting a controller, one needs to re-launch franka_control_node, which is rather annoying if it is part of a larger roslaunch file.
rpapallas commented 2 years ago

Hi @rhaschke do you have any update on this? We have the same problem.

We do the following:

It appears that the position_joint_trajectory_controller starts (as the appropriate topics appear), but we also get that error: [ERROR] [ros.franka_control]: libfranka: Move command rejected: command not possible in the current mode!.

We don't know if it's related, but when using MoveIt! to move the arm, MoveIt reports: "Controller 'position_joint_trajectory_controller' failed with error GOAL_TOLERANCE_VIOLATED:" even though MoveIt! finds a trajectory.

The same MoveIt! configuration works out-of-the-box when using Gazebo, so I doubt that it's a moveit issue.

rhaschke commented 2 years ago

@rhaschke do you have any update on this?

No, there is no update on this from @frankaemika.

The error message command not possible in the current mode is due to the arm not being in automatic control mode. You should release the external activation push button.

The problem is not MoveIt related. GOAL_TOLERANCE_VIOLATED is an error thrown by the controller, when the target position couldn't be reached.

rpapallas commented 2 years ago

Thanks. You are right about the activation button. Our problem with MoveIt was networking-related. We had the control connected to a router and from there to the real-time operating system. It turns out that the RTT was high enough to cause these errors. Taking the router out of the equation solved our problem.

We used the delayed_spawn_controller.py to spawn controller and we don't get any errors any more.

marcbone commented 2 years ago

We used the delayed_spawn_controller.py to spawn controller and we don't get any errors any more.

You are using it for the real robot? What problem is it solving for you? It was only meant as a hack to make controllers spawn properly in gazebo

rpapallas commented 2 years ago

Hi @marcbone, I assumed that the franka_control.launch might take some time to start everything up. But do you suggest that this won't be a problem? We are using the rea robot yes, with a custom launch file that starts the franka_control.launch and then spawns the trajectory joint position controller.

marcbone commented 2 years ago

The controllers are waiting until the robot ready, anyways. The problem in gazebo was, that they spawn the robot to the zero pose and only then change to the desired initial position that was defined in the launch file. However, sometimes a controller starts directly when the robot is spawned and still in his zero pose. But this cannot happen on the real robot. I mean there should be no harm in using the delayed_spawn_controller, but I wouldnt use it if you are not working in gazebo.

Back to the original issue. You will get " libfranka: Move command rejected: command not possible in the current mode!" if the user-stop is pressed (robot base led is white), then you have to "unpress" the user-stop (robot base led will turn blue), or if you had an error before, then you have to press and then unpress the user-stop button. I am closing this issue

rhaschke commented 2 years ago

@marcbone, in my opinion, the original issue isn't solved yet. My issue was about:

Please reopen.

marcbone commented 2 years ago

I just noticed, that it is possible to activate multiple controllers (e.g. effort and position), which doesn't make sense.

Interesting. Just tried this and can confirm. Seems like the libfranka exceptions do not trigger the stop of a controller. Also, the prepare_switch method seem to not enforce using only one interface at a time

rhaschke commented 2 years ago

Is there any progress on this issue? I shortly looked into this today and found the following:

I just noticed, that it is possible to activate multiple controllers (e.g. effort and position), which doesn't make sense.

Running up to two controllers in parallel is explicitly allowed by FrankaHW::checkForConflict: https://github.com/frankaemika/franka_ros/blob/54ab726ba2eea483332b4cafd783eeee487da432/franka_hw/src/resource_helpers.cpp#L140-L143

However, I don't really understand why. I have seen several controller combinations being supported in setRunFunction(), but JointTorque + JointPosition doesn't actually work in practice: only the effort controller accepts commands. The position controller doesn't.

BolunDai0216 commented 1 year ago

@rhaschke do you have any update on this?

No, there is no update on this from @frankaemika.

The error message command not possible in the current mode is due to the arm not being in automatic control mode. You should release the external activation push button.

The problem is not MoveIt related. GOAL_TOLERANCE_VIOLATED is an error thrown by the controller, when the target position couldn't be reached.

@rhaschke We have the same problem

libfranka: Move command rejected: command not possible in the current mode ("User stopped")!

when running the libfranka examples. I tried both pressing the external activation control and not none of them work. Is there a way that I can figure out what mode I am in? And are there any other ways to make the robot in automatic control mode?

Thanks in advance!

When running ./example/echo_robot_state <fci-ip> I get the result:

"control_command_success_rate": 0, "robot_mode": "User stopped", "time": 4004474

while in the documentation, the robot_mode is "Idle". How can I change this?

rhaschke commented 1 year ago

Is there a way that I can figure out what mode I am in?

Actually, the error message was already improved to include the current mode. In your case "User stopped" indicates that you pressed the user/emergency stop.

rhaschke commented 1 year ago

Ping @marcbone, is there any update on the points raised in https://github.com/frankaemika/franka_ros/issues/232#issuecomment-1224266971?

BolunDai0216 commented 1 year ago

Is there a way that I can figure out what mode I am in?

Actually, the error message was already improved to include the current mode. In your case "User stopped" indicates that you pressed the user/emergency stop.

@rhaschke Thanks for the response!

But I don't think the emergency stop is pressed. The lights on the robot was flashing green.

Are there any other reasons that may cause the mode to be "User stopped"?

marcbone commented 1 year ago

Ping @marcbone, is there any update on the points raised in #232 (comment)?

Sry for the late reply. Sadly, there are currently no updates on this, but we have this ticket in our backlog. Without looking into the code, my guess is that it is allowed to have 2 interfaces because you want to be able to have the state interface and an additional controller. I am aware the the controller is not properly crashing and that this needs to be fixed.

marcbone commented 1 year ago

@BolunDai0216

You have a entire different problem. Are you sure that the user stop is not pressed? The base led of the robot should be solid blue. If this is not the problem please write a mail to the support team (support@franka.de), instead of replying here

BolunDai0216 commented 1 year ago

@BolunDai0216

You have a entire different problem. Are you sure that the user stop is not pressed? The base led of the robot should be solid blue. If this is not the problem please write a mail to the support team (support@franka.de), instead of replying here

Yeah, I found the issue. I had to disable the safety rules in watchman.

Maverobot commented 1 year ago

I have seen several controller combinations being supported in setRunFunction(), but JointTorque + JointPosition doesn't actually work in practice: only the effort controller accepts commands. The position controller doesn't.

You are right. Only the effort controller accepts commands. However, the information sent by JointPosition also reaches the robot which will for example effect the O_T_EE_d in the next time step. It can be useful for users who can accept the one step delay and don't want to do their own forward kinematics.

Maverobot commented 1 year ago

I guess this issue was closed automatically by github when merging #279 due to the "fixes #232" in the PR description. I will reopen it now.