libfranka: Move command aborted: motion aborted by reflex! ["joint_position_limits_violation"]

[Tejas-Shastha]

I got this error when executing a move command through moveit.

Looking at some existing discussion like this and this, it seems that ros2_control does not yet respect joint limits, so moveit2 has to do it, and the best way is by defining it in the URDF. It seems like these definitions are indeed already defined here, but when I look at the params, I do not see these values populated as expected:

ros2 param dump /move_group | grep limit
  robot_description:
      type="revolute"> <safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-2.8973"
      soft_upper_limit="2.8973"/> <origin rpy="0 0 0" xyz="0 0 0.333"/> <parent link="panda_link0"/>
      <child link="panda_link1"/> <axis xyz="0 0 1"/> <limit effort="87" lower="-2.8973"
      type="revolute"> <safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-1.7628"
      soft_upper_limit="1.7628"/> <origin rpy="-1.5707963267948966 0 0" xyz="0 0 0"/>
      <limit effort="87" lower="-1.7628" upper="1.7628" velocity="2.1750"/> </joint>
      type="revolute"> <safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-2.8973"
      soft_upper_limit="2.8973"/> <origin rpy="1.5707963267948966 0 0" xyz="0 -0.316
      1"/> <limit effort="87" lower="-2.8973" upper="2.8973" velocity="2.1750"/> </joint>
      type="revolute"> <safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-3.0718"
      soft_upper_limit="-0.0698"/> <origin rpy="1.5707963267948966 0 0" xyz="0.0825
      0 1"/> <limit effort="87" lower="-3.0718" upper="-0.0698" velocity="2.1750"/>
      k_velocity="40.0" soft_lower_limit="-2.8973" soft_upper_limit="2.8973"/> <origin
      <child link="panda_link5"/> <axis xyz="0 0 1"/> <limit effort="12" lower="-2.8973"
      type="revolute"> <safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-0.0175"
      soft_upper_limit="3.7525"/> <origin rpy="1.5707963267948966 0 0" xyz="0 0 0"/>
      <limit effort="12" lower="-0.0175" upper="3.7525" velocity="2.6100"/> </joint>
      type="revolute"> <safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-2.8973"
      soft_upper_limit="2.8973"/> <origin rpy="1.5707963267948966 0 0" xyz="0.088
      0 1"/> <limit effort="12" lower="-2.8973" upper="2.8973" velocity="2.6100"/>
      0 0" xyz="0 0 0.0584"/> <axis xyz="0 1 0"/> <limit effort="20" lower="0.0" upper="0.04"
      0 0" xyz="0 0 0.0584"/> <axis xyz="0 -1 0"/> <limit effort="20" lower="0.0"
  robot_description_planning:   
     joint_limits:
          has_acceleration_limits: null
          has_jerk_limits: null
          has_velocity_limits: null
          has_acceleration_limits: null
          has_jerk_limits: null
          has_velocity_limits: null
       ...

This is noted when launching the franka moveit launch

[BarisYazici]

I also noticed the limit parameters not processed by moveit. Is there sth we can do to fix? Or is it something moveit related?

[Tejas-Shastha]

So I poked around a bit and decided to pass the joint limits expressly through a YAML file based on franka_ros values, and indeed moveit now picks them up correctly:

