Position Command Interface Sensitivity & Control Exceptions

[jcarpinelli-bdai]

We have tried to use the position command interface, but found strange motion generation exceptions. We posted to #52, and learned about the libfranka bug which requires reading from initial_joint_position. After applying that fix, we still have been unable to use the position command interface, and I believe we now have a repeatable demo which shows this issue. Commanding any change in (initial) position with a magnitude of 1e-5 radians or larger results in motion generation and control exceptions. This sensitivity is preventing us from using the position interface at all, unfortunately. This change in position, 0.00001 rad, seems to be well within the acceleration limits specified in the libfranka documentation.

I have forked humble, and changed the relevant line in joint_position_example_controller.cpp to highlight this issue. If you run that example, with my modification applied, you should immediately see a Franka control exception without any arm motion.

https://github.com/jcarpinelli-bdai/franka_ros2/commit/8ed4d99e90f3cec43eaf3a11d7262406dd987516

[TomCC7]

Hi, I'm also trying to use the position command interface, but the default joint_position_example_controller example was already not working for me and throws the error:

libfranka: Move command aborted: motion aborted by reflex! ["joint_motion_generator_velocity_limits_violation", "joint_motion_generator_velocity_discontinuity", "joint_motion_generator_acceleration_discontinuity"]

Have you ever met this condition before? I'm using version humble for franka_ros2 and the fr3-develop branch of libfranka. Any information will be helpful, thanks!

[hagarces]

Hi @jcarpinelli-bdai , today I was migrating my joint position controller from ROS1 to ROS2 and I have literally the same problem as you, thanks to you and the issue #52 I learned about the libfranka bug, and I still cant find a way to make the controller work, if i solve it i will make you know.

[BarisYazici]

I will try to explain what's exactly happening under the hood.

With this latest release in franka_ros2(https://github.com/frankaemika/franka_ros2/releases/tag/v1.0.0), you can directly start joint position examples from the current joint positions. From the example controllers you have access to the current joint position. You can send this as your initial command to the motion generators. And in the next cycle send another joint position command inside the discontinuity limits. The delta here depends on the desired joint position and calculated joint velocity and joint positions for your command.

A general intro to the signal discontinuities happening in our robot:

Discontinuities are calculated based on rate of change wrt. the control frequency 1ms. Even sending the same position without any delta can cause a joint velocity or joint acceleration discontinuity. For instance, robot arm is in motion with high velocity and suddenly in one cycle user sends the same joint position as the previous one. This forces the robot to stop in the current position in a matter of 1ms. So, this means you are sending 0.0 velocity to robot in that next cycle. Robot could be going at 2.5m/s and suddenly and user wants it to stop in 1ms. This causes the required joint accelerations to jump. And we don't want to send suddenly high accelerations to robot joints to avoid any damage to the joints. That's why we are escalating to discontinuity errors. This is a scenario even with the same position, literally 0.0 delta you can cause the robot to escalate to discontinuities.

In your example 1e-5rad is small but our control rate is 1ms and sometimes going to 1e-5 radian might require high acceleration especially from stand still position.

To sum up, with the fix we are only enabling users to start with the current joint position of the robot. We haven't changed any error limits or discontinuity limits. Users can always try to enable the rate-limiters available to adjust the commanded action to be in the range of our discontinuity limits.

We also did some fixes in the joint position and joint velocity example controllers to be more robust in terms of packages losses. When there is a package loss we increment the time based on the lost packages so we are sync with the robot joint position again. https://github.com/frankaemika/franka_ros2/blob/humble/franka_example_controllers/src/joint_position_example_controller.cpp#L67-L73