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
#include
#include
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;
}
↧