Hey all!
I'm trying to use the moveit group interface tutorial to move my custom robot (3-links). I'm having problems in planning motion when I give an End-effector pose. I keep getting RRTConnect: Unable to sample any valid states for goal tree.
I have tried the following solutions but none of them seem to work:
1. Trying a different IK solver (Trac_ik)
2. Setting longer planning times
I have triple-checked my goals for valid end-eff positions, but it still keeps failing . It however works flawlessly when I provide joint-space goals instead. Below is my code. If anyone could tell me where I am going wrong?
// MoveIt! operates on sets of joints called "planning groups" and stores them in an object called
// the `JointModelGroup`. Throughout MoveIt! the terms "planning group" and "joint model group"
// are used interchangably.
static const std::string PLANNING_GROUP = "Body";
// The :move_group_interface:`MoveGroup` class can be easily
// setup using just the name of the planning group you would like to control and plan for.
moveit::planning_interface::MoveGroupInterface move_group(PLANNING_GROUP);
// We will use the :planning_scene_interface:`PlanningSceneInterface`
// class to add and remove collision objects in our "virtual world" scene
moveit::planning_interface::PlanningSceneInterface planning_scene_interface;
// Raw pointers are frequently used to refer to the planning group for improved performance.
/* const robot_state::JointModelGroup* joint_model_group =
move_group.getCurrentState()->getJointModelGroup(PLANNING_GROUP);*/
//-----------------------------
//Getting Basic Information
//-----------------------------
// We can print the name of the reference frame for this robot.
ROS_INFO_NAMED("moveff", "Reference frame: %s", move_group.getPlanningFrame().c_str());
// We can also print the name of the end-effector link for this group.
ROS_INFO_NAMED("moveff", "End effector link: %s", move_group.getEndEffectorLink().c_str());
bool success; //Movement success variable
// Planning to a Pose goal
// ^^^^^^^^^^^^^^^^^^^^^^^
// We can plan a motion for this group to a desired pose for the
// end-effector.
geometry_msgs::Pose target_pose1;
target_pose1.position.x = 0.227560;
target_pose1.position.y = -0.000002;
target_pose1.position.z = 0.372389;
target_pose1.orientation.x = -0.110141;
target_pose1.orientation.y = 0.110147;
target_pose1.orientation.z = -0.698475;
target_pose1.orientation.w = 0.698476;
move_group.setPoseTarget(target_pose1);
move_group.setPlanningTime(30);
// 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;
success = (move_group.plan(my_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS);
if(success == true) {
move_group.move();
ROS_INFO("Movement Done");
}
↧