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;
}
Originally posted by crunchex on ROS Answers with karma: 376 on 2016-01-14
Post score: 1
Original comments
Comment by Humpelstilzchen on 2016-01-15:
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.
Comment by crunchex on 2016-01-15:
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.
Comment by Humpelstilzchen on 2016-01-15:
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.
