Robot is forced to 0rad joint positions once hardware interface establishes EGM connection

Yadunund commented 1 year ago

Hello @stephanie-eng and @gavanderhoorn,

I've noticed a potentially serious problem with the behavior of this hardware interface which is summarized in the video below. I've detailed the cause and a temporary fix i'm using. But would like to get your thoughts on what a proper fix should look like.

The problem

Say I move the ABB robot to a non-zero joint configuration before starting the EGM client on the robot. In this example, joints 1-3 are set to 30deg or 0.5235rad. You can see the console output in RobotStudio where the robot is waiting to connect to the EGM server.

I then launch the hardware_itnerface and joint_trajectory_controller following the exact command from the README but after replacing the IP address with that of my robot. You can see that as soon as the EGM server starts and registers the client, it moves the robot to all zero joint positions. This is even after I've explicitly set the initial position values to 0.5235rad of the resp state interfaces in the irb1200.ros2_control.xacro file (bottom terminal).

https://user-images.githubusercontent.com/13482049/202637512-793f3710-ccf0-4def-97ff-550847cde039.mp4

The behavior is dangerous as our robot operates within an enclosure with many obstacles which may collide with it as it moves to 0 joint value configuration on startup. The expected behavior is for the hardware_interface to not move the robot to this configuration at start but instead keep the robot at the initial value specified in the ros2 control xacro.

Root cause of the problem.

The problem lies in the way the initializeMotionData that is called by this hardware interface as seen here is implemented. The implementation hardcodes motion_joint.command.position = 0.0;. Hence, once EGM connects, it commands all the joints to 0.0 rad.

Temporary fix

As a interim solution, i'm using a fork of abb_egm_rws_managers where I hardcode the motion_joint.command.position values in initializeMotion() with initial joint values that my robot starts in. In the video below, you can see that once the EGM server connects with the robot client, it does not command the robot joints to 0rad and i'm able to plan and execute motions from this initial position of the robot.

https://user-images.githubusercontent.com/13482049/202643300-43ae71e0-ec4d-4109-8280-aa157cfe572a.mp4

Proper fix

Here's my proposal for a more general fix. If this sounds ok, I'm happy to open the necessary PRs.

Get and store the initial_values for each joint from the hardware_interface::HardwareInfo passed into AbbSystemHardware::on_init()
Pass these initial values to initializeMotion() and use the values to set motion_joint.command.position for each joint. Let me know your thoughts on this approach. I can try to do this without breaking any API.

On a slightly unrelated note, I would also like to propose not using RWS to get the RobotControllerDescription as seen here. This works well for RWS1.0 but when interfacing with RWS2.0 on omnicore, it fails. I've resorted to manually creating the RobotControllerDescription object as described in this comment when working with omnicore robots. I think it should be possible construct this description by parsing the HardwareInfo. Or atleast have an option to either use RWS or parse HardwareInfo to get this information. I understand that getting this data from RWS is useful when there are external axes involved.

gavanderhoorn commented 1 year ago

I understand why you @-mention me, but I'm not involved in this development at all. PickNik decided to create this driver without anyone else's involvement.

Having written that: https://github.com/UniversalRobots/Universal_Robots_ROS2_Driver/issues/520. This may actually be a ros2_control problem, or a problem with the way people are writing their hardware interfaces.

I would also like to propose not using RWS to get the RobotControllerDescription as seen here. This works well for RWS1.0 but when interfacing with RWS2.0 on omnicore, it fails. I've resorted to manually creating the RobotControllerDescription object as described in this comment when working with omnicore robots. I think it should be possible construct this description by parsing the HardwareInfo. Or atleast have an option to either use RWS or parse HardwareInfo to get this information.

This doesn't seem like a good idea to me.

The ABB controller basically holds the 'absolute truth', in the sense that there's no point in letting users define anything, as EGM (and RWS) will not be able to accept anything which doesn't correspond to how the actual controller/robot is configured.

I understand that getting this data from RWS is useful when there are external axes involved.

No, it's actually useful always. Letting users define this data has led to problems in the past, which is why abb_robot_driver does it the way it's currently implemented.

Yadunund commented 1 year ago

@gavanderhoorn apologies for the tag. I figured I'd get your inputs too since my proposal involves modifying abb_egm_rws_managers that you maintain.

Having written that: https://github.com/UniversalRobots/Universal_Robots_ROS2_Driver/issues/520. This may actually be a ros2_control problem, or a problem with the way people are writing their hardware interfaces.

Given the temporary fix I described and tested above, it feels like the problem also lies in the way the hardware_interfaces is written. From the ros2 controllers perspective, an initial value was provided in the xacro but the hardware interface is commanding the robot to go to 0rad on startup. Anyways, thanks for highlighting the similar problem with the UR driver. Will monitor that ticket as well.

No, it's actually useful always. Letting users define this data has led to problems in the past, which is why abb_robot_driver does it the way it's currently implemented.

I agree that relying on RWS provides an absolute source of truth and this can be the default mechanism. But I think having an option to bypass this and rely on the control xacro may be beneficial for advanced users who are aware and accepts the risk involved. I also can see how this could lead to more problems being reported when users misconfigure the xacro.

gavanderhoorn commented 1 year ago

This is largely off-topic here, but:

But I think having an option to bypass this

which exists, as that's why a service can be used to provide a different description.

I also can see how this could lead to more problems being reported when users misconfigure the xacro.

I may be a bit pessimistic (probably due to years of supporting robot drivers), but as the accepted/default flow-of-control for drivers based on ros2_control is to consider the xacro as the source-of-truth, adding such an override/backdoor doesn't seem like a good idea.

Documented or not, you will get users reporting problems and when it turns out they are caused by forcing the xacro to be used the response will likely be "but they do it everywhere else in ros2_control ..".

abb_robot_driver actually also checks whether the URDF you're trying to use corresponds to what the controller reports. As EGM basically doesn't check anything (and RWS only up to a certain level), I personally appreciate it's a bit defensive about these sort of things.

Yadunund commented 1 year ago

I agree with your points. Happy to discuss this in a separate ticket as I should not have raised this other issue in the same ticket. Will leave the focus here to be the joint commands issued by the hardware_interface on startup.

stephanie-eng commented 1 year ago

Thanks for the find @Yadunund - your proposed fix w.r.t. the joint position initialization sounds good to me, and it would be greatly appreciated if you could open PR's for this!

Yadunund commented 1 year ago

Revelant PRs to fix this issue:

PickNikRobotics / abb_ros2