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 plans
// ^^^^^^^^^^^^^^^^^
// We can also visualize the plan as a line with markers in RViz.
ROS_INFO_NAMED("tutorial", "Visualizing plan 1 as trajectory line");
visual_tools.publishAxisLabeled(target_pose1, "target_pose1");
visual_tools.publishText(text_pose, "Pose Goal", rvt::WHITE, rvt::XLARGE);
//visual_tools.publishTrajectoryLine(my_plan.trajectory_, joint_model_group);
visual_tools.trigger();
//visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to continue the demo");
// Moving to a pose goal
// ^^^^^^^^^^^^^^^^^^^^^
/* Uncomment below line when working with a real robot */
/* move_group.move(); */
ros::shutdown();
return 0;
}
And here is an image of the pose_goal in RViz:
And here is an image showing that if I use the jointstatepublisher slider gui that I can reach the pose_goal:
And here is my URDF:
<?xml version="1.0" encoding="UTF-8"?>
<robot name="test_robot">
<link name="D" />
<link name="tilt" />
<link name="pan" />
<link name="end" />
<link name="baseLink">
<!--<inertial>
<origin xyz="0 0 0" rpy="0 0 0"/>
<mass value="1"/>
<inertia ixx="100" ixy="0" ixz="0" iyy="100" iyz="0" izz="100" />
</inertial>-->
</link>
<joint name="D" type="prismatic">
<parent link="baseLink" />
<child link="D" />
<limit effort="30" velocity="1.0" lower="0" upper="0.4500" />
<axis xyz="1 0 0" />
<origin xyz="0 0 0" rpy="0 0 0" />
</joint>
<joint name="thetaTilt" type="revolute">
<parent link="D" />
<child link="tilt" />
<limit effort="30" velocity="1.0" lower="-.5" upper=".5" />
<axis xyz="0 1 0" />
<origin xyz="0 0 0.0635" rpy="0 0 0" />
</joint>
<joint name="phiPan" type="revolute">
<parent link="tilt" />
<child link="pan" />
<limit effort="30" velocity="1.0" lower="-.9" upper=".9" />
<axis xyz="0 0 1" />
<origin xyz="0 0.0889 0" rpy="0 0 0" />
</joint>
<joint name="end" type="fixed">
<parent link="pan" />
<child link="end" />
<origin xyz="0.5588 0 0" rpy="0 0 0" />
</joint>
</robot>
And Here is the output from the terminal running demo.launch
[ INFO] [1568048392.751663999]: Loading robot model 'test_robot'...
[ WARN] [1568048392.751738764]: No geometry is associated to any robot links
[ WARN] [1568048392.751780066]: Could not identify parent group for end-effector 'end'
[ INFO] [1568048392.785106805]: Starting scene monitor
[ INFO] [1568048392.787732976]: Listening to '/move_group/monitored_planning_scene'
[ INFO] [1568048392.811340857]: Constructing new MoveGroup connection for group 'group1' in namespace ''
[ INFO] [1568048393.750722561]: Ready to take commands for planning group group1.
[ INFO] [1568048393.750953306]: Looking around: no
[ INFO] [1568048393.751075878]: Replanning: no
[ INFO] [1568048399.956913044]: Combined planning and execution request received for MoveGroup action. Forwarding to planning and execution pipeline.
[ INFO] [1568048399.958020452]: Planning attempt 1 of at most 1
[ INFO] [1568048399.961672096]: Planner configuration 'group1' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed.
[ INFO] [1568048399.963288246]: RRTConnect: Starting planning with 1 states already in datastructure
[ERROR] [1568048404.971078802]: RRTConnect: Unable to sample any valid states for goal tree
[ INFO] [1568048404.971140463]: RRTConnect: Created 1 states (1 start + 0 goal)
[ INFO] [1568048404.971214635]: No solution found after 5.008364 seconds
[ INFO] [1568048404.991413882]: Unable to solve the planning problem
And here is the output from the terminal running m node:
[ INFO] [1568048398.145399968]: Loading robot model 'test_robot'...
[ WARN] [1568048398.146420670]: No geometry is associated to any robot links
[ WARN] [1568048398.146476044]: Could not identify parent group for end-effector 'end'
[ INFO] [1568048398.237453827]: Loading robot model 'test_robot'...
[ WARN] [1568048398.238001746]: No geometry is associated to any robot links
[ WARN] [1568048398.238089516]: Could not identify parent group for end-effector 'end'
[ INFO] [1568048399.348285005]: Ready to take commands for planning group group1.
[ INFO] [1568048399.669554589]: RemoteControl Ready.
[ INFO] [1568048399.948613767]: Planning frame: /baseLink
[ INFO] [1568048399.948914032]: End effector link: end
[ INFO] [1568048399.949158560]: Available Planning Groups:
end
[ INFO] [1568048404.991850290]: ABORTED: No motion plan found. No execution attempted.
[ INFO] [1568048404.991899784]: Visualizing plan 1 as trajectory line
Asked by PatFGarrett on 2019-09-09 12:02:13 UTC
Comments
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.Asked by BryceWilley on 2019-09-11 09:50:06 UTC
@BryceWilley thank you for the response. I made the changes to ensure no random data is being set and still the same issue.
Asked by PatFGarrett on 2019-09-11 11:08:11 UTC
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.Asked by BryceWilley on 2019-09-13 08:13:06 UTC