robot_description_planning:
      joint_limits:
        panda_joint1:
          has_acceleration_limits: true
          has_effort_limits: true
          has_jerk_limits: null
          has_position_limits: true
          has_velocity_limits: true
          max_acceleration: 3.75
          max_effort: 87
          max_position: 2.8973
          max_velocity: 2.175
          min_position: -2.8973
        panda_joint2:
          has_acceleration_limits: true
          has_effort_limits: true
          has_jerk_limits: null
          has_position_limits: true
          has_velocity_limits: true
          max_acceleration: 1.875
          max_effort: 87
          max_position: 1.7628
          max_velocity: 2.175
          min_position: -1.7628
        panda_joint3:
          has_acceleration_limits: true
          has_effort_limits: true
          has_jerk_limits: null
          has_position_limits: true
          has_velocity_limits: true
          max_acceleration: 2.5
          max_effort: 87
          max_position: 2.8973
          max_velocity: 2.175
          min_position: -2.8973
        panda_joint4:
          has_acceleration_limits: true
          has_effort_limits: true
          has_jerk_limits: null
          has_position_limits: true
          has_velocity_limits: true
          max_acceleration: 3.125
          max_effort: 12
          max_position: 2.8973
          max_velocity: 2.61
          min_position: -2.8973
        panda_joint5:
          has_acceleration_limits: true
          has_effort_limits: true
          has_jerk_limits: null
          has_position_limits: true
          has_velocity_limits: true
          max_acceleration: 3.75
          max_effort: 12
          max_position: 1.7628
          max_velocity: 2.61
          min_position: -1.7628
        panda_joint6:
          has_acceleration_limits: true
          has_effort_limits: true
          has_jerk_limits: null
          has_position_limits: true
          has_velocity_limits: true
          max_acceleration: 5.0
          max_effort: 12
          max_position: 3.7525
          max_velocity: 2.61
          min_position: -0.0175
        panda_joint7:
          has_acceleration_limits: true
          has_effort_limits: true
          has_jerk_limits: null
          has_position_limits: true
          has_velocity_limits: true
          max_acceleration: 5.0
          max_effort: 12
          max_position: 2.8973
          max_velocity: 2.61
          min_position: -2.8973

However, this does not solve my problem and I still run into the reflex state with joint limits exceeded :( . The last time this happened, the joint values looked like this:

name:
- panda_joint1
- panda_joint2
- panda_joint3
- panda_joint4
- panda_joint6
- panda_joint7
- panda_joint5
position:
- -0.5846188414192806
- 0.5743862018374197
- -0.9308654554855968
- -0.0807863111203012
- 1.0447302227815072
- -0.03548325803408056
- -0.9017913305650664
velocity:
- 0.0005321018174672895
- -0.0004305121441906692
- 0.00015391332496071937
- -0.0004063499873393697
- -0.0002132338215014766
- -0.0019501103657266297
- 7.913850579020568e-05
effort:
- 0.17199018597602844
- -30.422439575195312
- -3.2222132682800293
- 5.280417442321777
- 1.5263864994049072
- 0.08953773230314255
- -1.359389066696167

From this I can surmise that the offending joint is panda_joint6: Actual value -0.03548325803408056, permitted range: (-0.0175, 3.7525). I also dug through the runtime environment and robot model->joint model group->get varibale bounds does indeed reflect these limits being properly set. So it seems like moveit planned a joint space trajectory that violated the limits.

I will try to look into this problem a bit more or post an issue in the moveit2 repo, but of course I would welcome any suggestions in this thread as well!!

[BarisYazici]

It might also worth looking at JTC. Maybe MoveIt prepares position inside the limits, but the JTC making it go out of limits. We recently added the old patched JTC to the repo. It might worth trying with that.

[Tejas-Shastha]

It might also worth looking at JTC. Maybe MoveIt prepares position inside the limits, but the JTC making it go out of limits. We recently added the old patched JTC to the repo. It might worth trying with that.

Unfortunately, we are using the FER1 robot with ROS2, so I am restricted to using libfranka v0.9.2 and and this repo at version v0.1.0. So I cannot try out the juicy new features being released :( .

[Tejas-Shastha]

Whereas I could not solve this problem in Moveit itself, I found a simple enough workaround through testing with the robot. Narrowing the joint position limit windows by ~7% seems to work for the sequence of poses I test against.

Maybe MoveIt prepares position inside the limits, but the JTC making it go out of limits.

I think this makes the most sense, since it has been acknowledged that ros2_control does not yet respect joint limits. So I would reason out that the default planner OMPL/RRT prepares the trajectory with IK solutions that respect joint limits, but when the controller ros2_control/JTC tries to follow this trajectory, it sometimes arrives at different IK solutions that are unaware of joint limits.

In any case, since I am fairly certain that this is not a problem on the Franka side, I will close this issue.

@BarisYazici Thank you for the support!!