Joint Spline Trajectory Controller

Overview

The Joint Spline Trajectory 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 Spline Trajectory 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>

Example configuration (from pr2_controller_configuration/pr2_arm_controllers.yaml):

r_arm_controller:
  type: JointSplineTrajectoryController
  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: 140.0, d: 30.0}
    r_shoulder_lift_joint: {p: 170.0, d: 30.0}
    r_upper_arm_roll_joint: {p: 300.0, d: 7.0}
    r_elbow_flex_joint: {p: 100.0, d: 5.0}
    r_forearm_roll_joint: {p: 300.0, d: 20.0}
    r_wrist_flex_joint: {p: 200.0, d: 8.0}
    r_wrist_roll_joint: {p: 200.0, d: 8.0}

Published Topics

• Current state of the controller, including the current and desired positions, velocities, and accelerations.

Subscribed Topics

• The trajectory to follow. The absolute start time of the trajectory is given in header.stamp. If positions, velocities, and accelerations are all specified, then the controller will interpolate using quintic splines. If only positions and velocities are specified, it will use cubic splines. If only positions are specified, it will interpolate linearly (though using this behavior is not recommended, as the trajectory will not be smooth).

Services

• The query_state service allows the user to inquire where the controller setpoint will be at a desired time.

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

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.