Error: "Failed to Stop Motion" when using Joint Trajectory Streaming with Motoman

edit

I'm using the Motoman SIA5D robot with the FS100 controller and ROS Kinetic on Ubuntu 16.04.

I want to move my robot along a trajectory until a new trajectory is desired, and then begin moving along the new one. I'm using the motion streaming interface in the motoman_driver package to send these trajectories to the robot. I send the trajectories as a FollowJointTrajectoryActionGoal msg to joint_trajectory_action/goal topic, and when I want to cancel that trajectory I send the corresponding GoalID to the joint_trajectory_action/cancel topic. This works some of the time, though sometimes when trying to cancel the current trajectory, I will get the following error, and the robot will not stop moving along the current trajectory.

Failed to Stop Motion: Failed(2) Unknown(0)

Any ideas on why the robot fails to stop? I wrote the following very basic test which generates and sends a trajectory to rotate the base joint one direction for 3 seconds, send a cancel request, waits for 0.3 seconds, then makes and sends a new trajectory to rotate the base joint in the opposite direction, and so on. The current_state variable holds the most recently received joint angles from the joint_states topic.

int main(int argc, char **argv){
  ros::init(argc, argv, "joint_trajectory_action_test1");
  ros::AsyncSpinner spinner(1);
  spinner.start();
  ros::NodeHandle nh;
  ros::Publisher goal_pub = nh.advertise<control_msgs::FollowJointTrajectoryActionGoal>("joint_trajectory_action/goal", 1);
  ros::Publisher cancel_pub = nh.advertise<actionlib_msgs::GoalID>("joint_trajectory_action/cancel", 1);
  ros::Subscriber state_sub = nh.subscribe("/joint_states", 1000, jointStateCallback);

  ros::Duration(5).sleep();//give robot time to connect


  control_msgs::FollowJointTrajectoryActionGoal traj_action;
  traj_action.goal.trajectory.joint_names = SIA5D_JOINT_NAMES;
  int current_id = 0;
  std::vector<double> temp_state = current_state;
  std::vector<double> vels;
  while(ros::ok()){
    traj_action.goal_id.id = "trajectory"+std::to_string(current_id);
    temp_state = current_state;
    vels = {std::pow(-1,current_id)*0.2, 0,0,0,0,0,0};
    addPointToTrajectory(traj_action.goal.trajectory, temp_state, vels, 0);
    for(int i = 0; i < 10; i++){
      temp_state[0]+=std::pow(-1,current_id)*0.1;
      addPointToTrajectory(traj_action.goal.trajectory, temp_state, vels, 0.5*(i+1));
    }
    goal_pub.publish(traj_action);
    //wait 3 seconds and cancel trajectory
    ros::Duration(3).sleep();
    cancel_pub.publish(traj_action.goal_id);
    clearTrajectory(traj_action.goal.trajectory);
    current_id++;
    ros::Duration(0.3).sleep();
  }
  return 0;
}