Using moveit movegroup could not solve an ik problem

asked 2016-04-05 00:32:17 -0500

Gure Ling gravatar image

updated 2016-04-05 00:47:50 -0500

I follow moveit_pr2 tutorial(https://github.com/ros-planning/moveit_pr2/tree/indigo-devel/pr2_moveit_tutorials/planning/src) to build a program for my own 4DOF scara robot. when using setPoseTarget() to set a pose target, the method plan() cannot compute a valid solution, but setJointValueTarget() works fine. It seems that it cannot solve the ik problem.

platform Ubuntu14.04 indigo

cpp file:

int main(int argc, char** argv) {

ros::init(argc, argv, "scara_arm_nav");

ros::NodeHandle node_handle;

    ros::AsyncSpinner spinner(1);

spinner.start();
sleep(0.0);

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

ROS_INFO("Reference frame: %s", group.getPlanningFrame().c_str());
ROS_INFO("End-effector link: %s", group.getEndEffectorLink().c_str());

moveit::planning_interface::MoveGroup::Plan my_plan;
bool success = false;
/**
std::vector<double> group_variable_values;
group.getCurrentState()->copyJointGroupPositions(group.getCurrentState()->getRobotModel()->getJointModelGroup(group.getName()), group_variable_values);

group_variable_values[0] = -1.0;
group.setJointValueTarget(group_variable_values);
*   */
/**
geometry_msgs::Pose target_pose1;

target_pose1.orientation.w = 1.0;
target_pose1.position.x = -0.3;
target_pose1.position.y = 0.0;
target_pose1.position.z = 0.20;

group.setPoseTarget(target_pose1, "link4");
*/
group.setPoseReferenceFrame(group.getPlanningFrame());
geometry_msgs::PoseStamped pose = group.getCurrentPose();
ROS_INFO("x = %f, y = %f, z = %f  " , pose.pose.position.x , pose.pose.position.y, pose.pose.position.z);
pose.header.frame_id = "base_link";
pose.header.stamp = ros::Time::now();
pose.pose.position.x = -0.25;
pose.pose.position.y = 0.0;
pose.pose.position.z = 0.2;
pose.pose.orientation.w = 1.0;
group.setStartStateToCurrentState();
group.setPoseTarget(pose, group.getEndEffectorLink());
success = group.plan(my_plan);
ROS_INFO("Visualizing plan 2 (joint space goal) %s",success?"":"FAILED");
group.execute(my_plan);
sleep(5.0);


ros::shutdown();
return 0;

}

terminal output

**ling@ling:~$ roslaunch scara_robot_moveit_config demo.launch

ling@ling:~$ rosrun scara_robot nav_test8

[ INFO] [1459831987.157223847]: Loading robot model 'scara'...

[ INFO] [1459831987.636260172]: Loading robot model 'scara'...

[ INFO] [1459831988.759656863]: Ready to take MoveGroup commands for group scara_arm.

[ INFO] [1459831988.763104882]: Reference frame: /base_link

[ INFO] [1459831988.763557455]: End-effector link: link4

[ INFO] [1459831989.049124368]: x = -0.400000, y = 0.000000, z = 0.278272

[ WARN] [1459831994.951622692]: Fail: ABORTED: No motion plan found. No execution attempted.

[ INFO] [1459831994.951739830]: Visualizing plan 2 (joint space goal) FAILED

terminate called after throwing an instance of 'class_loader::LibraryUnloadException'

what(): Attempt to unload library that class_loader is unaware of.

已放弃 (核心已转储)**

new to Moveit! need you guys' help.

BTW, is there any advance methods to control velocity or acceleration beside setting a factor for the MAXs.

edit retag flag offensive close merge delete

Comments

Gure Ling gravatar image Gure Ling  ( 2016-04-05 00:43:44 -0500 )edit