Rosanswers logo

I am having trouble executing a simple Point-To-Point movement-plan which was calculated by a planning_interface::MotionPlanRequest. The trajectory calculation succeeds, but when trying to execute it nothing is working. Apparently I am fetching an incorrect robot_state, even though the actual robot in Rviz is in valid, different position. My guess is, that it is an issue with the planning_scene I am refering. My other applications working with joint space goals as well as cartesian paths work fine, the issue seems to be in absolute cartesian space as my trajectory seems to be computed from a static pose of the robot which is never updated subsequently (see my update at the bottom)


  • ROS Distro: Kinetic
  • OS Version: Ubuntu 16.04
  • Binary build, latest

Steps to reproduce

The code is quite similar to the one in the Motion-Planning-API tutorials, the only difference is that I am using a different robot, try it with your own robot: https://docs.ros.org/kinetic/api/moveit_tutorials/html/doc/motion_planning_api/motion_planning_api_tutorial.html

Expected behaviour

The planner (I am using OMPL RRT::geometric) is given a request, which should contain the start state of my robot, derived from the planningScene in use, as is explained here: http://docs.ros.org/melodic/api/moveit_msgs/html/definePlanningRequest.html

Actual behaviour

The robot is supposed to execute the calculated trajectory, but when asked to do so, I am given the following Error-Message in the terminal, where my planning_execution.launch (rviz) is running:

Invalid Trajectory: start point deviates from current robot state more than 0.01 joint 'joint_b': expected: 0, current: -1.81088

As I already said, this might be an issue with the planning_scene, as it is apparently not containing the robot_state which is displayed in rviz, but this is just my guess. It's worth noting that I have two planning scene's in the rviz dropdown menu being displayed, maybe this is causing the issue:

  • /move_group/monitored_planning_scene
  • planning_scene

Another info I can give is that I am using the trac_ik kinematics solver


