oscillations at the end of joint movements

[HumbertoE]

Context

System version: 5.2.1 libfranka version: 0.10.0 franka_ros version: 0.10.1 controller: position_joint_trajectory_controller

Problem

When the robot moves fast enough, at the end of a trajectory some joints oscillate. This is, the moving joint overshoots the target position, then goes back, overshoots again a bit less and this repeats for up to around 10 seconds until it stabilizes. We will send a video of this via e-mail.

We saw this happening in joints 1, 2, and 6 at least. We did some more detailed experiments on joint 1, so I will focus on this joint in the rest of the issue description, but this also happens in other joints.

We first set the joint limits of joint 1 to the default limits [-2.3093, 2.3093] rad and 2.0 rad/s, then move joint 1 back and forth to different positions. The oscillations did not happen even when going to $-\frac{11}{15}\pi$ and $\frac{11}{15}\pi$ (2.3038) rad, almost in the limit.

We then increased the speed to 2.22 rad/s and set the position limits to [-2.2209, 2.2209] rad following the recommended joint limits formulas. We then moved joint 1 between $-\frac{\pi}{2}$ and $\frac{\pi}{2}$ rad, not even close to the limits. More specifically we moved between the joint configuration: [-90,-79,0,-121,0,130,0] and [90,-79,0,-121,0,130,0] (degrees).

At the end of every movement of this experiment, joint one oscillated as described before for around 10 seconds.

We also checked that the position and velocity limits were not violated. This is the graph of one of the planned trajectory positions, velocities, and accelerations.

[HumbertoE]

This is the URDF file used in this test. Next to it, there is the joint_limits.yaml file used. If you can move to the specified positions with this limits without oscillations, please let us know. In that case, I could get and send the specific trajectory for which it occurs to us. We ran all motions at max speed.

