00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032 #include <ros/ros.h>
00033
00034 #include <planning_environment/models/collision_models.h>
00035 #include <kinematics_base/kinematics_base.h>
00036 #include <pluginlib/class_loader.h>
00037
00038 static const unsigned int TRIES_MAX=10;
00039
00040 inline double gen_rand(double min, double max)
00041 {
00042 int rand_num = rand()%100+1;
00043 double result = min + (double)((max-min)*rand_num)/101.0;
00044 return result;
00045 }
00046
00047 inline geometry_msgs::Quaternion generateRandomUnitQuaternion() {
00048
00049 geometry_msgs::Quaternion quat;
00050 quat.x = gen_rand(-1.0, 1.0);
00051 quat.w = gen_rand(-1.0, 1.0);
00052 quat.z = gen_rand(-1.0, 1.0);
00053 quat.w = gen_rand(-1.0, 1.0);
00054
00055 double mag = sqrt(pow(quat.x, 2.0)+pow(quat.y, 2.0)+pow(quat.z, 2.0)+pow(quat.w, 2.0));
00056
00057 quat.x /= mag;
00058 quat.y /= mag;
00059 quat.z /= mag;
00060 quat.w /= mag;
00061
00062 return quat;
00063 }
00064
00065 class IkPluginBakeoff {
00066
00067 public:
00068
00069 IkPluginBakeoff() : kl_base_("kinematics_base","kinematics::KinematicsBase"),
00070 priv_nh_("~")
00071 {
00072 priv_nh_.param<std::string>("group", group_, std::string());
00073 priv_nh_.param<std::string>("gold_solver_name", gold_solver_name_, std::string());
00074 priv_nh_.param<std::string>("comp_solver_name", comp_solver_name_, std::string());
00075 priv_nh_.param<double>("workspace_x_extents", workspace_x_extents_, 1.0);
00076 priv_nh_.param<double>("workspace_y_extents", workspace_y_extents_, 1.0);
00077 priv_nh_.param<double>("workspace_z_extents", workspace_z_extents_, 1.0);
00078
00079 cm_ = new planning_environment::CollisionModels("robot_description");
00080
00081 const planning_models::KinematicModel::JointModelGroup* joint_model_group = cm_->getKinematicModel()->getModelGroup(group_);
00082 if(joint_model_group == NULL) {
00083 ROS_WARN_STREAM("No joint group " << group_);
00084 return;
00085 }
00086
00087 for(unsigned int i = 0; i < joint_model_group->getJointModels().size(); i++) {
00088 const planning_models::KinematicModel::JointModel* jm = joint_model_group->getJointModels()[i];
00089 std::pair<double, double> bounds;
00090 jm->getVariableBounds(jm->getName(), bounds);
00091 joint_bounds_map_[jm->getName()] = bounds;
00092 }
00093
00094 try
00095 {
00096 ks_gold_ = kl_base_.createClassInstance(gold_solver_name_);
00097 }
00098 catch(pluginlib::PluginlibException& ex)
00099 {
00100 ROS_ERROR("The plugin failed to load. Error1: %s", ex.what());
00101 return;
00102 }
00103 if(!ks_gold_->initialize(group_)) {
00104 ROS_ERROR("Gold solver couldn't initialize group");
00105 return;
00106 }
00107
00108 try
00109 {
00110 ks_comp_ = kl_base_.createClassInstance(comp_solver_name_);
00111 }
00112 catch(pluginlib::PluginlibException& ex)
00113 {
00114 ROS_ERROR("The plugin failed to load. Error1: %s", ex.what());
00115 return;
00116 }
00117 if(!ks_comp_->initialize(group_)) {
00118 ROS_ERROR("Comp solver couldn't initialize group");
00119 return;
00120 }
00121 }
00122
00123 arm_navigation_msgs::RobotState generateRandomRobotStateWithinLimits() {
00124
00125 arm_navigation_msgs::RobotState rs;
00126 rs.joint_state.header.frame_id = cm_->getWorldFrameId();
00127 rs.joint_state.header.stamp = ros::Time::now();
00128
00129 for(std::map<std::string, std::pair<double, double> >::iterator it = joint_bounds_map_.begin();
00130 it != joint_bounds_map_.end();
00131 it++) {
00132 rs.joint_state.name.push_back(it->first);
00133 rs.joint_state.position.push_back(gen_rand(it->second.first, it->second.second));
00134 }
00135 return rs;
00136 }
00137
00138 std::vector<double> generateRandomRobotVectorWithinLimits() {
00139
00140 std::vector<std::string> ik_names = ks_gold_->getJointNames();
00141
00142 std::vector<double> rv;
00143
00144 for(unsigned int i = 0; i < ik_names.size(); i++) {
00145 std::pair<double, double>& bounds = joint_bounds_map_[ik_names[i]];
00146 rv.push_back(gen_rand(bounds.first, bounds.second));
00147 }
00148 return rv;
00149 }
00150
00151
00152 geometry_msgs::Pose generateRandomPoseInWorkspace() {
00153
00154 geometry_msgs::Pose rp;
00155 rp.position.x = gen_rand(-workspace_x_extents_, workspace_x_extents_);
00156 rp.position.y = gen_rand(-workspace_y_extents_, workspace_y_extents_);
00157 rp.position.z = gen_rand(-workspace_z_extents_, workspace_z_extents_);
00158
00159 rp.orientation = generateRandomUnitQuaternion();
00160
00161 return rp;
00162
00163 }
00164
00165 void runBakeoff(unsigned int num) {
00166
00167 unsigned int tot_comp_failures = 0;
00168 unsigned int tot_comp_tries = 0;
00169
00170 double gold_total_good_time;
00171 double gold_total_bad_time;
00172
00173 double comp_total_good_time;
00174 double comp_total_bad_time;
00175
00176 unsigned int successful_comp_count = 0;
00177
00178 unsigned int tot_gold_bad_count = 0;
00179
00180 std::vector<double> solution_gold;
00181 for(unsigned int i = 0; i < 1; i++) {
00182
00183 geometry_msgs::Pose good_pose;
00184 while(1) {
00185 int error;
00186 good_pose = generateRandomPoseInWorkspace();
00187 ros::WallTime wd = ros::WallTime::now();
00188 if(!ks_gold_->getPositionIK(good_pose,
00189 generateRandomRobotVectorWithinLimits(),
00190 solution_gold,
00191 error)) {
00192 gold_total_bad_time += (ros::WallTime::now()-wd).toSec();
00193 tot_gold_bad_count++;
00194 if(ks_gold_->getPositionIK(good_pose,
00195 generateRandomRobotVectorWithinLimits(),
00196 solution_gold,
00197 error)) {
00198 ROS_WARN_STREAM("Gold inconsistent");
00199 }
00200 } else {
00201 gold_total_good_time += (ros::WallTime::now()-wd).toSec();
00202 break;
00203 }
00204 }
00205
00206 unsigned int comp_count = 1;
00207 while(comp_count != TRIES_MAX) {
00208 std::vector<double> solution_comp;
00209 int error;
00210 ros::WallTime wd = ros::WallTime::now();
00211 if(ks_comp_->getPositionIK(good_pose,
00212 generateRandomRobotVectorWithinLimits(),
00213 solution_comp,
00214 error)) {
00215 comp_total_good_time += (ros::WallTime::now()-wd).toSec();
00216
00217 break;
00218 }
00219 comp_total_bad_time += (ros::WallTime::now()-wd).toSec();
00220 comp_count++;
00221 }
00222 if(comp_count == TRIES_MAX) {
00223 tot_comp_failures++;
00224 } else {
00225 successful_comp_count += comp_count;
00226 }
00227 tot_comp_tries += comp_count;
00228 }
00229
00230 ROS_INFO_STREAM("Average samples to produce good gold sample is " << (tot_gold_bad_count*1.0)/(num*1.0));
00231 ROS_INFO_STREAM("Average gold bad time " << gold_total_bad_time/(tot_gold_bad_count*1.0));
00232 ROS_INFO_STREAM("Average gold good time " << gold_total_good_time/(num*1.0));
00233 ROS_INFO_STREAM("Average comp bad time " << comp_total_bad_time/(tot_comp_tries*1.0));
00234 ROS_INFO_STREAM("Average comp good time " << comp_total_good_time/(num-tot_comp_failures*1.0));
00235
00236 ROS_INFO_STREAM("Average comp successful tries " << (successful_comp_count*1.0)/(num-tot_comp_failures*1.0));
00237
00238 ROS_INFO_STREAM("Total failures is " << tot_comp_failures << " out of " << num);
00239 }
00240
00241 private:
00242
00243 pluginlib::ClassLoader<kinematics::KinematicsBase> kl_base_;
00244
00245 kinematics::KinematicsBase* ks_gold_;
00246 kinematics::KinematicsBase* ks_comp_;
00247
00248 std::map<std::string, std::pair<double, double> > joint_bounds_map_;
00249 planning_environment::CollisionModels* cm_;
00250
00251 ros::NodeHandle priv_nh_;
00252 std::string group_;
00253 std::string gold_solver_name_;
00254 std::string comp_solver_name_;
00255
00256 double workspace_x_extents_;
00257 double workspace_y_extents_;
00258 double workspace_z_extents_;
00259
00260 };
00261
00262 int main(int argc, char** argv)
00263 {
00264 srand(time(0));
00265
00266 ros::init(argc, argv, "ik_plugin_bakeoff");
00267
00268 IkPluginBakeoff bake;
00269
00270 bake.runBakeoff(1000);
00271
00272 ros::shutdown();
00273 }
00274
