Robotics StackExchange | Archived questions

unable to sample any valid states for goal tree. (moveit2 Humble)

Hi all, I have installed the moveit2 from source as per the official moveit2 humble document. I was able to successfully run the panda arm and also simulate in rviz. With Panda arm, I was able to manipulate the arm at a certain location by giving a goal Pose.

Then I tried converting the urdf of "openmanipulatorx" arm to srdf. I was able to do it using "moveitsetupassistant". I was able to simulate it in Rviz. I was able to actuate it in Rviz as well. But when I tried giving a pose through a script, I got this error:

[move_group-3] [INFO] [1683889439.499061630] [moveit_move_group_default_capabilities.move_action_capability]: Received request
[move_group-3] [INFO] [1683889439.499475929] [moveit_move_group_default_capabilities.move_action_capability]: executing..
[move_group-3] [INFO] [1683889439.509899790] [moveit_move_group_default_capabilities.move_action_capability]: Planning request received for MoveGroup action. Forwarding to planning pipeline.
[move_group-3] type ################ geometric::RRTConnect
[move_group-3] [INFO] [1683889439.513308358] [moveit.ompl_planning.model_based_planning_context]: Planner configuration 'open_manipulator_arm' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed.
[move_group-3] [ERROR] [1683889444.517090934] [ompl]: ./src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:252 - open_manipulator_arm/open_manipulator_arm: Unable to sample any valid states for goal tree
[move_group-3] [WARN] [1683889444.517223990] [moveit.ompl_planning.model_based_planning_context]: Timed out
[move_group-3] [INFO] [1683889445.581082661] [moveit.ompl_planning.model_based_planning_context]: Unable to solve the planning problem
[move_group-3] [INFO] [1683889445.581306342] [moveit_move_group_default_capabilities.move_action_capability]: Timeout reached

I am using the launch file generated from the "setupassistant". To give the goalpose, I am using this code:

#include <memory>
#include <chrono>
#include <rclcpp/rclcpp.hpp>
#include <moveit/move_group_interface/move_group_interface.h>
#include <visualization_msgs/msg/marker.hpp>

int main(int argc, char *argv[])
{
  // Initialize ROS and create the Node
  rclcpp::init(argc, argv);
  auto const node = std::make_shared<rclcpp::Node>(
      "moveit",
      rclcpp::NodeOptions().automatically_declare_parameters_from_overrides(true));

  // Create a ROS logger
  auto const logger = rclcpp::get_logger("moveit");

  // Create the MoveIt MoveGroup Interface
  using moveit::planning_interface::MoveGroupInterface;
  auto move_group_interface = MoveGroupInterface(node, "open_manipulator_arm");
  move_group_interface.setMaxVelocityScalingFactor(0.10);
  // move_group_interface.setMaxAccelerationScalingFactor(0.10);
  // move_group_interface.setPlanningTime(5.0);
  // move_group_interface.setNumPlanningAttempts(10); 
  move_group_interface.setGoalTolerance(0.508458);




  // Set a target Pose
  auto const target_pose = []
  {
    geometry_msgs::msg::Pose msg;
    // msg.header.frame_id = "link1";
    msg.orientation.w = 1.0;
    // msg.orientation.x = 1e-6;
    // msg.orientation.y = 1e-6;
    // msg.orientation.z = 1e-6;

    // open_manipulator
    msg.position.x = -0.052064;
    msg.position.y = 0.0354723; //-0.2
    msg.position.z = 0.2781305;

    // msg.position.x = 0.28;
    // msg.position.y = 0.4; //-0.2
    // msg.position.z = 0.5;
    return msg;
  }();
  move_group_interface.setPoseTarget(target_pose);

  // Create a plan to that target pose
  auto const [success, plan] = [&move_group_interface]
  {
    moveit::planning_interface::MoveGroupInterface::Plan msg;
    printf("##########Planning########\n");
    auto const ok = static_cast<bool>(move_group_interface.plan(msg));
    return std::make_pair(ok, msg);
  }();

  // Execute the plan
  if (success)
  {
    move_group_interface.execute(plan);
  }
  else
  {
    RCLCPP_ERROR(logger, "Planing failed but will succeed! Keep trying!! ");
  }

  // Shutdown ROS
  rclcpp::shutdown();
  return 0;
}

In rviz I can move it to desire location but with this script, I am getting the above mentioned error. What can be the reason? The pose is not wrong. I am giving reachable location.

EDIT: Pandaarm is working as per expected. Same script with some linkname and location change works with panda_arm. I tried tolerance value as high as 1.5 but still it is not working.

Will changing planner config work? By default it is using "RRTconnect". Is other config supported in humble. Because when I am trying using different config for ompl planner, I am getting more errors.

Asked by Aki on 2023-05-12 05:47:50 UTC

Comments

Answers

I found the exact problem. The DoF of open_manipulator is 4 and the moveit2 library has IK solver which supports minimum 6 DoF. So I had to increase the DoF of my arm by adding two virtual links. Adding links solved the problem.

Asked by Aki on 2023-06-22 08:04:30 UTC

Comments