<?xml version="1.0" ?>
<!-- =================================================================================== -->
<!-- |    This document was autogenerated by a xacro
<!-- |    EDITING THIS FILE BY HAND IS NOT RECOMMENDED                                 | -->
<!-- =================================================================================== -->
<robot name="berry">
  <!-- sub-link defining detailed meshes for collision with environment -->
  <link name="panda_link0">
    <visual>
      <geometry>
        <mesh filename="package://franka_description/meshes/visual/link0.dae"/>
      </geometry>
    </visual>
    <collision>
      <geometry>
        <mesh filename="package://franka_description/meshes/collision/link0.stl"/>
      </geometry>
    </collision>
  </link>
  <!-- sub-link defining coarse geometries of real robot's internal self-collision -->
  <link name="panda_link0_sc">
    <collision>
      <origin rpy="0 1.5707963267948966 0" xyz="-0.075 0 0.06"/>
      <geometry>
        <cylinder length="0.03" radius="0.09"/>
      </geometry>
    </collision>
    <collision>
      <origin xyz="-0.06 0 0.06"/>
      <geometry>
        <sphere radius="0.09"/>
      </geometry>
    </collision>
    <collision>
      <origin xyz="-0.09 0 0.06"/>
      <geometry>
        <sphere radius="0.09"/>
      </geometry>
    </collision>
  </link>
  <!-- fixed joint between both sub-links -->
  <joint name="panda_link0_sc_joint" type="fixed">
    <origin rpy="0 0 0"/>
    <parent link="panda_link0"/>
    <child link="panda_link0_sc"/>
  </joint>
  <!-- sub-link defining detailed meshes for collision with environment -->
  <link name="panda_link1">
    <visual>
      <geometry>
        <mesh filename="package://franka_description/meshes/visual/link1.dae"/>
      </geometry>
    </visual>
    <collision>
      <geometry>
        <mesh filename="package://franka_description/meshes/collision/link1.stl"/>
      </geometry>
    </collision>
  </link>
  <!-- sub-link defining coarse geometries of real robot's internal self-collision -->
  <link name="panda_link1_sc">
    <collision>
      <origin rpy="0 0 0" xyz="0 0 -0.1915"/>
      <geometry>
        <cylinder length="0.283" radius="0.09"/>
      </geometry>
    </collision>
    <collision>
      <origin xyz="0 0 -0.05000000000000002"/>
      <geometry>
        <sphere radius="0.09"/>
      </geometry>
    </collision>
    <collision>
      <origin xyz="0 0 -0.33299999999999996"/>
      <geometry>
        <sphere radius="0.09"/>
      </geometry>
    </collision>
  </link>
  <!-- fixed joint between both sub-links -->
  <joint name="panda_link1_sc_joint" type="fixed">
    <origin rpy="0 0 0"/>
    <parent link="panda_link1"/>
    <child link="panda_link1_sc"/>
  </joint>
  <joint name="panda_joint1" type="revolute">
    <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.0" lower="-2.2209" upper="2.2209" velocity="2.22"/>
    <safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-2.2209" soft_upper_limit="2.2209"/>
    <dynamics D="1" K="7000" damping="0.003" friction="0.0" mu_coulomb="0" mu_viscous="16"/>
  </joint>
  <!-- sub-link defining detailed meshes for collision with environment -->
  <link name="panda_link2">
    <visual>
      <geometry>
        <mesh filename="package://franka_description/meshes/visual/link2.dae"/>
      </geometry>
    </visual>
    <collision>
      <geometry>
        <mesh filename="package://franka_description/meshes/collision/link2.stl"/>
      </geometry>
    </collision>
  </link>
  <!-- sub-link defining coarse geometries of real robot's internal self-collision -->
  <link name="panda_link2_sc">
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <cylinder length="0.12" radius="0.09"/>
      </geometry>
    </collision>
    <collision>
      <origin xyz="0 0 0.06"/>
      <geometry>
        <sphere radius="0.09"/>
      </geometry>
    </collision>
    <collision>
      <origin xyz="0 0 -0.06"/>
      <geometry>
        <sphere radius="0.09"/>
      </geometry>
    </collision>
  </link>
  <!-- fixed joint between both sub-links -->
  <joint name="panda_link2_sc_joint" type="fixed">
    <origin rpy="0 0 0"/>
    <parent link="panda_link2"/>
    <child link="panda_link2_sc"/>
  </joint>
  <joint name="panda_joint2" type="revolute">
    <origin rpy="-1.5707963267948966 0 0" xyz="0 0 0"/>
    <parent link="panda_link1"/>
    <child link="panda_link2"/>
    <axis xyz="0 0 1"/>
    <limit effort="87.0" lower="-1.5133" upper="1.5133" velocity="1.0"/>
    <safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-1.5133" soft_upper_limit="1.5133"/>
    <dynamics D="1" K="7000" damping="0.003" friction="0.0" mu_coulomb="0" mu_viscous="16"/>
  </joint>
  <!-- sub-link defining detailed meshes for collision with environment -->
  <link name="panda_link3">
    <visual>
      <geometry>
        <mesh filename="package://franka_description/meshes/visual/link3.dae"/>
      </geometry>
    </visual>
    <collision>
      <geometry>
        <mesh filename="package://franka_description/meshes/collision/link3.stl"/>
      </geometry>
    </collision>
  </link>
  <!-- sub-link defining coarse geometries of real robot's internal self-collision -->
  <link name="panda_link3_sc">
    <collision>
      <origin rpy="0 0 0" xyz="0 0 -0.145"/>
      <geometry>
        <cylinder length="0.15" radius="0.09"/>
      </geometry>
    </collision>
    <collision>
      <origin xyz="0 0 -0.06999999999999999"/>
      <geometry>
        <sphere radius="0.09"/>
      </geometry>
    </collision>
    <collision>
      <origin xyz="0 0 -0.21999999999999997"/>
      <geometry>
        <sphere radius="0.09"/>
      </geometry>
    </collision>
  </link>
  <!-- fixed joint between both sub-links -->
  <joint name="panda_link3_sc_joint" type="fixed">
    <origin rpy="0 0 0"/>
    <parent link="panda_link3"/>
    <child link="panda_link3_sc"/>
  </joint>
  <joint name="panda_joint3" type="revolute">
    <origin rpy="1.5707963267948966 0 0" xyz="0 -0.316 0"/>
    <parent link="panda_link2"/>
    <child link="panda_link3"/>
    <axis xyz="0 0 1"/>
    <limit effort="87.0" lower="-2.4937" upper="2.4937" velocity="1.5"/>
    <safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-2.4937" soft_upper_limit="2.4937"/>
    <dynamics D="1" K="7000" damping="0.003" friction="0.0" mu_coulomb="0" mu_viscous="16"/>
  </joint>
  <!-- sub-link defining detailed meshes for collision with environment -->
  <link name="panda_link4">
    <visual>
      <geometry>
        <mesh filename="package://franka_description/meshes/visual/link4.dae"/>
      </geometry>
    </visual>
    <collision>
      <geometry>
        <mesh filename="package://franka_description/meshes/collision/link4.stl"/>
      </geometry>
    </collision>
  </link>
  <!-- sub-link defining coarse geometries of real robot's internal self-collision -->
  <link name="panda_link4_sc">
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <cylinder length="0.12" radius="0.09"/>
      </geometry>
    </collision>
    <collision>
      <origin xyz="0 0 0.06"/>
      <geometry>
        <sphere radius="0.09"/>
      </geometry>
    </collision>
    <collision>
      <origin xyz="0 0 -0.06"/>
      <geometry>
        <sphere radius="0.09"/>
      </geometry>
    </collision>
  </link>
  <!-- fixed joint between both sub-links -->
  <joint name="panda_link4_sc_joint" type="fixed">
    <origin rpy="0 0 0"/>
    <parent link="panda_link4"/>
    <child link="panda_link4_sc"/>
  </joint>
  <joint name="panda_joint4" type="revolute">
    <origin rpy="1.5707963267948966 0 0" xyz="0.0825 0 0"/>
    <parent link="panda_link3"/>
    <child link="panda_link4"/>
    <axis xyz="0 0 1"/>
    <limit effort="87.0" lower="-2.7478" upper="-0.4461" velocity="1.25"/>
    <safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-2.7478" soft_upper_limit="-0.4461"/>
    <dynamics D="1" K="7000" damping="0.003" friction="0.0" mu_coulomb="0" mu_viscous="16"/>
  </joint>
  <!-- sub-link defining detailed meshes for collision with environment -->
  <link name="panda_link5">
    <visual>
      <geometry>
        <mesh filename="package://franka_description/meshes/visual/link5.dae"/>
      </geometry>
    </visual>
    <collision>
      <geometry>
        <mesh filename="package://franka_description/meshes/collision/link5.stl"/>
      </geometry>
    </collision>
  </link>
  <!-- sub-link defining coarse geometries of real robot's internal self-collision -->
  <link name="panda_link5_sc">
    <collision>
      <origin rpy="0 0 0" xyz="0 0 -0.26"/>
      <geometry>
        <cylinder length="0.1" radius="0.09"/>
      </geometry>
    </collision>
    <collision>
      <origin xyz="0 0 -0.21000000000000002"/>
      <geometry>
        <sphere radius="0.09"/>
      </geometry>
    </collision>
    <collision>
      <origin xyz="0 0 -0.31"/>
      <geometry>
        <sphere radius="0.09"/>
      </geometry>
    </collision>
    <collision>
      <origin rpy="0 0 0.08" xyz="0 0.08 -0.13"/>
      <geometry>
        <cylinder length="0.14" radius="0.055"/>
      </geometry>
    </collision>
    <collision>
      <origin xyz="0 0.08 -0.06"/>
      <geometry>
        <sphere radius="0.055"/>
      </geometry>
    </collision>
    <collision>
      <origin xyz="0 0.08 -0.2"/>
      <geometry>
        <sphere radius="0.055"/>
      </geometry>
    </collision>
  </link>
  <!-- fixed joint between both sub-links -->
  <joint name="panda_link5_sc_joint" type="fixed">
    <origin rpy="0 0 0"/>
    <parent link="panda_link5"/>
    <child link="panda_link5_sc"/>
  </joint>
  <joint name="panda_joint5" type="revolute">
    <origin rpy="-1.5707963267948966 0 0" xyz="-0.0825 0.384 0"/>
    <parent link="panda_link4"/>
    <child link="panda_link5"/>
    <axis xyz="0 0 1"/>
    <limit effort="12.0" lower="-2.48" upper="2.48" velocity="3.0"/>
    <safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-2.48" soft_upper_limit="2.48"/>
    <dynamics D="1" K="7000" damping="0.003" friction="0.0" mu_coulomb="0" mu_viscous="16"/>
  </joint>
  <!-- sub-link defining detailed meshes for collision with environment -->
  <link name="panda_link6">
    <visual>
      <geometry>
        <mesh filename="package://franka_description/meshes/visual/link6.dae"/>
      </geometry>
    </visual>
    <collision>
      <geometry>
        <mesh filename="package://franka_description/meshes/collision/link6.stl"/>
      </geometry>
    </collision>
  </link>
  <!-- sub-link defining coarse geometries of real robot's internal self-collision -->
  <link name="panda_link6_sc">
    <collision>
      <origin rpy="0 0 0" xyz="0 0 -0.03"/>
      <geometry>
        <cylinder length="0.08" radius="0.08"/>
      </geometry>
    </collision>
    <collision>
      <origin xyz="0 0 0.010000000000000002"/>
      <geometry>
        <sphere radius="0.08"/>
      </geometry>
    </collision>
    <collision>
      <origin xyz="0 0 -0.07"/>
      <geometry>
        <sphere radius="0.08"/>
      </geometry>
    </collision>
  </link>
  <!-- fixed joint between both sub-links -->
  <joint name="panda_link6_sc_joint" type="fixed">
    <origin rpy="0 0 0"/>
    <parent link="panda_link6"/>
    <child link="panda_link6_sc"/>
  </joint>
  <joint name="panda_joint6" type="revolute">
    <origin rpy="1.5707963267948966 0 0" xyz="0 0 0"/>
    <parent link="panda_link5"/>
    <child link="panda_link6"/>
    <axis xyz="0 0 1"/>
    <limit effort="12.0" lower="0.8521" upper="4.2094" velocity="1.5"/>
    <safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="0.8521" soft_upper_limit="4.2094"/>
    <dynamics D="1" K="7000" damping="0.003" friction="0.0" mu_coulomb="0" mu_viscous="16"/>
  </joint>
  <!-- sub-link defining detailed meshes for collision with environment -->
  <link name="panda_link7">
    <visual>
      <geometry>
        <mesh filename="package://franka_description/meshes/visual/link7.dae"/>
      </geometry>
    </visual>
    <collision>
      <geometry>
        <mesh filename="package://franka_description/meshes/collision/link7.stl"/>
      </geometry>
    </collision>
  </link>
  <!-- sub-link defining coarse geometries of real robot's internal self-collision -->
  <link name="panda_link7_sc">
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0.01"/>
      <geometry>
        <cylinder length="0.14" radius="0.07"/>
      </geometry>
    </collision>
    <collision>
      <origin xyz="0 0 0.08"/>
      <geometry>
        <sphere radius="0.07"/>
      </geometry>
    </collision>
    <collision>
      <origin xyz="0 0 -0.060000000000000005"/>
      <geometry>
        <sphere radius="0.07"/>
      </geometry>
    </collision>
    <collision>
      <origin rpy="0 1.5707963267948966 0" xyz="0.06 0 0.082"/>
      <geometry>
        <cylinder length="0.01" radius="0.06"/>
      </geometry>
    </collision>
    <collision>
      <origin xyz="0.065 0 0.082"/>
      <geometry>
        <sphere radius="0.06"/>
      </geometry>
    </collision>
    <collision>
      <origin xyz="0.055 0 0.082"/>
      <geometry>
        <sphere radius="0.06"/>
      </geometry>
    </collision>
  </link>
  <!-- fixed joint between both sub-links -->
  <joint name="panda_link7_sc_joint" type="fixed">
    <origin rpy="0 0 0.7853981633974483"/>
    <parent link="panda_link7"/>
    <child link="panda_link7_sc"/>
  </joint>
  <joint name="panda_joint7" type="revolute">
    <origin rpy="1.5707963267948966 0 0" xyz="0.088 0 0"/>
    <parent link="panda_link6"/>
    <child link="panda_link7"/>
    <axis xyz="0 0 1"/>
    <limit effort="12.0" lower="-2.6895" upper="2.6895" velocity="3.0"/>
    <safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-2.6895" soft_upper_limit="2.6895"/>
    <dynamics D="1" K="7000" damping="0.003" friction="0.0" mu_coulomb="0" mu_viscous="16"/>
  </joint>
  <link name="panda_link8"/>
  <joint name="panda_joint8" type="fixed">
    <origin rpy="0 0 0" xyz="0 0 0.107"/>
    <parent link="panda_link7"/>
    <child link="panda_link8"/>
  </joint>
  <!-- Frame to interface the ROS-Industrial 'flange' frame (attachment point for EEF models) -->
  <link name="flange_gripper">
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://our_robot_description/meshes/gripper/visual/ee_visual.stl" scale="0.001 0.001 0.001"/>
      </geometry>
      <material name="">
        <color rgba="0.702 0.702 0.702 1.0"/>
      </material>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://our_robot_description/meshes/gripper/collision/ee_collision.stl" scale="0.001 0.001 0.001"/>
      </geometry>
      <material name="">
        <color rgba="0.01 1.0 0.01 1.0"/>
      </material>
    </collision>
  </link>
  <joint name="panda_link8-flange_gripper" type="fixed">
    <origin rpy="0 0 3.14159" xyz="0 0 0"/>
    <parent link="panda_link8"/>
    <child link="flange_gripper"/>
  </joint>
  <!-- Tool Center Point (TCP) frame -->
  <link name="tcp"/>
  <joint name="flange_gripper-tcp" type="fixed">
    <origin rpy="0 0 0" xyz="-0.008 0 0.2194"/>
    <parent link="flange_gripper"/>
    <child link="tcp"/>
  </joint>
