Exceeding Speed limit when using xarm_controller on Lite6

[pedrofontanatf]

Hi everyone,

Sorry if this seems too basic, I'm no expert neither in robotics nor in ROS. We're using a Lite6 to test an AI model that directly outputs position commands to the joints. We setup controllers with ros_control (position_controllers/JointPositionController, so as per my understanding this just forwards our model's output without caring on the PID gains set) and bring up a uf_ros_controller node of xarm_controller package. The thing is that no matter which joint limits we define in the YAML file, we always exceed speed limit and need to reset the robot state (this log message shows the correct limits being set, so it makes me think that the issue is not coming from the limits not being read from the config file).

Digging a bit into your code we discovered that in uf_ros_controller you use move_servo_j which as per my understanding would execute the command as fast as possible, so I'd expect the joint_limits_interface interfaces set in uf_ros_controller to be the ones saturating commands within defined limits, but that's not happening). Our model would adapt if the command has not finished and it's currently outputting commands at 100 Hz (so we don't care much if last command was not completed)

I'm mentioning this because we got our model to work by spawning a uf_driver node from xarm_api package, setting working mode to 6 and calling the service it exposes move_joint with our model's output (in a client node that subscribes to our model's output and calls the mentioned service). By looking at the code, uf_driver uses the set_servo_angle function of the SDK (which again from what I've understood supports multiple modes and allows us to set desired speed).

The thing is that in order to have consistency and a smooth transition between our simulation (using Gazebo and gazebo_ros_control plugin which connects to the position controllers) and the real-world case, we'd truly like to use the HardwareInterface you setup on xarm_controller, but we need the speed limits defined in the YAML file to be enforced. Could you please tell me how to enforce those?

Looking forward to your reply, thanks in advance!

[penglongxiang]

Hi @pedrofontanatf, may I know how you interface with the FollowJointTrajectory action / JointTrajectoryController and send joint commands? Did you setup an action client and send your target trough /ufactory/lite6_traj_controller/follow_joint_trajectory/goal message? Another question is: are you updating your command in realtime and did you do speed planning yourself?

In our demo package, normally Moveit will be the role to generate the above mentioned trajectory message. And the command should not only specify the target joint positions, but also the joint velocity, acceleration as well as the time (from start) in a trajectory. I will post a sample moveit generated trajectory:

header: 
  seq: 1
  stamp: 
    secs: 1714964635
    nsecs:  55397964
  frame_id: ''
goal_id: 
  stamp: 
    secs: 1714964635
    nsecs:  55398578
  id: "/move_group-2-1714964635.55398578"
