Ask Your Question
0

Motion Planner unable to retreive the actual Robot State from the Planning Scene

asked 2020-04-25 03:14:38 -0500

Dheroplasma gravatar image

updated 2020-04-25 11:49:30 -0500

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)

Environment

  • 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/move...

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/movei...

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

Code

` 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;

try 
{
    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());
}
try
{
    planner_instance.reset(planner_plugin_loader->createUnmanagedInstance("ompl_interface/OMPLPlanner"));

    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 ...
(more)
edit retag flag offensive close merge delete

Comments

1

The code is only mildly interesting at this point. Much more important are your .launch files and a proper description of what robot you have, which ROS nodes you're using as a driver, how you've started those and whether the expected topics are subscribed and published to by the expected nodes.

gvdhoorn gravatar image gvdhoorn  ( 2020-04-25 04:52:04 -0500 )edit

Thanks for your answer, I will update my question with further information about my robot and packages

Dheroplasma gravatar image Dheroplasma  ( 2020-04-25 07:27:14 -0500 )edit

I cannot attach my rqt_screenshot becasue I "need 5 points". I added the rest as good as possible. I also found something helpful out and wrote an update at the bottom of my post.

Dheroplasma gravatar image Dheroplasma  ( 2020-04-25 08:22:10 -0500 )edit
1

I cannot attach my rqt_screenshot becasue I "need 5 points" wtf

there is nothing to "wtf" about: it's a simple anti-spam measure, which I can assure you is very much needed. I would appreciate it if you could not use expletives so gratuitously.


Edit: I've given you sufficient karma to post your screenshot. So please do that now.

gvdhoorn gravatar image gvdhoorn  ( 2020-04-25 08:23:58 -0500 )edit

Sadly I cannot share the package for contract reasons with Yaskawa.

The HC10 model has been supported for quite some time now in ros-industrial/motoman. I don't really see how a MoveIt configuration could contain information which would require an NDA. The only information about the robot which is not in the support package would be the acceleration and jerk limits. And from the parameters shown in the roslaunch output you don't have those.

From this btw:

/controller_joint_names: ['joint_s', 'join...

I have the impression you are not using ros-industrial/motoman/motoman_hc10_support. That's OK, but your current configuration will most likely not work (robustly) with real hardware. I suggest to compare your current urdf with the one in the official repository and rectify the differences.

gvdhoorn gravatar image gvdhoorn  ( 2020-04-25 08:27:01 -0500 )edit

I guess, sorry for the wording. I did as you requested

Dheroplasma gravatar image Dheroplasma  ( 2020-04-25 08:27:47 -0500 )edit

Sorry for sharing wrong info. I planted the robot on a mobile platform which I am not allowed to share. But I may ask the person responsible and do so later if it helps

Dheroplasma gravatar image Dheroplasma  ( 2020-04-25 08:29:41 -0500 )edit

As a high-level question: why are you loading a planner directly in your program, instead of using the regular move_group? Especially since you then still use the regular MoveGroupInterface class to submit plans to a remote move_group.

gvdhoorn gravatar image gvdhoorn  ( 2020-04-25 08:56:21 -0500 )edit

1 Answer

Sort by ยป oldest newest most voted
0

answered 2020-05-06 06:44:42 -0500

Dheroplasma gravatar image

updated 2020-05-06 06:47:20 -0500

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-assistantas described in the tutorials (https://docs.ros.org/kinetic/api/move...) 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

edit flag offensive delete link more

Comments

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?

gvdhoorn gravatar image gvdhoorn  ( 2020-05-06 06:51:30 -0500 )edit

Yes exactly, this would be the proper wording

Dheroplasma gravatar image Dheroplasma  ( 2020-05-06 07:10:28 -0500 )edit

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

1 follower

Stats

Asked: 2020-04-25 03:14:38 -0500

Seen: 391 times

Last updated: May 06 '20