</robot>

# joint_limits.yaml allows the dynamics properties specified in the URDF to be overwritten or augmented as needed

# For beginners, we downscale velocity and acceleration limits.
# You can always specify higher scaling factors (<= 1.0) in your motion requests.  # Increase the values below to 1.0 to always move at maximum speed.
default_velocity_scaling_factor: 0.1
default_acceleration_scaling_factor: 0.1

# Joint limits for $(find franka_description)/robots/common/franka_arm.xacro to generate the URDF.
joint1:
  limit:
    lower:      -2.2209
    upper:       2.2209
    velocity:    2.22
    effort:     87.0

joint2:
  limit:
    lower:     -1.5133
    upper:      1.5133
    velocity:   1.0
    effort:    87.0

joint3:
  limit:
    lower:     -2.4937
    upper:      2.4937
    velocity:   1.50
    effort:    87.0

joint4:
  limit:
    lower:     -2.7478
    upper:     -0.4461
    velocity:   1.25
    effort:    87.0

joint5:
  limit:
    lower:     -2.4800
    upper:      2.4800
    velocity:   3.00
    effort:    12.0

joint6:
  limit:
    lower:      0.8521
    upper:      4.2094
    velocity:   1.50
    effort:    12.0

joint7:
  limit:
    lower:     -2.6895
    upper:      2.6895
    velocity:   3.00
    effort:    12.0