goal: 
  trajectory: 
    header: 
      seq: 0
      stamp: 
        secs: 0
        nsecs:         0
      frame_id: "/world"
    joint_names: [joint1, joint2, joint3, joint4, joint5, joint6]
    points: 
      - 
        positions: [-0.00019941749633289874, -0.00014572817599400878, 2.6844663807423785e-05, 0.0007669904152862728, -0.0011984225129708648, -0.0007669904152862728]
        velocities: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
        accelerations: [0.7305645075038962, 0.0, 0.0, 0.0, 0.0, 0.0]
        effort: []
        time_from_start: 
          secs: 0
          nsecs:         0
      - 
        positions: [0.056956150458428084, -0.014049212643344768, 0.06357482033722511, -0.028522969150947586, 0.05687346352742193, 0.07533651101995062]
        velocities: [0.24745113298543725, -0.06019418767064828, 0.2751266261891522, -0.1268088821284905, 0.2514182696368324, 0.3294849185859511]
        accelerations: [0.737170085513407, -0.17932168641633692, 0.8196168516504968, -0.37777040401758943, 0.7489883965846457, 0.9815530956719347]
        effort: []
        time_from_start: 
          secs: 0
          nsecs: 395562384
      - 
        positions: [0.11411171841318907, -0.027952697110695528, 0.1271227960106428, -0.057812928717181446, 0.11494534956781473, 0.1514400124551875]
        velocities: [0.4005174635316333, -0.09742862388354467, 0.4453122406102705, -0.2052492999754173, 0.4069385596483914, 0.5332950480842538]
        accelerations: [0.6912596469322965, -0.16815365690419964, 0.7685716859998368, -0.354243126836596, 0.702341921835993, 0.9204226537311714]
        effort: []
        time_from_start: 
          secs: 0
          nsecs: 558672773
      - 
        positions: [0.17126728636795005, -0.041856181578046284, 0.1906707716840605, -0.08710288828341531, 0.17301723560820753, 0.2275435138904244]
        velocities: [0.4898669660201763, -0.11916350404427095, 0.5446547931165822, -0.25103737292788875, 0.4977205133923791, 0.6522652592849654]
        accelerations: [0.6683577923111762, -0.1625826234704971, 0.7431084362690131, -0.34250683552066746, 0.6790729046734983, 0.8899284886236254]
        effort: []
        time_from_start: 
          secs: 0
          nsecs: 685509103
      - 
        positions: [0.22842285432271103, -0.05575966604539705, 0.2542187473574782, -0.11639284784964916, 0.23108912164860032, 0.3036470153256613]
        velocities: [0.565886996064717, -0.13765589848199902, 0.6291770748964893, -0.28999462041759727, 0.5749592965854357, 0.7534870767320532]
        accelerations: [0.7251816271874545, -0.17640541158805914, 0.8062875770877352, -0.37162679505360136, 0.7368077392905757, 0.9655902824576919]
        effort: []
        time_from_start: 
          secs: 0
          nsecs: 793531325
      - 
        positions: [0.28557842227747204, -0.06966315051274781, 0.31776672303089587, -0.14568280741588302, 0.2891610076889931, 0.37975051676089816]
        velocities: [0.6327955951560937, -0.1539318747602063, 0.7035688827211739, -0.32428262125718194, 0.6429405743610704, 0.8425768863374907]
        accelerations: [0.6656746566005904, -0.1619299322802792, 0.7401252126047969, -0.3411318349864116, 0.676346752930264, 0.8863558529258712]
        effort: []
        time_from_start: 
          secs: 0
          nsecs: 888369422
      - 
        positions: [0.342733990232233, -0.08356663498009856, 0.38131469870431356, -0.1749727669821169, 0.3472328937293859, 0.4558540181961351]
        velocities: [0.6868367701139358, -0.16707776174042932, 0.7636541446557952, -0.35197657805032945, 0.6978481374549179, 0.914533526488828]
        accelerations: [0.5739603989955228, -0.13961980919558115, 0.6381534254325247, -0.29413191891477874, 0.5831621323148642, 0.764236If you did not specify 9345939808]
        effort: []
        time_from_start: 
          secs: 0
          nsecs: 974586467
      - 
        positions: [0.39988955818699395, -0.09747011944744932, 0.4448626743777312, -0.2042627265483507, 0.40530477976977863, 0.531957519631372]
        velocities: [0.683554955509683, -0.1662794377974135, 0.7600052844992344, -0.3502947783499306, 0.6945137087977709, 0.9101637408077086]
        accelerations: [-0.649379819632628, 0.15796610126977675, -0.7220079242933095, 0.3327813779266823, -0.6597906771302531, -0.8646590315495093]
        effort: []
        time_from_start: 
          secs: 1
          nsecs:  55002661
      - 
        positions: [0.45704512614175496, -0.11137360391480008, 0.5084106500511489, -0.2335526861145846, 0.4633766658101715, 0.6080610210666089]
        velocities: [0.626687431416592, -0.15244602197778948, 0.6967775681630832, -0.32115242982779135, 0.6367344845383403, 0.8344437741220054]
        accelerations: [-0.6493000166624542, 0.1579466886491669, -0.7219191959788519, 0.3327404820725655, -0.6597095947588555, -0.8645527726901796]
        effort: []
        time_from_start: 
          secs: 1
          nsecs: 142081876
      - 
        positions: [0.5142006940965159, -0.12527708838215085, 0.5719586257245666, -0.26284264568081844, 0.5214485518505643, 0.6841645225018457]
        velocities: [0.5639551079475633, -0.13718595342868822, 0.627029120052093, -0.2890046044513013, 0.5729964364373055, 0.7509147385442336]
        accelerations: [-0.6501035019501215, 0.15814214196398493, -0.7228125448745318, 0.3331522363850487, -0.6605259615229357, -0.8656226254784788]
        effort: []
        time_from_start: 
          secs: 1
          nsecs: 237817931
      - 
        positions: [0.571356262051277, -0.1391805728495016, 0.6355066013979843, -0.2921326052470523, 0.5795204378909571, 0.7602680239370826]
        velocities: [0.4915890431161234, -0.11958241112562529, 0.5465694712016224, -0.25191986907507335, 0.4994701988289364, 0.6545582309311706]
        accelerations: [-0.6718654658493861, 0.1634358891504654, -0.7470084159324533, 0.3443043789583744, -0.6826368132351047, -0.8945990088843913]
        effort: []
        time_from_start: 
          secs: 1
          nsecs: 345476160
      - 
        positions: [0.6285118300060378, -0.15308405731685235, 0.699054577071402, -0.3214225648132861, 0.6375923239313498, 0.8363715253723195]
        velocities: [0.4013738335931072, -0.09763694178281619, 0.4462643890322243, -0.20568815563501225, 0.4078086590349934, 0.5344353177471165]
        accelerations: [-0.7034716172727933, 0.17112430256516686, -0.7821494706653136, 0.36050127683486594, -0.7147496744893004, -0.9366830765664673]
        effort: []
        time_from_start: 
          secs: 1
          nsecs: 471848301
      - 
        positions: [0.6856673979607989, -0.1669875417842031, 0.7626025527448197, -0.35071252437952005, 0.6956642099717427, 0.9124750268075564]
        velocities: [0.24879151938535388, -0.060520245868621876, 0.2766169244292518, -0.12749577694664302, 0.2527801451119858, 0.33126966330916946]
        accelerations: [-0.7373292340175557, 0.17936040038305243, -0.819793799682543, 0.37785196130795307, -0.7491500965576707, -0.9817650043073873]
        effort: []
        time_from_start: 
          secs: 1
          nsecs: 634931925
      - 
        positions: [0.7428229659155599, -0.18089102625155387, 0.8261505284182373, -0.38000248394575387, 0.7537360960121354, 0.9885785282427932]
        velocities: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
        accelerations: [-0.7573322463451932, 0.18422627051870913, -0.8420339940551703, 0.38810271102918853, -0.7694737971860053, -1.0083993171461711]
        effort: []
        time_from_start: 
          secs: 2
          nsecs:  23440891
  path_tolerance: []
  goal_tolerance: []
  goal_time_tolerance: 
    secs: 0
    nsecs:         0
---

After receiving this trajectory message (speed/acceleration/time calculated by time parameterization algorithm), the loaded ros controller (position_controllers/JointTrajectoryController in this case) would do interpolation (Normally by Quintic Spline) based on the via points given, and enforce the joint speed/acceleration limits based on loaded yaml file. Through this mechanism, the ros controller can generate a smooth position command update in fixed 100 Hz and send it to the hardware by move_servo_j. Please note move_servo_j will not do more interpolation or saturation, it just executes the received command and report error if the command is abrupt, it will be the job of upper application to do trajectory smoothing and speed control (just as what Moveit does).

That is to say, you may need to be able to generate the trajectory via points with speed/acceleration/time info in order to make the ros controller interpolation and limit enforcement to work. If you are just sending pure position updates, maybe only move_joint in mode 6 would be suitable for you currently, as there is internal speed control in this mode.

[vimior]

@pedrofontanatf
I tried position_controllers/JointPositionController. The speed limit in joint_limits.yaml is valid. You can try to modify the maximum velocity in joint_limits.yaml to a smaller value.
In addition, if you want to use mode6 in uf_ros_controller, you need to modify the code of uf_ros_controller to support it.