'Undderstanding JointTrajectoryController of ros_control

currently, I am trying to understand how the hardware_interface and ros_control work: velocity_controllers/JointTrajectoryController is chosen for the controller type and so far everything seems to work. As you can see the output of $rostopic echo /robot_arm_controller_trajectory/follow_joint_trajectory/goal the trajectory controller computed 8 interpolation points with specific velocity values. What would be the proper next step to make sure that the real robot actually moving towards these points with the desired velocity values?

  1. In hardware_interface.cpp I locally store the values from /robot_arm_controller_trajectory/follow_joint_trajectory/goal
  2. In write() I make sure that the actual velocity matches the desired velocity value (PID should be applied). For each point, I check whether the velocity has reached its desired value within the given time, otherwise set_abort()

Is this the right approach? If yes, JointTrajectoryController actually only cares about generating the desired interpolation points. Then what is $rostopic echo /robot_arm_controller_trajectory/follow_joint_trajectory/feedback meant for, if JointTrajectoryController is not taking care of the "reaching" task (checking whether the computed points are reached or not)?

header:    seq: 0   stamp: 
    secs: 1634781253
    nsecs: 480294199   frame_id: '' goal_id:    stamp: 
    secs: 1634781253
    nsecs: 480294676   id: "/move_group_plan-2-1634781253.480294676" goal:    trajectory: 
    header: 
      seq: 0
      stamp: 
        secs: 1634781313
        nsecs: 480279134
      frame_id: "world"
    joint_names: 
      - jnt0
      - jnt1
      - jnt2
      - jnt3
      - jnt4
    points: 
      - 
        positions: [1.5700000524520874,
0.7400000095367432, 0.6600000262260437, 1.7000000476837158, 1.0]
        velocities: [0.0, 0.0, 0.0, 0.0, 0.0]
        accelerations: []
        effort: []
        time_from_start: 
          secs: 0
          nsecs:         0
      - 
        positions: [1.5703209370473115,
0.7406213365581189, 0.6710738072625566, 1.6212431102458411, 0.9413782050925622]
        velocities: [0.0013806954234580442,
0.002673432715851713, 0.047648029930578414, -0.33887367827729736, -0.2522366144972447]
        accelerations: []
        effort: []
        time_from_start: 
          secs: 0
          nsecs: 398756937
      - 
        positions: [1.5706418216425353,
0.7412426635794948, 0.6821475882990696, 1.5424861728079666, 0.8827564101851244]
        velocities: [0.0022349043068913945,
0.004327432530974492, 0.07712692133070972, -0.5485281041759992, -0.40829040678906914]
        accelerations: []
        effort: []
        time_from_start: 
          secs: 0
          nsecs: 562751472
      - 
        positions: [1.5709627062377594,
0.7418639906008705, 0.6932213693355825, 1.463729235370092, 0.8241346152776866]
        velocities: [0.002753033119682365,
0.005330682411870807, 0.09500763329677239, -0.6756960614448504, -0.502946371740638]
        accelerations: []
        effort: []
        time_from_start: 
          secs: 0
          nsecs: 690434717
      - 
        positions: [1.5712835908329834,
0.7424853176222463, 0.7042951503720954, 1.3849722979322172, 0.7655128203702488]
        velocities: [0.0027706307883390013,
0.00536475667786027, 0.09561493178463103, -0.6800151796264982, -0.506161256276052]
        accelerations: []
        effort: []
        time_from_start: 
          secs: 0
          nsecs: 797648694
      - 
        positions: [1.5716044754282075,
0.7431066446436221, 0.7153689314086084, 1.3062153604943427, 0.706891025462811]
        velocities: [0.0022505020740580467,
0.004357634399054199, 0.07766520288374407, -0.5523563726288884, -0.41113993313426167]
        accelerations: []
        effort: []
        time_from_start: 
          secs: 0
          nsecs: 923568484
      - 
        positions: [1.5719253600234313,
0.7437279716649979, 0.7264427124451214, 1.227458423056468, 0.6482692305553732]
        velocities: [0.0013786955219680596,
0.0026695603179419584, 0.04757901299575412, -0.33838282854853885, -0.25187125630702323]
        accelerations: []
        effort: []
        time_from_start: 
          secs: 1
          nsecs:  87898940
      - 
        positions: [1.5722462446186554,
0.7443492986863737, 0.7375164934816343, 1.1487014856185933, 0.5896474356479354]
        velocities: [0.0, 0.0, 0.0, 0.0, 0.0]
        accelerations: []
        effort: []
        time_from_start: 
          secs: 1
          nsecs: 486655878   path_tolerance: []   goal_tolerance: []   goal_time_tolerance: 
    secs: 0
    nsecs:         0
---


Sources

This article follows the attribution requirements of Stack Overflow and is licensed under CC BY-SA 3.0.

Source: Stack Overflow

Solution Source