# Specific joint properties can be changed with the keys [max_position, min_position, max_velocity, max_acceleration]
# Joint limits can be turned off with [has_velocity_limits, has_acceleration_limits]
# Only acceleration are included here for the move_group node to record them as this information is not included in the URDF
joint_limits:
  panda_joint1:
    has_acceleration_limits: true
    max_acceleration: 3.75
  panda_joint2:
    has_acceleration_limits: true
    max_acceleration: 1.875
  panda_joint3:
    has_acceleration_limits: true
    max_acceleration: 2.5
  panda_joint4:
    has_acceleration_limits: true
    max_acceleration: 3.125
  panda_joint5:
    has_acceleration_limits: true
    max_acceleration: 3.75
  panda_joint6:
    has_acceleration_limits: true
    max_acceleration: 5
  panda_joint7:
    has_acceleration_limits: true
    max_acceleration: 5

[Maverobot]

Can you please deactivate "rate limiting" and "cutoff frequency filter" in franka_ros/franka_control/config/franka_control_node.yaml with

# Activate rate limiter? [true|false]
rate_limiting: false
# Cutoff frequency of the low-pass filter. Set to >= 1000 to deactivate.
cutoff_frequency: 1000

and write us about the results?

[Maverobot]

This is the graph of one of the planned trajectory positions, velocities, and accelerations.