` planning_interface::MotionPlanRequest req; planning_interface::MotionPlanResponse res;

moveit::planning_interface::MoveGroupInterface move_group("gripper");
robot_model_loader::RobotModelLoader robot_model_loader("robot_description");
robot_model::RobotModelPtr robot_model = robot_model_loader.getModel();
robot_state::RobotStatePtr robot_state(new robot_state::RobotState(robot_model));
const robot_state::JointModelGroup* joint_model_group = robot_state->getJointModelGroup(PLANNING_GROUP);

/* robot_state::RobotState start_state(*move_group.getCurrentState());
start_state.setFromIK(joint_model_group, move_group.getCurrentPose().pose);
move_group.setStartState(start_state); */

// Refering the robot's environment from different topics through Moveit! messages:
planning_scene::PlanningScenePtr planning_scene(new planning_scene::PlanningScene(robot_model));
planning_scene->getCurrentStateNonConst().setToDefaultValues(joint_model_group, "ready");

// Loading a valid planner from the parameter sever:
boost::scoped_ptr<pluginlib::ClassLoader<planning_interface::PlannerManager>> planner_plugin_loader;
planning_interface::PlannerManagerPtr planner_instance;

    planner_plugin_loader.reset(new pluginlib::ClassLoader<planning_interface::PlannerManager>("moveit_core", "planning_interface::PlannerManager"));
catch (pluginlib::PluginlibException& ex)
    ROS_FATAL_STREAM("Exception while creating planning plugin loader " << ex.what());
    if (!planner_instance->initialize(robot_model, nh_.getNamespace()))
        ROS_FATAL_STREAM("Could not initialize planner instance");
        ROS_INFO_STREAM("Using planning interface '" << planner_instance->getDescription() << "'");
catch (pluginlib::PluginlibException& ex)
    const std::vector<std::string>& classes = planner_plugin_loader->getDeclaredClasses();
    std::stringstream ss;

    for (std::size_t i = 0; i < classes.size(); ++i)
        ss << classes[i] << " ";
        ROS_ERROR_STREAM("Exception while loading planner "  ": " << ex.what() << std::endl << "Available plugins: " << ss.str());

// Constraints:
double goal_coordinates[7] = {0.251, 0.638, 1.357, -0.707, 0.041, 0.032, 0.706};
geometry_msgs::PoseStamped target_pose = class.movePTP(goal_coordinates, "base_link");
std::vector<double> tolerance_pose(3, 0.1);
std::vector<double> tolerance_angle(3, 0.1);
moveit_msgs::Constraints pose_goal = kinematic_constraints::constructGoalConstraints("link_grip", target_pose, tolerance_pose, tolerance_angle);
moveit_msgs::Constraints path_constraints;

geometry_msgs::QuaternionStamped quaternion_constraint;
quaternion_constraint.header.frame_id = "base_link";
quaternion_constraint.quaternion = move_group.getCurrentPose().pose.orientation;

req.group_name = PLANNING_GROUP;
req.allowed_planning_time = 60.0;
req.path_constraints = kinematic_constraints::constructGoalConstraints("link_grip", quaternion_constraint);
req.workspace_parameters.min_corner.x = req.workspace_parameters.min_corner.y = req.workspace_parameters.min_corner.z = -2.0;
req.workspace_parameters.max_corner.x = req.workspace_parameters.max_corner.y = req.workspace_parameters.max_corner.z = 2.0;

// Taking constraints into account and computing a valid path:
planning_interface::PlanningContextPtr context = planner_instance->getPlanningContext(planning_scene, req, res.error_code_);

// Control mechanism with potential execution:
if(res.error_code_.val != res.error_code_.SUCCESS)
    ROS_INFO("Could not compute a valid path.", action_name_.c_str());
    ROS_INFO("%s: Succeeded. Managed to compute a valid path!", action_name_.c_str());

    moveit::planning_interface::MoveGroupInterface::Plan plan;
    moveit_msgs::MotionPlanResponse response;
    // plan.start_state_ = start_state;
    plan.trajectory_ = response.trajectory;

Robot Specifics:

I am using a Yaskawa hc10 with the coresponding motoman-driver, which can be found here: https://github.com/ros-industrial/motoman.git

In my usecase, i built myself a moveit support/config package, which has been exactly derived from the one used for the panda-robot used in the moveit tutorials. The packages structure is EXACTLY the same, of course I adapted some configurations and the URDF. Sadly I cannot share the package for contract reasons with Yaskawa. The package for the panda robot can be found here: https://github.com/ros-planning/panda_moveit_config.git

Nodes & Setup:

I embedded the code I shared with you in an action-server, because I am mainly programming the robot through SMACH, which is not the case right now for debugging reasons:

roslaunch hc10_setup planning_execution.launch (has the same name in the panda package)
rosrun manipulator ptp_server
rosrun manipulator ptp_client
rostopic list

The rqt screenshot is appended to this post EDIT: Wtf I can't append any files because I have less than 5 points. Any idea how I can upload my rqt-screenshot here ?

Here is the output of my launch:

This code block was moved to the following github gist: https://gist.github.com/answers-se-migration-openrobotics/ad39759cba2ce269bd4a8561c5d1ff2c



Any help is greatly appreciated as I've been having trouble with this issue for days. I am an intermediate (even though I am beginning to doubt myself slowly but surely) ROS-user, so don't hold back asking me more complicated things to try which can help solving this.

UPDATE: It is working when I am moving the simulated robot out of its initial state! When I am trying to move the simulated robot again, the same error as mentioned above happening when trying to move the real robot appears. So obviously my start state is never update somehow in the planning_scene I am listening to. Any idea what the issue could be ?C:\fakepath\rqt_debug.png

Originally posted by Dheroplasma on ROS Answers with karma: 11 on 2020-04-25

Post score: 0


1 Answer 1


Rosanswers logo

After everything is working, I will add a more detailed answer on what was causing issues and how I fixed most of them:

Basically, what I did was checking my moveit_config and moveit_source packages from scratch. After measuring my real robot I noticed that my robot was sitting about 3,5 cm lower in my URDF than the real one. I tried two things:

  • Changing move_group.setPoseReferenceFrame("base_link") to "robot_base", as my robot is sitting on a mobile platform. It did the trick but I rather considered this to be a dirty workaround
  • Rearranging my URDF and XACRO properly with a new base-mesh as well as running the moveit-setup-assistant as described in the tutorials (https://docs.ros.org/kinetic/api/moveit_tutorials/html/doc/setup_assistant/setup_assistant_tutorial.html) on my config package in order to update my SRDF. I also renamed the joints to their original names.

Short version: Working with absolute coordinates in cartesian space is not working when you interact with Moveit! while your robot control has deviating positions

Originally posted by Dheroplasma with karma: 11 on 2020-05-06

This answer was ACCEPTED on the original site

Post score: 0

Original comments

Comment by gvdhoorn on 2020-05-06:\

Working with absolute coordinates in cartesian space is not working when you interact with Moveit! while your robot control has deviating positions

you mean when your real hardware doesn't correspond with the model you feed the motion planner?

Comment by Dheroplasma on 2020-05-06:
Yes exactly, this would be the proper wording


Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.