Ask Your Question
1

How do I enable approximate solutions programmatically with Moveit?

asked 2016-01-14 16:35:47 -0500

crunchex gravatar image

updated 2016-09-28 20:36:48 -0500

130s gravatar image

I have a 5-DOF arm that works just fine in the RViz motion planning plugin. Solutions are found almost immediately when using the interactive markers with query goal state. The key to this is checking "Allow Approximate IK Solutions", or nothing works.

Now I'm running into the problem of nothing working when I try to plan toward target poses programmatically. I believe I've narrowed down how the motion planning plugin does it successfully to these key lines:

  kinematics::KinematicsQueryOptions o = query_goal_state_->getKinematicsQueryOptions();
  o.return_approximate_solution = true;
  query_goal_state_->setKinematicsQueryOptions(o);

But I don't have enough understanding of how the RobotInteraction and KinematicOptions are set up to be able to set the flag in my own node. Can anyone offer an explanation or some guidance as to what else I'm missing for the set up? My complete test node below:

#include <moveit/move_group_interface/move_group.h>
#include <moveit/robot_interaction/robot_interaction.h>
#include <moveit/robot_interaction/interaction_handler.h>

int main(int argc, char **argv)
{
  ros::init(argc, argv, "move_test");
  ros::NodeHandle node_handle;
  ros::AsyncSpinner spinner(1);
  spinner.start();

  moveit::planning_interface::MoveGroup group("right_arm");

  //ROS_INFO("Reference frame: %s", group.getPlanningFrame().c_str());

  // Enable Approximate Solutions - how to get this to work?
  // kinematics::KinematicsQueryOptions o = query_goal_state_->getKinematicsQueryOptions();
  // o.return_approximate_solution = true;
  // query_goal_state_->setKinematicsQueryOptions(o);

  // Planning to a Pose goal
  geometry_msgs::PoseStamped poseStamped = group.getCurrentPose();
  geometry_msgs::Pose target_pose1 = poseStamped.pose;
  target_pose1.orientation.w = target_pose1.orientation.w;
  target_pose1.position.x = target_pose1.position.x + 0.01;
  target_pose1.position.y = target_pose1.position.y;
  target_pose1.position.z = target_pose1.position.z - 0.01;
  group.setPoseTarget(target_pose1);

  moveit::planning_interface::MoveGroup::Plan my_plan;
  bool success = group.plan(my_plan);

  ros::shutdown();
  return 0;
}
edit retag flag offensive close merge delete

Comments

I think you are looking at the wrong direction. When moving your arm in rviz you also rotate your gripper in a valid direction. In your program code you only do a translation, possibly ending up with a completely invalid rotation. Try to reach a full 6dof position that you know works.

Humpelstilzchen gravatar imageHumpelstilzchen ( 2016-01-15 01:07:27 -0500 )edit

I was under the impression that enabling approximate solutions would iron that out. Until I check that box in Rviz, I can't even drag the goal state for my end effector. I have also tried tweaking the goal tolerance but it didn't seem to have an effect - like it only helps during the planning step.

crunchex gravatar imagecrunchex ( 2016-01-15 08:40:14 -0500 )edit

My assumption: By dragging your eef you make only small changes which are easier to approximate. Something is probably iterating a few cm and degree around this position until a solution is found. If you would make one big movement it would fail.

Humpelstilzchen gravatar imageHumpelstilzchen ( 2016-01-15 10:09:45 -0500 )edit

1 Answer

Sort by ยป oldest newest most voted
0

answered 2016-09-28 20:41:04 -0500

130s gravatar image

See this thread. Possible solution in both C++ and Python is discussed.

edit flag offensive delete link more

Comments

If I want OMPL to take over the planning and execution, where do I set that? Im a bit lost.

Mehdi. gravatar imageMehdi. ( 2019-07-30 12:33:37 -0500 )edit

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

2 followers

Stats

Asked: 2016-01-14 16:35:47 -0500

Seen: 814 times

Last updated: Sep 28 '16