In addition to my previous request, can you also try to use plotjuggler to plot the derivative of /position_joint_trajectory_controller/state/desired/positions.0 for the test of oscillation of joint 1? On my local test, it seems that the derivative of the desired position does not really match the desired velocity. (See the screenshot below)

Can you please confirm it on your setup? If that is the case, probably the issue is not really caused by franka_ros/libfranka.

[HumbertoE]

Sorry for the very delay reply.

I did some experiments with and without the rate limiting (and changing the cutoff frequency) as you recommended and visualizing the data in plotjuggler. Thank you for suggesting that tool. It's very useful.

Context

The joint_limits.yaml file specifies for joint 1:

joint1:
  limit:
    lower:      -2.4693 #physical limit: -2.7437
    upper:       2.4693 #physical limit:  2.7437
    velocity:    2.62
    effort:     87.0

With rate limiting

Plotting the commanded trajectory directly from the generated moveit_msgs.msg.RobotTrajectory in python (using matplotlib) I get the following desired trajectory to move from [90,-79,0,-121,0,130,0] to [-90,-79,0,-121,0,130,0] (degrees):

Then plotting the in plotjuggler the position of joint 1 from/position_joint_trajectory_controller/state/desired/positions.0 and its derivatives I got the following:

Without the rate limiting and getting the information directly from the robot I got a plot similar of what you got , but with the rate limiting and getting the information from a rosbag file I get this extreme numbers. I think the big jumps are only an artifact of the way the derivative is calculated, because we are not using the real output of the controller, but the output taken from the state it publishes (at 50 Hz) and maybe the timestamps are not accurate, maybe the taken times are just when the message arrived. This is worst when taken from the rosbag file. It can be seen that even the position has steps that are not there when adding the subscriber in plotjuggler. Still with the subscriber in plotjuggler, the time when the messages are received may play a roll so I wouldn't take this calculated derivative as the real velocity of the robot.

