Hybrid planner from simulation to real hardware - speed control

[sp-sophia-labs]

ROS Humble Ubuntu 22.04 FR3

Demo example of hybrid planning works fine on simulation with panda. However when testing on FR3 robot i get libfranka: Move command aborted: motion aborted by reflex! ["joint_velocity_violation"]

I believe it's due to high velocity of joint trajectory waypoints send to the robot from the local planner, specifically in local_constraints_solver (forward_trajectory) plugin from:

if (is_path_valid)
{
  if (path_invalidation_event_send_)
  {
    path_invalidation_event_send_ = false;  // Reset flag
  }
  // Forward next waypoint to the robot controller
  robot_command.addSuffixWayPoint(local_trajectory.getWayPoint(0), 0.0); // line 124
}

According to my understanding, this is the final step between local planner finding the local solution and control commands sent to the robot.

The simulation controller is of type :

controller_manager:
  ros__parameters:
    update_rate: 500  # Hz
    panda_arm_controller:
      type: joint_trajectory_controller/JointTrajectoryController
    joint_state_broadcaster:
      type: joint_state_broadcaster/JointStateBroadcaster
    panda_joint_group_position_controller:
      type: position_controllers/JointGroupPositionController

panda_arm_controller:
  ros__parameters:
    joints:
      - panda_joint1
      - panda_joint2
      - panda_joint3
      - panda_joint4
      - panda_joint5
      - panda_joint6
      - panda_joint7
    command_interfaces:
      - position
    state_interfaces:
      - position
      - velocity

panda_joint_group_position_controller:
  ros__parameters:
    joints:
      - panda_joint1
      - panda_joint2
      - panda_joint3
      - panda_joint4
      - panda_joint5
      - panda_joint6
      - panda_joint7

According to joint_trajectory_controller doc I can not set the velocity of trajectory waypoint in case command_interface is position.

The controller I am using on the real robot is of type joint_trajectory_controller/JointTrajectoryController with the following config :

controller_manager:
  ros__parameters:
    update_rate: 1000  # Hz

    fr3_arm_controller:
      type: joint_trajectory_controller/JointTrajectoryController

    joint_state_broadcaster:
      type: joint_state_broadcaster/JointStateBroadcaster

    franka_robot_state_broadcaster:
      type: franka_robot_state_broadcaster/FrankaRobotStateBroadcaster

franka_robot_state_broadcaster:
  ros__parameters:
    arm_id: fr3

fr3_arm_controller:
  ros__parameters:
    command_interfaces:
      - effort
    state_interfaces:
      - position
      - velocity
    joints:
      - fr3_joint1
      - fr3_joint2
      - fr3_joint3
      - fr3_joint4
      - fr3_joint5
      - fr3_joint6
      - fr3_joint7
    gains:
      fr3_joint1: { p: 600., d: 30., i: 0., i_clamp: 1. }
      fr3_joint2: { p: 600., d: 30., i: 0., i_clamp: 1. }
      fr3_joint3: { p: 600., d: 30., i: 0., i_clamp: 1. }
      fr3_joint4: { p: 600., d: 30., i: 0., i_clamp: 1. }
      fr3_joint5: { p: 250., d: 10., i: 0., i_clamp: 1. }
      fr3_joint6: { p: 150., d: 10., i: 0., i_clamp: 1. }
      fr3_joint7: { p: 50., d: 5., i: 0., i_clamp: 1. }

My questions are :

• is it possible to set the velocity of trajectory waypoints with this controller ? or i should use a velocity type of controller ?
• and is I believe setting the duration of robot_command.addSuffixWayPoint(); can also impact the final trajectory of the robot, is there a way to set the appropriate duration between waypoints in a way that respect robot limits ?