Problem at during switching controllers

[DongjuSin]

Hi, thanks for your great work!

Recently I'm struggling with a problem related to switching controller.

As you can see, in below codes,

1. we use position controller to move the robot to initial position.
2. send torque commands for certain time steps.
3. if robot is disabled during torque commands execution, enable the robot and move it to initial position again.

However, an error occurs when the robot enabled again. After robot is enabled, it tries to swtich a controller from 'franka_ros_interface/effort_joint_torque_controller' to 'position_joint_trajectory controller'. A torque controller is successfully stopped, but position_joint_trajectory_controller exists in the active controller list. This causes an assertion error because only one controller should be in active controller list.

In the terminal that we launched interface.launch we observed that it tries to switch controller from 'effort_joint_torque_controller' to ''. We couldn't find why this happens.

Thanks in advance.

[justagist]

Hi! This problem will mostly be fixed if you put a delay (1 or 2 sec) before and/or after re-enabling the robot. The issue here is mostly because, when the robot is disabled, the controller interface node switches from torque controller to the safer position controller as it doesn't receive torque commands at the required rate (this is done for safety). This causes it to load the position controller again. Meanwhile, the interface API sees that the position controller is again loaded and the assertion that only one controller should be loaded fails. By putting a slight delay between the disabling of robot and calling move_to_joint_position, the controller manager is refreshed and the ArmInterface object will see that it already has the position controller loaded in its controller manager, and will not attempt to load it again.

[DongjuSin]

Thanks, I will try what you suggest.

[DongjuSin]

We added a 2 seconds delay after enabling the robot, and it solved the problem for most of the cases.

However, in some case the problem still existed. Actually we could solve the problem by changing the order of control command('exec_torque_cmd' and 'move_to_joint_position') and checking robot status( function 'is_enabled_robot'). Previously, we checked a status of the robot after we send a control command, now it is flipped. As a result, operation of the robot is very stable now.

So I'm wondering about why the order of these code lines affects much in real robot execution. Is there a proper way to use a torque command? It seems these is no example code using torque controller in your libraries('panda_robot' and 'franka_ros_interface'). It would be very helpful if we can get some example codes! Thanks in advance :)

[justagist]

Glad you fixed the issue! There is no specific way to use the torque command. All you would have to do is send the command at a reasonably high frequency for smooth motion (an example is in the panda_simulator package; I would not recommend using it directly on the real robot as it is, but you should be able to easily change some values in the code for the real robot; you might have to change the controller gains and maybe increase the controller thread frequency when using on the real robot!). The issues you face are because you are asking the interface to switch controllers before the internal motion controller interface stops the torque controller and starts the position controller on its own (for safety). For safety, I had written the controller interface node to switch to position control if it does not continuously receive torque (or velocity) commands within the specified timeout (default: 0.2s) once a command is sent. So when your robot fails and you stop sending torque commands, it will wait 0.2 seconds and then switch to position control. But you may be asking the robot interface to change to position control before the internal controller interface completes its switching. You can try reducing this timeout to make the controller_interface revert to position controller quicker using the set_command_timeout method in the API. This also means you have to be able to send the torque commands within that time. By changing the order of the code lines, you are giving the internal controller interface more time to switch to position control. This could be why it works now.