frankaemika/franka_ros

fp3 deviates from trajectory when close to joint limits

Closed this issue · 4 comments

HumbertoE commented

Context

System: Franka Disruption 3
System version: 5.2.1
libfranka version: 0.10.0
franka_ros version: 0.10.1
controller: position_joint_trajectory_controller

Problem

When close to joint limits, the system doesn't follow the trajectory correctly. In the example photo, an almost straight trajectory can be seen as a green line. The robot should go back a little bit. Instead of it, the robot just went down deviating from the path, and then stopped.

trajectory_deviation

The other thing is that the system is not even close to the joint limits we indicated. In the joint_limits.yaml configuration file, we specify the limits of joint 2 as:

  panda_joint2:
    max_position: 1.7837
    min_position: -1.7837
    has_velocity_limits: true
    max_velocity: 2.62
    has_acceleration_limits: true
    max_acceleration: 1.875

Nevertheless, when this happens, we receive a warning from FrankaHW:

[ WARN] [1671185048.709777923]: FrankaHW: 
	panda_joint2: 0.010531 degrees to joint limits (limits: [-1.513300, 1.513300] q: -1.513484)

It is worth mentioning that we tested the system at a lower speed, retiming the trajectories with a velocity scaling factor of 0.4 and this still happened.

Logs

The move group node used to execute the trajectory gives the following logs when failing the execution:

[ INFO] [1671185486.561096687] [/move_group]: Execution request received
[ WARN] [1671185487.829839542] [/move_group]: Controller 'position_joint_trajectory_controller' failed with error GOAL_TOLERANCE_VIOLATED: panda_joint2 goal error -0.073205
[ WARN] [1671185487.830000923] [/move_group]: Controller handle position_joint_trajectory_controller reports status ABORTED
[ INFO] [1671185487.830027370] [/move_group]: Completed trajectory execution with status ABORTED ...
[ INFO] [1671185487.830125222] [/move_group]: Execution completed: ABORTED
[ INFO] [1671185487.830877439] [/move_group]: Received event 'stop'
andrejpan commented

Hey @HumbertoE HumbertoE

Can you please update the Context section that you use FR(esearch)3 and not FrankaP(roduction). Also, add Robot system version from the Desk/settings.

Secondly, can you provide a demo repository of what you are trying to execute? This kind of problem is hard to reproduce and debug.

From what you described above, I would say that you linking the default yaml file from the repository where joint position limits for joint2 are [-1.513300, 1.513300].

HumbertoE commented

I updated the version. Our system is a special case where FCI was enabled in a FP3 robot. The system, as I can see in Franka World, is called Franka Disruption 3.

To reproduce the problem, maybe the initial and final state of the robot are useful.
The initial position was at this joint configuration:
[1.5271146947877825, -1.5134893046578948, 0.018819685675540928, -2.351497353575113, -0.01094299672073909, 2.392510117859008, 0.027194409017560255]
while the final one was at:
[ 1.57079633, -1.49609623, 0, -2.0630839 , 0, 2.1280997 , 0 ]

And our joint limits specified in the joint_limits.yaml file are:

joint_limits:
  panda_joint1:
    max_position: 2.7437
    min_position: -2.7437
    has_velocity_limits: true
    max_velocity: 2.62
    has_acceleration_limits: true
    max_acceleration: 3.75
  panda_joint2:
    max_position: 1.7837
    min_position: -1.7837
    has_velocity_limits: true
    max_velocity: 2.62
    has_acceleration_limits: true
    max_acceleration: 1.875
  panda_joint3:
    max_position: 2.9007
    min_position: -2.9007
    has_velocity_limits: true
    max_velocity: 2.62
    has_acceleration_limits: true
    max_acceleration: 2.5
  panda_joint4:
    max_position: -0.1518
    min_position: -3.0421
    has_velocity_limits: true
    max_velocity: 2.62
    has_acceleration_limits: true
    max_acceleration: 3.125
  panda_joint5:
    max_position: 2.8065
    min_position: -2.8065
    has_velocity_limits: true
    max_velocity: 5.26
    has_acceleration_limits: true
    max_acceleration: 3.75
  panda_joint6:
    max_position: 4.5169
    min_position: 0.5445
    has_velocity_limits: true
    max_velocity: 2.
    has_acceleration_limits: true
    max_acceleration: 5
  panda_joint7:
    max_position: 3.0159
    min_position: -3.0159
    has_velocity_limits: true
    max_velocity: 5.26
    has_acceleration_limits: true
    max_acceleration: 5

If this is not enough to reproduce the error, I could send more data or a demo repository, but in this case, I would appreciate it if I could share it privately.

HumbertoE commented

And regarding linking wrong the repository, the system responds to changes in this yaml file. We have 2 computers, one that runs most of our code and the move_group node and one that runs franka control, but both have the same joint_limits.yaml file.

We load the robot description in a launch file with:
<param name="robot_description" textfile="$(find our_robot_description)/urdf/our_robot.urdf" />

But the urdf has different limits, actually the joint 2 limits are [-1.513300, 1.513300] as the warning from FrankaHW shows. I thought the joint limits where overwritten, but do we have to change something for this? Or do we have to change the urdf every time the limits changes?

Thank you in advance for your help.

HumbertoE commented

You were right, we were linking the default yaml file when running the xacro file. Our move_group node used for planning had our defined limits, but the franka_control did not.

We changed this, regenerated the URDF file and now the problem is solved.

I will therefore close the issue. Thank you for the help

  • 👍1