$search
00001 #include <ros/ros.h> 00002 #include <kinematics_msgs/GetKinematicSolverInfo.h> 00003 #include <kinematics_msgs/GetConstraintAwarePositionIK.h> 00004 #include <sensor_msgs/JointState.h> 00005 #include <ompl/util/RandomNumbers.h> 00006 #include <tf/transform_broadcaster.h> 00007 #include <tf/transform_datatypes.h> 00008 #include <arm_navigation_msgs/DisplayTrajectory.h> 00009 00010 int main(int argc, char **argv) 00011 { 00012 ros::init(argc, argv, "get_constraint_aware_ik_performance"); 00013 ros::NodeHandle rh; 00014 ros::NodeHandle pn("~"); 00015 00016 ROS_DEBUG("Starting 'get_constraint_aware_ik_test'..."); 00017 00018 std::string ik_service; 00019 pn.param<std::string> ("ik_service", ik_service, "katana_constraint_aware_kinematics/get_constraint_aware_ik"); 00020 00021 ros::service::waitForService(ik_service); 00022 ros::service::waitForService("katana_constraint_aware_kinematics/get_ik_solver_info"); 00023 00024 ros::Publisher pub = 00025 rh.advertise<arm_navigation_msgs::DisplayTrajectory> ("planned_robot_pose", 1000); 00026 ros::ServiceClient ik_client_ = 00027 rh.serviceClient<kinematics_msgs::GetConstraintAwarePositionIK> (ik_service); 00028 ros::ServiceClient query_client = 00029 rh.serviceClient<kinematics_msgs::GetKinematicSolverInfo> ("katana_constraint_aware_kinematics/get_ik_solver_info"); 00030 ROS_ERROR("Connect to the necessary services..."); 00031 00032 // define the query service messages 00033 ompl::RNG rng = ompl::RNG(); 00034 kinematics_msgs::GetKinematicSolverInfo::Request request; 00035 kinematics_msgs::GetKinematicSolverInfo::Response response; 00036 00037 sensor_msgs::JointState initialState; 00038 arm_navigation_msgs::DisplayTrajectory planned_robot_pose; 00039 00040 if (query_client.call(request, response)) 00041 { 00042 for (unsigned int i = 0; i < response.kinematic_solver_info.joint_names.size(); i++) 00043 { 00044 // ROS_INFO("Joint: %d %s", i, 00045 // response.kinematic_solver_info.joint_names[i].c_str()); 00046 } 00047 00048 // publish initial joint state 00049 initialState.header.stamp = ros::Time::now(); 00050 initialState.name = response.kinematic_solver_info.joint_names; 00051 initialState.position.resize(response.kinematic_solver_info.joint_names.size()); 00052 00053 for (unsigned int i = 0; i < response.kinematic_solver_info.joint_names.size(); i++) 00054 { 00055 // double rand = rng.uniformReal(response.kinematic_solver_info.limits[i].min_position, 00056 // response.kinematic_solver_info.limits[i].max_position); 00057 // initialState.position[i] = rand; 00058 // ROS_INFO("Initial Joint Position: %s %f",response.kinematic_solver_info.joint_names[i].c_str(), rand); 00059 00060 } 00061 00062 // publish initial position 00063 00064 00065 00066 planned_robot_pose.trajectory.joint_trajectory.header = initialState.header; 00067 planned_robot_pose.trajectory.joint_trajectory.joint_names = initialState.name; 00068 planned_robot_pose.trajectory.joint_trajectory.points.resize(1); 00069 planned_robot_pose.trajectory.joint_trajectory.points[0].positions.resize(initialState.position.size()); 00070 00071 for (size_t i = 0; i < initialState.position.size(); i++) 00072 { 00073 planned_robot_pose.trajectory.joint_trajectory.points[0].positions[i] = initialState.position[i]; 00074 } 00075 00076 pub.publish(planned_robot_pose); 00077 ROS_DEBUG("Published initial joint state..."); 00078 ros::Duration(1.0).sleep(); 00079 00080 } 00081 else 00082 { 00083 ROS_ERROR("Could not call query service"); 00084 ros::shutdown(); 00085 exit(1); 00086 } 00087 00088 // define the service messages 00089 kinematics_msgs::GetConstraintAwarePositionIK::Request gcapik_req; 00090 kinematics_msgs::GetConstraintAwarePositionIK::Response gcapik_res; 00091 00092 sensor_msgs::JointState jointState = initialState; 00093 00094 tf::TransformBroadcaster br; 00095 tf::Transform transform; 00096 00097 int failed_ik_count = 0; 00098 int count = -1; 00099 00100 do 00101 { 00102 gcapik_req.timeout = ros::Duration(5.0); 00103 gcapik_req.ik_request.ik_link_name = "katana_gripper_tool_frame"; 00104 00105 // generate a random pose in katana's operation space: 00106 // [-0.12, 0.68] in the z direction and [-0.48, 0.48] in both x and y 00107 00108 gcapik_req.ik_request.pose_stamped.header.frame_id = "katana_base_link"; 00109 00110 00111 // position should have a solution 00112 /* 00113 gcapik_req.ik_request.pose_stamped.pose.position.x = 0.16192; 00114 gcapik_req.ik_request.pose_stamped.pose.position.y = 0.161038; 00115 gcapik_req.ik_request.pose_stamped.pose.position.z = 0.517586; 00116 00117 gcapik_req.ik_request.pose_stamped.pose.orientation.x = 0.708434; 00118 gcapik_req.ik_request.pose_stamped.pose.orientation.y = 0.024962; 00119 gcapik_req.ik_request.pose_stamped.pose.orientation.z = 0.704894; 00120 gcapik_req.ik_request.pose_stamped.pose.orientation.w = 0.024962; 00121 00122 00123 */ 00124 00125 // testarmik5d.py: alles von -0.5 bis +0.5 00126 gcapik_req.ik_request.pose_stamped.pose.position.x = rng.uniformReal(-0.48, 0.48); 00127 gcapik_req.ik_request.pose_stamped.pose.position.y = rng.uniformReal(-0.48, 0.48); 00128 gcapik_req.ik_request.pose_stamped.pose.position.z = rng.uniformReal(-0.12, 0.68); 00129 00130 double value[4]; 00131 00132 rng.quaternion(value); 00133 00134 gcapik_req.ik_request.pose_stamped.pose.orientation.x = value[0]; 00135 gcapik_req.ik_request.pose_stamped.pose.orientation.y = value[1]; 00136 gcapik_req.ik_request.pose_stamped.pose.orientation.z = value[2]; 00137 gcapik_req.ik_request.pose_stamped.pose.orientation.w = value[3]; 00138 00139 // publishing the goal pose as TF for visualising it in rviz 00140 00141 transform.setOrigin(tf::Vector3(gcapik_req.ik_request.pose_stamped.pose.position.x, 00142 gcapik_req.ik_request.pose_stamped.pose.position.y, 00143 gcapik_req.ik_request.pose_stamped.pose.position.z)); 00144 00145 transform.setRotation(tf::Quaternion(gcapik_req.ik_request.pose_stamped.pose.orientation.x, 00146 gcapik_req.ik_request.pose_stamped.pose.orientation.y, 00147 gcapik_req.ik_request.pose_stamped.pose.orientation.z, 00148 gcapik_req.ik_request.pose_stamped.pose.orientation.w)); 00149 00150 br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "katana_base_link", "katana_goal_pose")); 00151 00152 ROS_DEBUG("cartesian goal pose orientation: %f, %f, %f", gcapik_req.ik_request.pose_stamped.pose.position.x, 00153 gcapik_req.ik_request.pose_stamped.pose.position.y, 00154 gcapik_req.ik_request.pose_stamped.pose.position.z); 00155 00156 ROS_DEBUG("cartesian goal pose orientation: %f, %f, %f, %f", gcapik_req.ik_request.pose_stamped.pose.orientation.x, 00157 gcapik_req.ik_request.pose_stamped.pose.orientation.y, 00158 gcapik_req.ik_request.pose_stamped.pose.orientation.z, 00159 gcapik_req.ik_request.pose_stamped.pose.orientation.w); 00160 00161 gcapik_req.ik_request.ik_seed_state.joint_state.position.resize(response.kinematic_solver_info.joint_names.size()); 00162 00163 gcapik_req.ik_request.ik_seed_state.joint_state.name = response.kinematic_solver_info.joint_names; 00164 00165 // let the IK plan from the initial state... 00166 // gcapik_req.ik_request.ik_seed_state.joint_state = initialState; 00167 00168 // or from the middle position of all joints... 00169 00170 for (unsigned int i = 0; i < response.kinematic_solver_info.joint_names.size(); i++) 00171 { 00172 gcapik_req.ik_request.ik_seed_state.joint_state.position[i] 00173 = (response.kinematic_solver_info.limits[i].min_position 00174 + response.kinematic_solver_info.limits[i].max_position) / 2.0; 00175 } 00176 00177 00178 // ros::Rate loop_rate1(100); 00179 // ros::Time end_time = ros::Time::now() + ros::Duration(0.2); 00180 /* 00181 while (ros::ok() && ros::Time::now() < end_time) 00182 { 00183 jointState.header.stamp = ros::Time::now(); 00184 planned_robot_pose.trajectory.joint_trajectory.header = jointState.header; 00185 pub.publish(planned_robot_pose); 00186 br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "katana_base_link", "katana_goal_pose")); 00187 00188 loop_rate1.sleep(); 00189 } 00190 */ 00191 // ROS_WARN("Calling the constraint_aware IK client..."); 00192 00193 bool success = ik_client_.call(gcapik_req, gcapik_res); 00194 00195 //ROS_WARN("Calling the client succeded..."); 00196 00197 00198 if (!success) 00199 { 00200 ROS_ERROR("Could not call service"); 00201 break; 00202 } 00203 count++; 00204 if(gcapik_res.error_code.val != gcapik_res.error_code.SUCCESS){ 00205 failed_ik_count++; 00206 00207 } 00208 else{ 00209 } 00210 00211 ROS_DEBUG("IK Error Code: %d", gcapik_res.error_code.val); 00212 // } while (gcapik_res.error_code.val != gcapik_res.error_code.SUCCESS); 00213 } while (count < 1000); 00214 00215 // ROS_INFO("The IK succeeded after %d unsuccessful attempts.", failed_ik_count); 00216 // ROS_INFO("Success rate: %f", 1.0/(double)(failed_ik_count + 1) ); 00217 00218 ROS_INFO("Failed Rate: %f", (double) failed_ik_count/(double) count); 00219 ROS_INFO("Success rate: %f", (double) 1 - (double) failed_ik_count/(double) count); 00220 00221 00222 00223 ROS_INFO("Joint Set Up..."); 00224 00225 for (unsigned int i = 0; i < gcapik_res.solution.joint_state.name.size(); i++) 00226 { 00227 ROS_INFO("Joint: %s - %f",gcapik_res.solution.joint_state.name[i].c_str(),gcapik_res.solution.joint_state.position[i]); 00228 00229 } 00230 00231 ROS_INFO("...leads to:"); 00232 00233 00234 ROS_INFO("pose position: %f, %f, %f", gcapik_req.ik_request.pose_stamped.pose.position.x, 00235 gcapik_req.ik_request.pose_stamped.pose.position.y, 00236 gcapik_req.ik_request.pose_stamped.pose.position.z); 00237 00238 ROS_INFO("pose orientation: %f, %f, %f, %f", gcapik_req.ik_request.pose_stamped.pose.orientation.x, 00239 gcapik_req.ik_request.pose_stamped.pose.orientation.y, 00240 gcapik_req.ik_request.pose_stamped.pose.orientation.z, 00241 gcapik_req.ik_request.pose_stamped.pose.orientation.w); 00242 00243 btScalar roll, pitch, yaw; 00244 btQuaternion q = btQuaternion(gcapik_req.ik_request.pose_stamped.pose.orientation.x, 00245 gcapik_req.ik_request.pose_stamped.pose.orientation.y, 00246 gcapik_req.ik_request.pose_stamped.pose.orientation.z, 00247 gcapik_req.ik_request.pose_stamped.pose.orientation.w); 00248 00249 btMatrix3x3(q).getRPY(roll, pitch, yaw); 00250 00251 ROS_INFO("pose orientation RPY: %f, %f, %f", roll, pitch, yaw); 00252 00253 ROS_INFO("..."); 00254 // publish goal state 00255 00256 jointState = gcapik_res.solution.joint_state; 00257 00258 planned_robot_pose.trajectory.joint_trajectory.header = gcapik_res.solution.joint_state.header; 00259 planned_robot_pose.trajectory.joint_trajectory.joint_names = gcapik_res.solution.joint_state.name; 00260 00261 planned_robot_pose.trajectory.joint_trajectory.points[0].positions.clear(); 00262 planned_robot_pose.trajectory.joint_trajectory.points[0].positions.resize(gcapik_res.solution.joint_state.position.size()); 00263 00264 00265 for (size_t i = 0; i <gcapik_res.solution.joint_state.position.size(); i++) 00266 { 00267 planned_robot_pose.trajectory.joint_trajectory.points[0].positions[i] = gcapik_res.solution.joint_state.position[i]; 00268 } 00269 00270 ros::Rate loop_rate(100); 00271 00272 while (ros::ok()) 00273 { 00274 ROS_DEBUG("Publish goal state"); 00275 00276 jointState.header.stamp = ros::Time::now(); 00277 planned_robot_pose.trajectory.joint_trajectory.header = jointState.header; 00278 pub.publish(planned_robot_pose); 00279 br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "katana_base_link", "katana_goal_pose")); 00280 00281 loop_rate.sleep(); 00282 } 00283 } 00284