Motion Planner unable to retreive the actual Robot State from the Planning Scene
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 ...
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.Thanks for your answer, I will update my question with further information about my robot and packages
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.
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.
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 theroslaunch
output you don't have those.From this btw:
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.I guess, sorry for the wording. I did as you requested
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
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 regularMoveGroupInterface
class to submit plans to a remotemove_group
.