0
$\begingroup$

Rosanswers logo

I am working with 7DOF Barrett WAM and have created the IKFast plugin using URDF. I tested the plugin using demo.launch and everything works fine. I am also able to use the interactive marker.

The IKFast plugin was also tested before integration with MoveIt! using ikastdemo.cpp, the result was good.

I also set a value for each joint and asked KDL solver and IKFast to solve forward kinematics and then inverse kinematics. I believe the IKFast plugin is working fine since the results of KDL and IKFast are the same, as shown below, the only difference of the inverse kinematics solution is wam/shoulder_yaw_joint, which is reasonable for a redundant manipulator (set the 3rd joint free when I created the IKFast). The rest looks fine.

**IKFAST:**
[ INFO] [1428970205.913659853]: Joint(set) wam/wam_fixed_joint: 0.000000
[ INFO] [1428970205.913681942]: Joint(set) wam/base_yaw_joint: -1.281158
[ INFO] [1428970205.913708099]: Joint(set) wam/shoulder_pitch_joint: 1.658700
[ INFO] [1428970205.913732104]: Joint(set) wam/shoulder_yaw_joint: -2.247921
[ INFO] [1428970205.913758343]: Joint(set) wam/elbow_pitch_joint: 0.219933
[ INFO] [1428970205.913785120]: Joint(set) wam/wrist_yaw_joint: 1.160293
[ INFO] [1428970205.913810199]: Joint(set) wam/wrist_pitch_joint: -1.387798
[ INFO] [1428970205.913832614]: Joint(set) wam/palm_yaw_joint: -0.044969
[ INFO] [1428970205.913894368]: Translation: -0.735978
-0.160909
0.580133
[ INFO] [1428970205.913994440]: Rotation:   0.237783   0.432643  -0.869643
  0.411344   0.766221   0.493662
  0.879918  -0.475107 0.00422984
[ INFO] [1428970205.920762528]: Joint(IK) wam/wam_fixed_joint: -0.000000
[ INFO] [1428970205.920799765]: Joint(IK) wam/base_yaw_joint: -1.281158
[ INFO] [1428970205.920830629]: Joint(IK) wam/shoulder_pitch_joint: 1.658700
[ INFO] [1428970205.920856726]: Joint(IK) wam/shoulder_yaw_joint: -0.600000
[ INFO] [1428970205.920881040]: Joint(IK) wam/elbow_pitch_joint: 0.219933
[ INFO] [1428970205.920907838]: Joint(IK) wam/wrist_yaw_joint: 1.160293
[ INFO] [1428970205.920929596]: Joint(IK) wam/wrist_pitch_joint: -1.387798
[ INFO] [1428970205.920953596]: Joint(IK) wam/palm_yaw_joint: -0.044969



**KDL:**
[ INFO] [1428970281.101417384]: Joint(set) wam/wam_fixed_joint: 0.000000
[ INFO] [1428970281.101440046]: Joint(set) wam/base_yaw_joint: -1.281158
[ INFO] [1428970281.101466399]: Joint(set) wam/shoulder_pitch_joint: 1.658700
[ INFO] [1428970281.101492322]: Joint(set) wam/shoulder_yaw_joint: -2.247921
[ INFO] [1428970281.101514999]: Joint(set) wam/elbow_pitch_joint: 0.219933
[ INFO] [1428970281.101538865]: Joint(set) wam/wrist_yaw_joint: 1.160293
[ INFO] [1428970281.101561124]: Joint(set) wam/wrist_pitch_joint: -1.387798
[ INFO] [1428970281.101583497]: Joint(set) wam/palm_yaw_joint: -0.044969
[ INFO] [1428970281.101652882]: Translation: -0.735978
-0.160909
0.580133
[ INFO] [1428970281.101756504]: Rotation:   0.237783   0.432643  -0.869643
  0.411344   0.766221   0.493662
  0.879918  -0.475107 0.00422984
[ INFO] [1428970281.102042516]: Joint(IK) wam/wam_fixed_joint: 0.000000
[ INFO] [1428970281.102078929]: Joint(IK) wam/base_yaw_joint: -1.281158
[ INFO] [1428970281.102105266]: Joint(IK) wam/shoulder_pitch_joint: 1.658700
[ INFO] [1428970281.102128559]: Joint(IK) wam/shoulder_yaw_joint: -0.900000
[ INFO] [1428970281.102151188]: Joint(IK) wam/elbow_pitch_joint: 0.219933
[ INFO] [1428970281.102175909]: Joint(IK) wam/wrist_yaw_joint: 1.160293
[ INFO] [1428970281.102203092]: Joint(IK) wam/wrist_pitch_joint: -1.387798
[ INFO] [1428970281.102230072]: Joint(IK) wam/palm_yaw_joint: -0.044969

