Joint Trajectory Action Controller

Overview

The Joint Trajectory Action Controller executes joint-space trajectories on a set of joints. It takes, as a command, a message specifying the desired position and velocity of each joint at every point in time, and executes this command as well as the mechanism allows.

The Joint Trajectory Action Controller takes the trajectory points and interpolates between them using splines. If you've specified accelerations, it will use quintic splines, otherwise if you've specified velocities, it will use cubic splines. (If you specify neither accelerations nor velocities it will interpolate between the positions linearly, however specifying only positions is discouraged, as the linear interpolation will lead to discontinuities in the followed trajectory).

ROS API

Parameters

• The list of joints to control

• The position gain for joint <joint>. (There should be a p gain for every joint listed in the joints parameter)

• The position derivative gain for joint <joint>

• The integral gain around position for joint <joint>

• The clamp of the integral gain for joint <joint>

• The approximate inertia for joint <joint>

• Bandwidth of proxy reconvergence

• Acceleration of proxy reconvergence

• Velocity limit (enforced by the proxy)

• Acceleration limit (enforced by the proxy)

• If the timestamp of the goal trajectory point is t, then following the trajectory succeeds if it reaches the goal within t +/- goal_time, and aborts otherwise.

• Position tolerance for a particular joint to reach the goal. When the joint is within goal_position +/- goal_tolerance, than the trajectory can succeed.

• Position tolerance for a particular joint throughout the trajectory. If the joint position every falls outside trajectory_position +/- tolerance, then the trajectory aborts.

Example configuration (from pr2_controller_configuration/pr2_arm_controllers.yaml):

r_arm_controller:
  type: "robot_mechanism_controllers/JointTrajectoryActionController"
  joints:
    - r_shoulder_pan_joint
    - r_shoulder_lift_joint
    - r_upper_arm_roll_joint
    - r_elbow_flex_joint
    - r_forearm_roll_joint
    - r_wrist_flex_joint
    - r_wrist_roll_joint
  gains:
    r_shoulder_pan_joint:
      p: 2400.0
      d: 18.0
      i: 800.0
      i_clamp: 4.0
      mass: 3.3
      proxy:
        lambda: 3.0
        acc_converge: 1.5
        vel_limit: 2.088
        effort_limit: 11.8
    r_shoulder_lift_joint:
      p: 1200.0
      d: 10.0
      i: 700.0
      i_clamp: 4.0
      mass: 2.0
      proxy:
        lambda: 3.0
        acc_converge: 1.5
        vel_limit: 2.082
        effort_limit: 8.0
    r_upper_arm_roll_joint: {p: 1000.0, d: 6.0, i: 600.0, i_clamp: 4.0}
    r_elbow_flex_joint: {p: 700.0, d: 4.0, i: 450, i_clamp: 4.0}
    r_forearm_roll_joint: {p: 300.0, d: 6.0, i: 300, i_clamp: 2.0}
    r_wrist_flex_joint: {p: 300.0, d: 4.0, i: 300, i_clamp: 2.0}
    r_wrist_roll_joint: {p: 300.0, d: 4.0, i: 300, i_clamp: 2.0}
  joint_trajectory_action_node:
    joints:
      - r_shoulder_pan_joint
      - r_shoulder_lift_joint
      - r_upper_arm_roll_joint
      - r_elbow_flex_joint
      - r_forearm_roll_joint
      - r_wrist_flex_joint
      - r_wrist_roll_joint
    constraints:
      goal_time: 0.6
      r_shoulder_pan_joint:
        goal: 0.02
      r_shoulder_lift_joint:
        goal: 0.02
      r_upper_arm_roll_joint:
        goal: 0.02
      r_elbow_flex_joint:
        goal: 0.02
      r_forearm_roll_joint:
        goal: 0.02
      r_wrist_flex_joint:
        goal: 0.02
      r_wrist_roll_joint:
        goal: 0.02

Sending trajectories

The primary way to send trajectories to the Joint Trajectory Action Controller is by connecting to the control_msgs::FollowJointTrajectoryAction on the follow_joint_trajectory topic. The action goal specifies the trajectory, and (optional) tolerances, which override the tolerances given on the parameter server. See the action definition for more information on what to send.

Advanced behaviors

Trajectory Replacement

The controller provides an interface that allows you to interrupt only part of the running trajectory.

When you send the controller a new trajectory_msgs/JointTrajectory message, it will splice the trajectory in at the time given in the message header (t*). At t*, the controller will switch from the previous trajectory to following the new trajectory. It is the user's responsibility to ensure that the trajectories fit together by making sure the positions and velocities are continuous at t*. The query_state service call helps by informing the user what the positions and velocities of the current trajectory will be at a given time.

Partial Execution

There are two ways of preventing trajectories from executing to the end. Sending an empty trajectory will preempt the current trajectory, however, this practice has a serious problem: if the node that sends the "abort" message goes down, then the trajectory cannot be aborted. Since the node sending the abort message is likely the node enforcing the safety of the system, this means that the trajectory is being executed without any safety constraints.

The solution is to send only the "cleared" parts of the trajectory to the controller. The controllers will execute what it receives, but if you stop sending it the rest of the trajectory, then it will stop the mechanism. The commanding node sends small chunks of the trajectory, possibly only 50ms into the future, after checking that these chunks are "safe" to execute.