Instead, I plot /position_joint_trajectory_controller/state/desired/velocities.0 and /position_joint_trajectory_controller/state/desired/accelerations.0. Furthermore I also plot /joint_states/panda_joint1/position and /joint_states/panda_joint1/velocity and get the following graph:

It can be seen that the velocity does not go below -2 rad/s for some reason even when we specify the joint 1 speed limit at 2.62. And this does not happen when rate_limiting is set to False and cutoff_frequency is set to 1000.

The problem could be that there is somewhere a speed limit that doesn't takes the URDF file into account and uses default parameters (2 is the default joint 1 speed limit). Then when the desired velocity is below the negative limit, the output is limited, but there might be an integral controller that accumulates negative error so later when the desired velocity is above the negative limit again, the output is still below the limit. Then when the accumulated error is positive again, it has to react very fast causing the oscillations.

This is only a theory, but if it would be the case, looking and fixing this limit so it matches with the one given in the URDF file could fix the problem.

Either way I think there is an obvious error there that should have been investigated further from your side. It was easy to see a problem once I plotted not only the desired, but also the actual state of the robot and you could have done the same.

With rate limiting

First the robot moved from [0,-79,0,-121,0,130,0] to [90,-79,0,-121,0,130,0] (degrees) successfully.

The next movement from [90,-79,0,-121,0,130,0] to [-90,-79,0,-121,0,130,0] (degrees) was unsuccessful and resulted in the error: libfranka: Move command aborted: motion aborted by reflex! ["joint_motion_generator_velocity_limits_violation"]

Plotting the commanded trajectory directly from the generated moveit_msgs.msg.RobotTrajectory in python (using matplotlib) I get the following desired trajectory to move from [90,-79,0,-121,0,130,0] to [-90,-79,0,-121,0,130,0] (degrees):

Is not very visible, but the trajectory has 8 points where the velocity is exactly the limit, -2.62 rad/s.

From the execution, I plot in plotjuggler positions.0, velocities.0 and accelerations.0 from /position_joint_trajectory_controller/state/desired/ and position and velocity from /joint_states/panda_joint1. I get the following:

At the point where the movement was interrupted because the "joint_motion_generator_velocity_limits_violation" error was raised, the desired and actual velocity did descend below -2.62 as it can be seen in this zoomed image:

Conclusion

WIthout the rate limiting and with higher cut-off frequency, the velocity can go below the velocity limits causing an error. We will look into the controller, see why this happens and prevent it maybe by adding a limiter by ourselves, but it would be nice to use the limiters that you already implemented.

I think it could be checked why and where this joint speed limit is present even when it is not in the URDF file.