However, when I tried to use move_group, the planner always says RRTConnect: Unable to sample any valid states for goal tree

[ INFO] [1428976350.804524212]: Planning request received for MoveGroup action. Forwarding to planning pipeline. 
[ INFO] [1428976350.808251121]: No planner specified. Using default.
[ INFO] [1428976350.809310340]: RRTConnect: Starting planning with 1 states already in datastructure 
[ERROR] [1428976410.809924691]: RRTConnect: Unable to sample any valid states for goal tree
[ INFO] [1428976410.810012024]: RRTConnect: Created 1 states (1 start + 0 goal) 
[ INFO] [1428976410.810048295]: No solution found after 60.001474 seconds 
[ INFO] [1428976411.337647183]: Unable to solve the planning problem 
[ WARN] [1428976411.338331393]: Fail: ABORTED: No motion plan found. No execution attempted.

My move_group code is the following,

ros::init(argc, argv, "barrett_wam_constrained_move");
    ros::NodeHandle node_handle;
    ros::AsyncSpinner spinner(1);
    spinner.start();
    
    sleep(15.0);
    
    moveit::planning_interface::MoveGroup group("arm");
    moveit::planning_interface::PlanningSceneInterface planning_scene_interface;
    group.setPlanningTime(60);
    
    ros::Publisher display_publisher = node_handle.advertise<moveit_msgs::DisplayTrajectory>("/move_group/display_planned_path",1, true);
    moveit_msgs::DisplayTrajectory display_trajectory;
    
    geometry_msgs::Pose pose1;
    pose1.orientation.w = 1.0;
    pose1.position.x = 0.4;
    pose1.position.y = 0.5;
    pose1.position.z = 0.6;
    group.setPoseTarget(pose1);
    
    moveit::planning_interface::MoveGroup::Plan my_plan;
    bool success = group.plan(my_plan);        
    ROS_INFO("Visualizing plan 1 (pose1 goal) %s",success?"":"FAILED");    
    sleep(5.0);
    ros::shutdown();
    return 0;

I am sure the given pose is valid and reachable. The same code works for KDL perfectly. Any ideas and comments would be greatly appreciated!


Originally posted by Huitan on ROS Answers with karma: 63 on 2015-04-13

Post score: 0


Original comments

Comment by yizheng on 2015-06-27:
Dear Huitan, I'm currently trying to work on Barrett Wam too, but I didn't find the package on Moveit wiki, would mind telling me where do you get the wam package ?

Comment by Huitan on 2015-06-29:
Check out JHU repository following this https://github.com/jhu-lcsr (credit goes to JHU). MoveIt! configuration files generated by setupAssistant are under the tab "barrett_moveit". Robot urdf files are under "barrett_model". You need both to work with the arm. Look at PR2 when you have questions.

Comment by yizheng on 2015-06-29:
Hi Huitan, that's exactly what I did, so basically I just download these two packages, put them in my barrett_ws workspace, and then used catkin_make to compile them. However when I tried to launch the demo file, only the arm showed in the Rviz, but the hand was not shown.

Comment by yizheng on 2015-06-29:
Would you mind leaving me you e-mail address ? So I can ask you more about how you configure as well as work with this stuff. THX !

Comment by Huitan on 2015-06-29:
Have a look at your urdf to see what you included and what part you did not. If the hand is not included, then that is what you need to do. The readme file in the "barrett_model" repository may be helpful.

Comment by Huitan on 2015-06-29:
Also please open a new thread when you ask a new question. Besides ROS answers, MoveIt! mailing list is where you can ask questions.

$\endgroup$

1 Answer 1

0
$\begingroup$

Rosanswers logo

I have managed to solve the problem. It turned out that the starting configuration of the arm is singular, so that is where IKFast failed to get an IK solution. I intentionally brought the starting configuration to a non-singular one, then it started working.

For future readers with the similar problem, I suggest you test the IKFast thoroughly using ikfastdemo.cpp, then integrate with ROS and run a bunch of forward kinematics and inverse kinematics examples both using KDL and IKFast. If the result is good but IKFast is still not working, you may double check your model like Simon suggested and pay attention to singularities. Thanks Simon for the information!

You may also find useful discussions here. Thanks Billy and Martin!


Originally posted by Huitan with karma: 63 on 2015-04-15

This answer was ACCEPTED on the original site

Post score: 1

$\endgroup$

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.