Hi,
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; #https://searchcode.com/file/97976562/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller_impl.h/
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);
558
559 if (update_ok)
560 {
561 // Accept new goal
562 preemptActiveGoal();
563 gh.setAccepted();
564 rt_active_goal_ = rt_goal;
565
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 }
579}
Link to open rosject: http://www.rosject.io/l/12a500e1/
The error message can be recreated by following the beneath steps in their respective shells;
- Spawning manipulator in empty initiated gazebo world
roslaunch r7_description spawn2.launch
- Launching the moveit configuration to be able to control manipulator
roslaunch r7_moveit_config r7_planning_execution.launch
- 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.
Regards,
Pål