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("/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!
↧