Joint_trajectory_controller failed with error code INVALID_Goal


I am having some problems with a strange (or not wel ldefined) error message when using the CartesianPathPlanner of MoveIt! I’ve looked all over the internet and your tutorials, but have found no solution… I use your developer license, but I am not an extensive user of the robot ignite academy.

I have followed your tutorials for defining a manipulator, configuring Moveit and a gazebo environment, connecting the manipulator through using a joint_trajectory_controller, and by utilizing C++ I am able to do most of the features of moveit (such as moving to a pose, input joint angles etc.). Nevertheless, when implementing some pre-defined waypoints into the CartesianPathPlanner I am able to compute the plan by 100 % (the fraction of completed path), but when executing the defined path with all its waypoints I get a strange error message as seen below.

[ WARN] [1587989098.121210004, 985.526000000]: Controller r7/joint_trajectory_controller failed with error code INVALID_GOAL

[ WARN] [1587989098.127561604, 985.531000000]: Controller handle r7/joint_trajectory_controller reports status FAILED

If the waypoints are pushed through the planner one by one as poses, or the actual created trajectory positions (joint angles) at each trajectory point is pushed to the setJointAngles, they all work out without a problem, hence all created waypoints and trajectory points are feasible.

I’ve tried to redefine some of the trajectory following parameters etc. with no luck. Also changed PID gains, efforts on joints and so on. I am using an effort joint interface, but have also tested position joint interface with no luck. The closes I’ve come to find some kind of explanation to the error message is the following;

INVALID_GOAL: The reason for the invalid goal (e.g., the requested trajectory is in the past). This is mentioned in the control messages of the FollowJointTrajectory.

The same error message can be found in line 536 and 576 og the joint_trajectory_controller code found here; #
I think it might be the latter error message at line 576 that is happening. That the software is NOT able to update new trajectory (?) as can be seen in code snippet below.

// Try to update new trajectory
555  RealtimeGoalHandlePtr rt_goal(new RealtimeGoalHandle(gh));
556  const bool update_ok = updateTrajectoryCommand(internal::share_member(gh.getGoal(), gh.getGoal()->trajectory),
557                                                 rt_goal);
559  if (update_ok)
560  {
561    // Accept new goal
562    preemptActiveGoal();
563    gh.setAccepted();
564    rt_active_goal_ = rt_goal;
566    // Setup goal status checking timer
567    goal_handle_timer_ = controller_nh_.createTimer(action_monitor_period_,
568                                                    &RealtimeGoalHandle::runNonRealtime,
569                                                    rt_goal);
570    goal_handle_timer_.start();
571  }
572  else
573  {
574    // Reject invalid goal
575    control_msgs::FollowJointTrajectoryResult result;
576    result.error_code = control_msgs::FollowJointTrajectoryResult::INVALID_GOAL;
577    gh.setRejected(result);
578  }

Link to open rosject:

The error message can be recreated by following the beneath steps in their respective shells;

  1. Spawning manipulator in empty initiated gazebo world

roslaunch r7_description spawn2.launch

  1. Launching the moveit configuration to be able to control manipulator

roslaunch r7_moveit_config r7_planning_execution.launch

  1. Running motion planning scripts from C++ namely the run.cpp script that uses functions defined in planning.cpp and planning.hpp. Number 1 moves the manipulator to known starting position for where the movegroupnode is able to compute the cartesian path of the input points. Number 7 computes the cartesian path of the input waypoints that is defined in the csv file.

cd catkin_ws
rosrun motion_planning run 1
rosrun motion_planning run 7

Might be worth noting that when specifying a couple of waypoints, the ComputeCartesianPath works with no problem. Calculates 100% and the trajectory is also executed in the Gazebo simulation.
Been stuck on this problem for quite some time now… All ideas are greatly appreciated.


Never mind this post. Was an obvious mistake from my part… Used Matlab for path-planning creating the waypoints for the manipulator to follow. The jointTrajectoryController will state the error of not strictly time increasing points if any points are duplicates. In the path planning, two points for where the robot was turning around had duplicate values. If anybody gets the same error messages above or in the Gazebo log;

[ERROR] [1588244922.183046960, 2319.748000000]: Trajectory message contains waypoints that are not strictly increasing in time.

Please have a look at all defined waypoints. Typically duplicated waypoints at start, end or at “turns” (if other path planning tools have been used)

Regards, Pål

Hello @paaalhs,

I’m glad you figured out the error by yourself. Also, thanks for sharing the solution with everybody in case someone else experiences this same error.