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/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
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 << "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.goal_constraints.push_back(pose_goal);
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_);
context->solve(res);
// 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());
}
else
{
ROS_INFO("%s: Succeeded. Managed to compute a valid path!", action_name_.c_str());
moveit::planning_interface::MoveGroupInterface::Plan plan;
move_group.setMaxVelocityScalingFactor(0.05);
moveit_msgs::MotionPlanResponse response;
res.getMessage(response);
// plan.start_state_ = start_state;
plan.trajectory_ = response.trajectory;
move_group.execute(plan);
}`
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
rqt_graph
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
Topics:
/attached_collision_object
/collision_object
/display_robot_state
/execute_trajectory/cancel
/execute_trajectory/feedback
/execute_trajectory/goal
/execute_trajectory/result
/execute_trajectory/status
/feedback_states
/head_mount_kinect/depth_registered/points
/joint_path_command
/joint_states
/joint_trajectory_action/cancel
/joint_trajectory_action/feedback
/joint_trajectory_action/goal
/joint_trajectory_action/result
/joint_trajectory_action/status
/move_group/cancel
/move_group/display_contacts
/move_group/display_planned_path
/move_group/feedback
/move_group/filtered_cloud
/move_group/goal
/move_group/monitored_planning_scene
/move_group/ompl/parameter_descriptions
/move_group/ompl/parameter_updates
/move_group/plan_execution/parameter_descriptions
/move_group/plan_execution/parameter_updates
/move_group/planning_scene_monitor/parameter_descriptions
/move_group/planning_scene_monitor/parameter_updates
/move_group/result
/move_group/sense_for_plan/parameter_descriptions
/move_group/sense_for_plan/parameter_updates
/move_group/status
/move_group/trajectory_execution/parameter_descriptions
/move_group/trajectory_execution/parameter_updates
/ompl/parameter_descriptions
/ompl/parameter_updates
/pickup/cancel
/pickup/feedback
/pickup/goal
/pickup/result
/pickup/status
/place/cancel
/place/feedback
/place/goal
/place/result
/place/status
/planning_scene
/planning_scene_world
/ptp_action/cancel
/ptp_action/feedback
/ptp_action/goal
/ptp_action/result
/ptp_action/status
/recognized_object_array
/robot_status
/rosout
/rosout_agg
/rviz_bioslos_26740_8937627090099414375/motionplanning_planning_scene_monitor/parameter_descriptions
/rviz_bioslos_26740_8937627090099414375/motionplanning_planning_scene_monitor/parameter_updates
/rviz_moveit_motion_planning_display/robot_interaction_interactive_marker_topic/feedback
/rviz_moveit_motion_planning_display/robot_interaction_interactive_marker_topic/update
/rviz_moveit_motion_planning_display/robot_interaction_interactive_marker_topic/update_full
/rviz_visual_tools
/rviz_visual_tools_gui
/stereo/points2
/tf
/tf_static
/trajectory_execution_event
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