Simple RRTConnect planning fails
Hello, I am trying to use the MoveIt package to generate the joint values for a pose/position target. Everyone time I try to plan a goal state the OMPL RRTConnect fails stating its unable to sample any valid states for the goal tree.
I know this is a common issue and the first question is always: " is the goal state truly reachable?" And I believe mine is.. the goal state I specify is only sliding forward with one joint. (Hopefully the code and picture will show this).
My process is in one terminal I run demo.launch (from a package I generated in setup_assistant) and in a second terminal I run my node.
Can anyone provide insight into what I may be doing wrong? Any advice would be greatly appreciated!!
Code:
#include <pluginlib/class_loader.h>
#include <ros/ros.h>
// MoveIt!
#include <moveit/move_group_interface/move_group_interface.h>
#include <moveit/planning_scene_interface/planning_scene_interface.h>
#include <moveit_msgs/DisplayRobotState.h>
#include <moveit_msgs/DisplayTrajectory.h>
#include <moveit_msgs/AttachedCollisionObject.h>
#include <moveit_msgs/CollisionObject.h>
#include <moveit_visual_tools/moveit_visual_tools.h>
#include "std_msgs/Float32MultiArray.h"
#include "std_msgs/String.h"
ros::Publisher paramPub;
ros::Publisher display_publisher;
ros::Publisher marker_pub;
int main(int argc, char * argv[]){
ros::init(argc, argv, "move_group_interface_tutorial");
ros::NodeHandle node_handle;
ros::AsyncSpinner spinner(1);
spinner.start();
static const std::string PLANNING_GROUP = "group1";
moveit::planning_interface::MoveGroupInterface move_group(PLANNING_GROUP);
moveit::planning_interface::PlanningSceneInterface planning_scene_interface;
const robot_state::JointModelGroup* joint_model_group =
move_group.getCurrentState()->getJointModelGroup(PLANNING_GROUP);
// Visualization
// ^^^^^^^^^^^^^
// The package MoveItVisualTools provides many capabilties for visualizing objects, robots,
// and trajectories in RViz as well as debugging tools such as step-by-step introspection of a script
namespace rvt = rviz_visual_tools;
moveit_visual_tools::MoveItVisualTools visual_tools("end");
visual_tools.deleteAllMarkers();
// Remote control is an introspection tool that allows users to step through a high level script
// via buttons and keyboard shortcuts in RViz
visual_tools.loadRemoteControl();
// RViz provides many types of markers, in this demo we will use text, cylinders, and spheres
Eigen::Isometry3d text_pose = Eigen::Isometry3d::Identity();
text_pose.translation().z() = 1.75;
visual_tools.publishText(text_pose, "MoveGroupInterface Demo", rvt::WHITE, rvt::XLARGE);
// Batch publishing is used to reduce the number of messages being sent to RViz for large visualizations
visual_tools.trigger();
// Getting Basic Information
// ^^^^^^^^^^^^^^^^^^^^^^^^^
ROS_INFO_NAMED("tutorial", "Planning frame: %s", move_group.getPlanningFrame().c_str());
move_group.setPoseReferenceFrame("end");
// We can also print the name of the end-effector link for this group.
ROS_INFO_NAMED("tutorial", "End effector link: %s", move_group.getEndEffectorLink().c_str());
// We can get a list of all the groups in the robot:
ROS_INFO_NAMED("tutorial", "Available Planning Groups:");
//std::copy(move_group.getJointModelGroupNames().begin(), move_group.getJointModelGroupNames().end(),
// std::ostream_iterator<std::string>(std::cout, ", "));
std::cout << move_group.getEndEffectorLink() << std::endl;
geometry_msgs::Pose target_pose1;
target_pose1.orientation.w = 1.0;
target_pose1.position.x = 0.35;
target_pose1.position.y = 0.0;
target_pose1.position.z = 0.0;
move_group.setPoseTarget(target_pose1, "end");
move_group.move();
//move_group.setPositionTarget(0.3, 0.0, 0.0, "end");
// Now, we call the planner to compute the plan and visualize it.
// Note that we are just planning, not asking move_group
// to actually move the robot.
//moveit::planning_interface::MoveGroupInterface::Plan my_plan;
//bool success = (move_group.plan(my_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS);
// Visualizing ...
You might want to explicitly set the other quaternion values in
target_pose1
. You are only explicitly settingw
, which means that the otherx
,y
,z
values might be default initialized to some garbge. A mucked-up rotation could easily cause the goal to out of reach for your robot.@BryceWilley thank you for the response. I made the changes to ensure no random data is being set and still the same issue.
The only other missing piece here is what's in the robot's SRDF and in demo.launch. From the
Could not identify parent group for end-effector 'end'
, it could be that your planning group is not set up correctly.