0
$\begingroup$

Rosanswers logo

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?

  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
---

Originally posted by xman236 on ROS Answers with karma: 51 on 2021-10-20

Post score: 0

$\endgroup$

1 Answer 1

0
$\begingroup$

Rosanswers logo

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 actionlib at http://wiki.ros.org/actionlib


Originally posted by Mike Scheutzow with karma: 4903 on 2021-10-21

This answer was ACCEPTED on the original site

Post score: 2


Original comments

Comment by xman236 on 2021-10-21:
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"?

Comment by Mike Scheutzow on 2021-10-22:
Yes. For real hardware, you often just call a function provided by the hardware manufacturer.

Comment by xman236 on 2021-10-29:
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 ?

Comment by Mike Scheutzow on 2021-11-02:
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 until it says it is done.

Comment by Mike Scheutzow on 2021-11-02:
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.

$\endgroup$

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.