Understanding JointTrajectoryController of ros_control

asked 2021-10-20 21:24:56 -0600

xman236
51 ●15 ●21 ●26

Hi, 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?

In hardware_interface.cpp I locally store the values from /robot_arm_controller_trajectory/follow_joint_trajectory/goal
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
---

answered 2021-10-21 07:59:37 -0600

Mike Scheutzow
4918 ●2 ●430 ●45

updated 2021-10-21 08:01:33 -0600

No, I would say you are not using the JointTrajectoryController in the way it is intended.

In a software-layer sense, it sits on top of a simple JointController. Conceptually, for each joint, this JointController takes 1 input (target_velocity) and provides 2 status outputs (cur_position,cur_velocity). It is meant to be a very simple command interface. Note that I'm describing a velocity cmd interface; there are other types.

In a real robot, there is normally one JointController, with N inputs. Note that gazebo models this differently, using N one-input JointControllers. In the bigger picture, this difference is not important.

The higher level JointTrajectoryController exists to provide convenience for you, 1) it lets you specify a sequence of waypoints, and 2) it allows you to describe the time element for the arm's movement. In contrast, the lower level JointController is a "get to this velocity right now" cmd interface.

Lastly, the JointTrajectoryController is an Action Server. Your code is not really intended to directly subscribe to the goal topic. You can begin learning about actionlibat http://wiki.ros.org/actionlib

edit flag offensive delete link

Comments

A ha, so instead of subscribing to the goal topic. The write() in hardware_interface is communicating with the real robot with for instance vel_command ( for example if JointHandle is defined like this:

hardware_interface::JointHandle vel_handle_jnt0( state_handle_jnt0, &vel_command[0]); )

Then, the update() in hardware_interface is automatically going through the interpolation points by analyzing the real robot state from read(). And the Action Client is for example the move_group_node defining the goal/waypoints. My only task is then just to make sure that the lower level JointController properly "gets the joint to the computed velocity"?

xman236 ( 2021-10-21 08:29:06 -0600 )edit

Yes. For real hardware, you often just call a function provided by the hardware manufacturer.

Mike Scheutzow ( 2021-10-22 06:23:26 -0600 )edit

Update: By echoing the vel_command, I have checked that the vel_commands deviate from the values in /robot_arm_controller_trajectory/follow_joint_trajectory/goal. Is this normal? Is it necessary that I extra modify/publish the /robot_arm_controller_trajectory/command messages using /robot_arm_controller_trajectory/follow_joint_trajectory/goal ?

xman236 ( 2021-10-29 06:58:24 -0600 )edit

Have you studied the ros_control diagram on this page? I suggest you figure out how the boxes relate to your setup. You may not have all those boxes in your setup.

It may or may not be a problem that vel_command does not match the current element in your specified trajectory. 1) There is a feedback loop in play, so the next calculated value of vel_command for a specific joint depends on the current velocity, how far the joint is the from target position, and how much time it has to get to the target position, 2) It depends on whether you specify position-only, or position+velocity for each element.

You should not try to inject new commands into different levels of the software stack during a single movement unless your intent is to confuse the software and cause problems. If the ActionServer is driving command_message input, don't interfere ...(more)

Mike Scheutzow ( 2021-11-02 09:48:50 -0600 )edit

You may want to temporarily switch to using a position_controller, as it is easier to debug. This of course will change the input of your hardware_interface from a joint velocity to a joint position.

Mike Scheutzow ( 2021-11-02 10:59:20 -0600 )edit

add a comment

Understanding JointTrajectoryController of ros_control

1 Answer

Comments

Question Tools

Stats

Related questions

Understanding JointTrajectoryController of ros_control edit

1 Answer

Comments

Question Tools

Stats

Related questions

Understanding JointTrajectoryController of ros_control