I attach the rosbag files I used. In 2023_04_25_oscillation_test_without_rate_limiting.bag there are two movements, first from 0° to 90° in joint 1 (successful movement) and then from 90° to -90° in joint 1 (unsuccessful movement) 2023_04_25_oscillation_test.zip

[HumbertoE]

Looking a bit more on how ros_control is structured to see what we could do from our side, in the structure shown in their paper the part that enforces joint limits is in the hardware interface, which makes sense since the controller should be robot agnostic.

So from my unterstanding, the following should happen:

1. We send a trajectory that complies with the limits specified in the joint_limits.yaml file which was also used to generate the URDF file (which we do as far as I can tell). This trajectory we send as a moveit_msgs.msg.RobotTrajectory message

2. The controller follows the trajectory as good as it can. It logically could have small deviations that could slightly violate the joint limits since it doesn't know about the limits (see the little "horns" in the following plot at the beginning and end of the section with max speed)

3. The hardware interface should enforce the joint limits given in the URDF file

So from my understanding there is no much we can do from our side and the joint limit enforcer has to be fixed from your side.

[HumbertoE]

@Maverobot, please let me know if something of what I wrote is unclear or if you don't get the same result

[Maverobot]

Hello @HumbertoE ,

thank you so much for your detailed reports! I think we found the issue. The issue is that the rate limiter in libfranka is using the hard coded velocity limits:

constexpr std::array<double, 7> kMaxJointVelocity{
    {2.00 - kLimitEps - kTolNumberPacketsLost * kDeltaT * kMaxJointAcceleration[0],
     1.00 - kLimitEps - kTolNumberPacketsLost* kDeltaT* kMaxJointAcceleration[1],
     1.50 - kLimitEps - kTolNumberPacketsLost* kDeltaT* kMaxJointAcceleration[2],
     1.25 - kLimitEps - kTolNumberPacketsLost* kDeltaT* kMaxJointAcceleration[3],
     3.00 - kLimitEps - kTolNumberPacketsLost* kDeltaT* kMaxJointAcceleration[4],
     1.50 - kLimitEps - kTolNumberPacketsLost* kDeltaT* kMaxJointAcceleration[5],
     3.00 - kLimitEps - kTolNumberPacketsLost* kDeltaT* kMaxJointAcceleration[6]}};

Since you have higher velocity limits the first joint, the rate limiter is working against your planner trajectory. Can you kindly change this kMaxJointVelocity in a local libfranka repo to reflect your velocity limits, build franka_ros using this local libfranka and then check if the oscillation still happens? If you can confirm that changing kMaxJointVelocity fixes the oscillation, we will work on providing a proper solution.

[HumbertoE]

Hello @Maverobot,

I changed kMaxJointVelocity and, with 2.62 rad/s (max velocity of joint 1) it worked without any oscillations as shown in the following plot:

The ideal solution in my understanding would be that this values are read from the information taken from the URDF file. Another solution could be to set the real limits of the robot as the hard-coded values, but this could be unsafe for using it at lower velocities. You know more about the system, so you can weight the alternatives better.

[Maverobot]

Hi @HumbertoE ,

thanks for the testing. We will decide on a proper solution and let you know here.

Cheers, Zheng

[Maverobot]

Hi @HumbertoE ,

can you please tell me if if "Rate Limiter" is necessary for your case? Actually you should always try to send only the trajectories where the robot limits are respected, e.g. max torque rate, max velocity, acceleration or jerk.

Another usage of "Rate limiter" for panda was to deal with the reoccurring package losses, which seldom happens with FR3 if the pc is correctly set up. Therefore, actually "Rate limiter" should not be needed for FR3.

We have the idea to use the position based velocity limit for the "Rate limiter" instead of hard coding the values as constants. After this change, no negative effects should be observed by activating "Rate limiter" if the trajectory already obeys all the limits of the robot. If you are interested, you are very welcome to create a PR to implement it with the formula in this libfranka documentation section. This PR can be a "good first issue" :)

Cheers, Zheng

[HumbertoE]

Hi @Maverobot,

Thank you for your answer.

Regarding the trajectory and joint limits, as I mentioned in a previous message, the trajectory we send already complies with the limits specified in the joint_limits.yaml file, which is used to generate the URDF file. However, it is possible for the controller to have slight deviations from the trajectory, leading to potential violations of the joint limits. That's why having a velocity limiter in the hardware interface becomes necessary to ensure the limits are respected.

