$search
00001 #include <ros/ros.h> 00002 #include <actionlib/client/simple_action_client.h> 00003 #include <ompl/util/RandomNumbers.h> 00004 #include <arm_navigation_msgs/MoveArmAction.h> 00005 #include <arm_navigation_msgs/utils.h> 00006 #include <geometry_msgs/Pose.h> 00007 #include <tf/transform_broadcaster.h> 00008 #include <tf/transform_datatypes.h> 00009 int counter; 00010 int success_counter; 00011 00012 void move_to_pose(geometry_msgs::Pose & desired_pose, 00013 actionlib::SimpleActionClient<arm_navigation_msgs::MoveArmAction> & move_arm) 00014 { 00015 00016 counter++; 00017 00018 arm_navigation_msgs::SimplePoseConstraint simple_pose_constraint; 00019 00020 simple_pose_constraint.header.frame_id = "katana_base_link"; 00021 00022 simple_pose_constraint.link_name = "katana_gripper_tool_frame"; 00023 00024 simple_pose_constraint.pose = desired_pose; 00025 00026 simple_pose_constraint.absolute_position_tolerance.x = 0.03; 00027 simple_pose_constraint.absolute_position_tolerance.y = 0.03; 00028 simple_pose_constraint.absolute_position_tolerance.z = 0.03; 00029 simple_pose_constraint.absolute_roll_tolerance = 3.14; 00030 simple_pose_constraint.absolute_pitch_tolerance = 3.14; 00031 simple_pose_constraint.absolute_yaw_tolerance = 3.14; 00032 00033 /* 00034 simple_pose_constraint.absolute_roll_tolerance = 3.14; 00035 simple_pose_constraint.absolute_pitch_tolerance = 3.14; 00036 simple_pose_constraint.absolute_yaw_tolerance = 3.14; 00037 */ 00038 simple_pose_constraint.orientation_constraint_type = 0; //HEADER_FRAME 00039 // simple_pose_constraint.orientation_constraint_type = 1; //LINK_FRAME 00040 00041 00042 arm_navigation_msgs::MoveArmGoal goal; 00043 00044 goal.motion_plan_request.group_name = "arm"; 00045 goal.motion_plan_request.num_planning_attempts = 1; 00046 goal.motion_plan_request.planner_id = std::string(""); 00047 goal.planner_service_name = std::string("ompl_planning/plan_kinematic_path"); 00048 goal.motion_plan_request.allowed_planning_time = ros::Duration(15.0); 00049 00050 arm_navigation_msgs::addGoalConstraintToMoveArmGoal(simple_pose_constraint, goal); 00051 00052 bool finished_within_time = false; 00053 00054 move_arm.sendGoal(goal); 00055 00056 finished_within_time = move_arm.waitForResult(ros::Duration(15.5)); 00057 00058 if (!finished_within_time) 00059 { 00060 move_arm.cancelGoal(); 00061 ROS_INFO("Timed out achieving goal!"); 00062 } 00063 else 00064 { 00065 actionlib::SimpleClientGoalState state = move_arm.getState(); 00066 bool success = (state == actionlib::SimpleClientGoalState::SUCCEEDED); 00067 if (success) 00068 { 00069 00070 success_counter++; 00071 btScalar roll, pitch, yaw; 00072 btQuaternion q = btQuaternion(simple_pose_constraint.pose.orientation.x, 00073 simple_pose_constraint.pose.orientation.y, 00074 simple_pose_constraint.pose.orientation.z, 00075 simple_pose_constraint.pose.orientation.w); 00076 00077 btMatrix3x3(q).getRPY(roll, pitch, yaw); 00078 00079 ROS_INFO("Action finished: %s",state.toString().c_str()); 00080 ROS_INFO("xyz: %f / %f / %f ", 00081 simple_pose_constraint.pose.position.x, 00082 simple_pose_constraint.pose.position.y, 00083 simple_pose_constraint.pose.position.z); 00084 ROS_INFO("quad: %f / %f / %f / %f", 00085 simple_pose_constraint.pose.orientation.x, 00086 simple_pose_constraint.pose.orientation.y, 00087 simple_pose_constraint.pose.orientation.z, 00088 simple_pose_constraint.pose.orientation.w); 00089 ROS_INFO("rpy: %f / %f / %f",roll, pitch, yaw); 00090 00091 ROS_INFO("Trial #%d - succeeded: %f", counter, (double) success_counter/counter); 00092 00093 tf::TransformBroadcaster br; 00094 tf::Transform transform; 00095 transform.setOrigin(tf::Vector3(simple_pose_constraint.pose.position.x, simple_pose_constraint.pose.position.y, 00096 simple_pose_constraint.pose.position.z)); 00097 00098 transform.setRotation(tf::Quaternion(simple_pose_constraint.pose.orientation.x, 00099 simple_pose_constraint.pose.orientation.y, 00100 simple_pose_constraint.pose.orientation.z, 00101 simple_pose_constraint.pose.orientation.w)); 00102 00103 ros::Rate loop_rate1(100); 00104 ros::Time end_time = ros::Time::now() + ros::Duration(1.0); 00105 00106 while (ros::ok() && ros::Time::now() < end_time) 00107 { 00108 br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "katana_base_link", "katana_goal_pose")); 00109 loop_rate1.sleep(); 00110 } 00111 } 00112 else 00113 { 00114 /* 00115 tf::TransformBroadcaster br2; 00116 tf::Transform transform2; 00117 00118 transform2.setOrigin(tf::Vector3(simple_pose_constraint.pose.position.x, simple_pose_constraint.pose.position.y, 00119 simple_pose_constraint.pose.position.z)); 00120 00121 transform2.setRotation(tf::Quaternion(simple_pose_constraint.pose.orientation.x, 00122 simple_pose_constraint.pose.orientation.y, 00123 simple_pose_constraint.pose.orientation.z, 00124 simple_pose_constraint.pose.orientation.w)); 00125 00126 ros::Rate loop_rate2(100); 00127 ros::Time end_time = ros::Time::now() + ros::Duration(0.2); 00128 00129 while (ros::ok() && ros::Time::now() < end_time) 00130 { 00131 00132 br2.sendTransform(tf::StampedTransform(transform2, ros::Time::now(), "katana_base_link", "katana_missed_goal_pose")); 00133 00134 loop_rate2.sleep(); 00135 } 00136 */ 00137 ROS_DEBUG_THROTTLE(1.0, "Action failed: %s (this is expected to happen a lot)",state.toString().c_str()); 00138 } 00139 } 00140 } 00141 00142 int main(int argc, char **argv) 00143 { 00144 ros::init(argc, argv, "move_arm_pose_goal_test"); 00145 ros::NodeHandle nh; 00146 00147 actionlib::SimpleActionClient<arm_navigation_msgs::MoveArmAction> move_arm("move_arm", true); 00148 move_arm.waitForServer(); 00149 ROS_INFO("Connected to server"); 00150 00151 counter = 0; 00152 00153 while (nh.ok()) 00154 { 00155 00159 00160 geometry_msgs::Pose pose1; 00161 00162 ompl::RNG rng = ompl::RNG(); 00163 00164 00165 pose1.position.x = rng.uniformReal(0.1, 0.15); 00166 pose1.position.y = rng.uniformReal(0.1, 0.15); 00167 pose1.position.z = rng.uniformReal(0.05, 0.45); 00168 00169 double value2[4]; 00170 00171 rng.quaternion(value2); 00172 00173 pose1.orientation.x = value2[0]; 00174 pose1.orientation.y = value2[1]; 00175 pose1.orientation.z = value2[2]; 00176 pose1.orientation.w = value2[3]; 00177 ROS_INFO("Sending move_arm goal for pose 1 (new)"); 00178 00179 move_to_pose(pose1, move_arm); 00180 00181 geometry_msgs::Pose pose2; 00182 pose2.position.x = rng.uniformReal(-0.15, 0.15); 00183 pose2.position.y = rng.uniformReal(-0.15, 0.15); 00184 pose2.position.z = rng.uniformReal(0.05, 0.45); 00185 00186 double value[4]; 00187 00188 rng.quaternion(value); 00189 00190 pose2.orientation.x = value2[0]; 00191 pose2.orientation.y = value2[1]; 00192 pose2.orientation.z = value2[2]; 00193 pose2.orientation.w = value2[3]; 00194 // ROS_INFO("Sending move_arm goal for pose 2 (new)"); 00195 00196 //move_to_pose(pose2, move_arm); 00197 00198 00202 00203 00204 // [ INFO] [1307438122.669621150]: pose position: -0.183997, 0.183440, 0.415803 00205 // [ INFO] [1307438122.669667690]: pose orientation: 0.209215, -0.875412, -0.396642, 0.1804 00206 /* 0.0826566386589 00207 -0.105906316163 00208 0.519365423008 00209 [ 0.1485272 0.6907988 0.21326708 0.67472501] 00210 */ 00211 /* 00212 geometry_msgs::Pose pose1; 00213 00214 pose1.position.x = 0.0826566386589; 00215 pose1.position.y = -0.105906316163; 00216 pose1.position.z = 0.519365423008; 00217 00218 pose1.orientation.x = 0.1485272; 00219 pose1.orientation.y = 0.6907988; 00220 pose1.orientation.z = 0.21326708; 00221 pose1.orientation.w = 0.67472501; 00222 ROS_INFO("Sending move_arm goal for pose 1 (OpenRave approved)"); 00223 00224 move_to_pose(pose1, move_arm); 00225 */ 00226 // determined with get_constraint_aware_ik_test to have an openrave ik solution 00227 // [INFO] [1307438247.951109908]: pose position: -0.018995, -0.270915, 0.369523 00228 // [INFO] [1307438247.951154217]: pose orientation: -0.664450, 0.151834, 0.036378, -0.730842 00229 // [INFO] [1307438247.951195536]: ... 00230 00231 /* 00232 0.107560697725 00233 0.0477060075151 00234 0.537819082027 00235 [ 0.01373857 0.59140089 0.6834396 0.42774589] 00236 00237 */ 00238 /* 00239 geometry_msgs::Pose pose2; 00240 pose2.position.x = 0.107560697725; 00241 pose2.position.y = 0.0477060075151; 00242 pose2.position.z = 0.537819082027; 00243 00244 pose2.orientation.x = 0.01373857; 00245 pose2.orientation.y = 0.59140089; 00246 pose2.orientation.z = 0.6834396; 00247 pose2.orientation.w = 0.42774589; 00248 00249 ROS_INFO("Sending move_arm goal for pose 2 (OpenRave approved)"); 00250 00251 move_to_pose(pose2, move_arm); 00252 00253 */ 00254 00258 00259 /* 00260 // all joints = 1.5 rad, katana_base_link --> katana_gripper_tool_frame (obtained via get_fk) 00261 // Position: 0.161907,0.161043,0.517593 00262 // Orientation: 0.708431 0.024943 0.704897 0.024953 00263 00264 geometry_msgs::Pose pose2; 00265 pose2.position.x = 0.161907; 00266 pose2.position.y = 0.161043; 00267 pose2.position.z = 0.517593; 00268 00269 pose2.orientation.x = 0.708431; 00270 pose2.orientation.y = 0.024943; 00271 pose2.orientation.z = 0.704897; 00272 pose2.orientation.w = 0.024953; 00273 00274 ROS_INFO("Sending move_arm goal for pose 1 (all joints = 1.5)"); 00275 00276 move_to_pose(pose1, move_arm); 00277 00278 // all joints = 1.8 rad, katana_base_link --> katana_gripper_tool_frame (obtained via get_fk) 00279 // Position: 0.139384,0.0476408,0.583725 00280 // Orientation: 0.720839 -0.078734 0.684101 -0.078725 00281 00282 geometry_msgs::Pose pose2; 00283 pose2.position.x = 0.139384; 00284 pose2.position.y = 0.0476408; 00285 pose2.position.z = 0.583725; 00286 00287 pose2.orientation.x = 0.720839; 00288 pose2.orientation.y = -0.078734; 00289 pose2.orientation.z = 0.684101; 00290 pose2.orientation.w = -0.078725; 00291 00292 ROS_INFO("Sending move_arm goal for pose 2 (all joints = 1.8)"); 00293 00294 move_to_pose(pose2, move_arm); 00295 */ 00296 00300 00301 /* 00302 00303 // [ INFO] [1308126081.863256828]: Position: -0.193108,-0.421214,0.0501686 00304 // [ INFO] [1308126081.863297275]: Orientation: -0.544680 0.823614 -0.015894 -0.157259 00305 00306 geometry_msgs::Pose pose1; 00307 00308 pose1.position.x = -0.19; 00309 pose1.position.y = -0.42; 00310 pose1.position.z = 0.050; 00311 00312 pose1.orientation.x = -0.54; 00313 pose1.orientation.y = 0.82; 00314 pose1.orientation.z = -0.01; 00315 pose1.orientation.w = -0.15; 00316 00317 ROS_INFO("Sending move_arm goal for pose 1 (right)"); 00318 00319 move_to_pose(pose1, move_arm); 00320 00321 // [ INFO] [1308126369.712751011]: Position: 0.162202,-0.419585,0.0597439 00322 // [ INFO] [1308126369.712814982]: Orientation: 0.837765 -0.535153 -0.035609 0.1024 00323 00324 geometry_msgs::Pose pose2; 00325 00326 pose2.position.x = 0.16; 00327 pose2.position.y = -0.41; 00328 pose2.position.z = 0.05; 00329 00330 pose2.orientation.x = 0.83; 00331 pose2.orientation.y = -0.53; 00332 pose2.orientation.z = -0.03; 00333 pose2.orientation.w = 0.10; 00334 00335 ROS_INFO("Sending move_arm goal for pose 2 (left)"); 00336 00337 move_to_pose(pose2, move_arm); 00338 */ 00339 } 00340 }