Regarding the idea of using a position-based velocity limit for the "Rate limiter," I think it's an interesting suggestion. However, I have other tasks and have to focus on our product, so I have limited capacity to implement such changes directly. Nonetheless, I would appreciate it if your team could consider incorporating this feature.

It's worth mentioning that if the position-based velocity limit is implemented, it should be provided as an option alongside a fixed (but not hard-coded) velocity limiter. This is because if the trajectory is generated using a fixed velocity limit and then the hardware interface limits the velocity based on the position, compatibility issues may arise, and the resulting behavior might not be as desired. (after discussing with @Maverobot, I don't think this is necessary)

Best regards, HumbertoE

[Maverobot]

The oscillation should be fixed by https://github.com/frankaemika/libfranka/commit/b8249fc22e179f7d72a81832d9bf311b019a8fa2, which has already been merged into the fr3_develop branch of libfranka.

@HumbertoE Please close the issue, if you confirm that the issue is gone.

[HumbertoE]

Perfect, thanks. I am not able to test it at the moment and I will take vacations soon, but since you were able to reproduce the error and it did not happen with these changes, I think we can close the issue already.

I will test the changes when possible and, if the error is still there, I can reopen the issue. I will also make an update here to confirm when I test it.

Thank you for the solution @Maverobot

[HumbertoE]

Hello @Maverobot ,

I still haven't test the solution (that I think will work as you tested it already). But one thing that we observed and commented but then forgot about is about the controller itself.

If you see the plot shared in a previous comment of the joint 1 velocity during the oscillations when the hard-coded velocity limit was there, you can see that it was limiting the velocity when it was below the limit (-2) as expected, but then when the desired joint velocity (green) goes above -2, the joint velocity (red) still stays some time at -2 and then comes back to 0 abruptly to then oscillate.

My theory would be that maybe the negative error keeps accumulating in the integral component of the controller (assuming it is a PID or PI controller), which could cause that even when the error at a given time is positive, the integral component is too negative and it takes some time for it to be overcome, provoking that the system has short time to react and has to do a fast maneuver causing the oscillations. This problem is known as integral windup

With your fix this shouldn't be a problem as long as we send trajectories that comply with the limits, but could happen if we don't.

Is there an anti-windup feature in the controller like for example an upper and lower limit for the integral component?

[Maverobot]

@HumbertoE Welcome back! I wish that you had a good vacation! If you are talking about the position_joint_trajecory_controller then maybe you should report this to ros_controllers? From my understanding, the anti-windup feature is not really related to franka_ros.

[HumbertoE]

Thank you @Maverobot =)

I think that it would be a controller with a lower level, one that takes the desire joint velocity and compares it with the actual joint velocity

[Maverobot]

Hi @HumbertoE , as far as I know, since there is no integral term in our internal controller, the only I term in PID controller in this control loop is here :

https://github.com/ros-controls/ros_controllers/blob/495fb970d8f37b9ccaec237861982e997ac75f13/joint_trajectory_controller/include/joint_trajectory_controller/hardware_interface_adapter.h#L210

[HumbertoE]

In the joint trajectory controller description it mentions that:

For position-controlled joints, desired positions are simply forwarded to the joints

So the PID controller there is not used as we also just specify in the description of the controller a yaml file:

 controller_list:
  - name: position_joint_trajectory_controller
    action_ns: follow_joint_trajectory
    type: FollowJointTrajectory
    default: true
    joints:
      - panda_joint1
      - panda_joint2
      - panda_joint3
      - panda_joint4
      - panda_joint5
      - panda_joint6
      - panda_joint7

But if internally you don't use an integral term, then the integral windup shouldn't be a problem.

Ok, thank you for the clarification

[Maverobot]

@HumbertoE Thanks for the clarification too.

[HumbertoE]

I tested building libfranka at https://github.com/frankaemika/libfranka/commit/b8249fc22e179f7d72a81832d9bf311b019a8fa2 but still got the oscillations running the test described in the description of this issue, therefore I will reopen the issue.

I will also communicate this through the appropriate Franka ticket.

[HumbertoE]

We identified an error in our test setup. We tested the solution again and saw no oscillations therefore the proposed solution does solve the issue.

Thank you for the solution.