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
00033
00034 #include <arm_kinematics_constraint_aware/kdl_arm_kinematics_plugin.h>
00035 #include <pluginlib/class_list_macros.h>
00036
00037 using namespace KDL;
00038 using namespace tf;
00039 using namespace std;
00040 using namespace ros;
00041
00042
00043 PLUGINLIB_DECLARE_CLASS(arm_kinematics_constraint_aware,KDLArmKinematicsPlugin, arm_kinematics_constraint_aware::KDLArmKinematicsPlugin, kinematics::KinematicsBase)
00044
00045 namespace arm_kinematics_constraint_aware {
00046
00047 KDLArmKinematicsPlugin::KDLArmKinematicsPlugin():active_(false)
00048 {
00049 srand ( time(NULL) );
00050 }
00051
00052 bool KDLArmKinematicsPlugin::isActive()
00053 {
00054 if(active_)
00055 return true;
00056 return false;
00057 }
00058
00059 double KDLArmKinematicsPlugin::genRandomNumber(const double &min, const double &max)
00060 {
00061 int rand_num = rand()%100+1;
00062 double result = min + (double)((max-min)*rand_num)/101.0;
00063 return result;
00064 }
00065
00066 KDL::JntArray KDLArmKinematicsPlugin::getRandomConfiguration()
00067 {
00068 KDL::JntArray jnt_array;
00069 jnt_array.resize(dimension_);
00070 for(unsigned int i=0; i < dimension_; i++)
00071 jnt_array(i) = genRandomNumber(joint_min_(i),joint_max_(i));
00072 return jnt_array;
00073 }
00074
00075 bool KDLArmKinematicsPlugin::initialize(std::string name)
00076 {
00077
00078 std::string urdf_xml, full_urdf_xml;
00079 ros::NodeHandle node_handle;
00080 ros::NodeHandle private_handle("~"+name);
00081 node_handle.param("urdf_xml",urdf_xml,std::string("robot_description"));
00082 node_handle.searchParam(urdf_xml,full_urdf_xml);
00083 ROS_DEBUG("Reading xml file from parameter server");
00084 std::string result;
00085 if (!node_handle.getParam(full_urdf_xml, result)) {
00086 ROS_FATAL("Could not load the xml from parameter server: %s", urdf_xml.c_str());
00087 return false;
00088 }
00089
00090
00091 if (!private_handle.getParam("root_name", root_name_)) {
00092 ROS_FATAL("GenericIK: No root name found on parameter server");
00093 return false;
00094 }
00095 if (!private_handle.getParam("tip_name", tip_name_)) {
00096 ROS_FATAL("GenericIK: No tip name found on parameter server");
00097 return false;
00098 }
00099
00100 if (!loadModel(result)) {
00101 ROS_FATAL("Could not load models!");
00102 return false;
00103 }
00104
00105
00106 int max_iterations;
00107 double epsilon;
00108
00109 private_handle.param("max_solver_iterations", max_iterations, 1000);
00110 private_handle.param("max_search_iterations", max_search_iterations_, 1000);
00111 private_handle.param("epsilon", epsilon, 1e-2);
00112
00113
00114 fk_solver_.reset(new KDL::ChainFkSolverPos_recursive(kdl_chain_));
00115 ik_solver_vel_.reset(new KDL::ChainIkSolverVel_pinv(kdl_chain_));
00116 ik_solver_pos_.reset(new KDL::ChainIkSolverPos_NR_JL(kdl_chain_, joint_min_, joint_max_,*fk_solver_, *ik_solver_vel_, max_iterations, epsilon));
00117 active_ = true;
00118 return true;
00119 }
00120
00121 bool KDLArmKinematicsPlugin::loadModel(const std::string xml)
00122 {
00123 urdf::Model robot_model;
00124 KDL::Tree tree;
00125
00126 if (!robot_model.initString(xml)) {
00127 ROS_FATAL("Could not initialize robot model");
00128 return -1;
00129 }
00130 if (!kdl_parser::treeFromString(xml, tree)) {
00131 ROS_ERROR("Could not initialize tree object");
00132 return false;
00133 }
00134 if (!tree.getChain(root_name_, tip_name_, kdl_chain_)) {
00135 ROS_ERROR("Could not initialize chain object");
00136 return false;
00137 }
00138 if (!readJoints(robot_model)) {
00139 ROS_FATAL("Could not read information about the joints");
00140 return false;
00141 }
00142 return true;
00143 }
00144
00145 bool KDLArmKinematicsPlugin::readJoints(urdf::Model &robot_model)
00146 {
00147 dimension_ = 0;
00148
00149 boost::shared_ptr<const urdf::Link> link = robot_model.getLink(tip_name_);
00150 boost::shared_ptr<const urdf::Joint> joint;
00151 while (link && link->name != root_name_) {
00152 joint = robot_model.getJoint(link->parent_joint->name);
00153 if (!joint) {
00154 ROS_ERROR("Could not find joint: %s",link->parent_joint->name.c_str());
00155 return false;
00156 }
00157 if (joint->type != urdf::Joint::UNKNOWN && joint->type != urdf::Joint::FIXED) {
00158 ROS_INFO( "adding joint: [%s]", joint->name.c_str() );
00159 dimension_++;
00160 }
00161 link = robot_model.getLink(link->getParent()->name);
00162 }
00163 joint_min_.resize(dimension_);
00164 joint_max_.resize(dimension_);
00165 chain_info_.joint_names.resize(dimension_);
00166 chain_info_.limits.resize(dimension_);
00167 link = robot_model.getLink(tip_name_);
00168 if(link)
00169 chain_info_.link_names.push_back(tip_name_);
00170
00171 unsigned int i = 0;
00172 while (link && i < dimension_) {
00173 joint = robot_model.getJoint(link->parent_joint->name);
00174 if (joint->type != urdf::Joint::UNKNOWN && joint->type != urdf::Joint::FIXED) {
00175 ROS_INFO( "getting bounds for joint: [%s]", joint->name.c_str() );
00176
00177 float lower, upper;
00178 int hasLimits;
00179 if ( joint->type != urdf::Joint::CONTINUOUS ) {
00180 lower = joint->limits->lower;
00181 upper = joint->limits->upper;
00182 hasLimits = 1;
00183 } else {
00184 lower = -M_PI;
00185 upper = M_PI;
00186 hasLimits = 0;
00187 }
00188 int index = dimension_ - i -1;
00189
00190 joint_min_.data[index] = lower;
00191 joint_max_.data[index] = upper;
00192 chain_info_.joint_names[index] = joint->name;
00193 chain_info_.limits[index].joint_name = joint->name;
00194 chain_info_.limits[index].has_position_limits = hasLimits;
00195 chain_info_.limits[index].min_position = lower;
00196 chain_info_.limits[index].max_position = upper;
00197 i++;
00198 }
00199 link = robot_model.getLink(link->getParent()->name);
00200 }
00201 return true;
00202 }
00203
00204 int KDLArmKinematicsPlugin::getJointIndex(const std::string &name)
00205 {
00206 for (unsigned int i=0; i < chain_info_.joint_names.size(); i++) {
00207 if (chain_info_.joint_names[i] == name)
00208 return i;
00209 }
00210 return -1;
00211 }
00212
00213 int KDLArmKinematicsPlugin::getKDLSegmentIndex(const std::string &name)
00214 {
00215 int i=0;
00216 while (i < (int)kdl_chain_.getNrOfSegments()) {
00217 if (kdl_chain_.getSegment(i).getName() == name) {
00218 return i+1;
00219 }
00220 i++;
00221 }
00222 return -1;
00223 }
00224
00225 bool KDLArmKinematicsPlugin::getPositionIK(const geometry_msgs::Pose &ik_pose,
00226 const std::vector<double> &ik_seed_state,
00227 std::vector<double> &solution,
00228 int &error_code)
00229 {
00230 if(!active_)
00231 {
00232 ROS_ERROR("kinematics not active");
00233 return false;
00234 }
00235 KDL::Frame pose_desired;
00236 tf::PoseMsgToKDL(ik_pose, pose_desired);
00237
00238 KDL::JntArray jnt_pos_in;
00239 KDL::JntArray jnt_pos_out;
00240 jnt_pos_in.resize(dimension_);
00241 for(unsigned int i=0; i < dimension_; i++)
00242 {
00243 jnt_pos_in(i) = ik_seed_state[i];
00244 }
00245 int ik_valid = ik_solver_pos_->CartToJnt(jnt_pos_in,pose_desired,jnt_pos_out);
00246 if(ik_valid >= 0)
00247 {
00248 solution.resize(dimension_);
00249 for(unsigned int i=0; i < dimension_; i++)
00250 {
00251 solution[i] = jnt_pos_out(i);
00252 }
00253 error_code = kinematics::SUCCESS;
00254 return true;
00255 }
00256 else
00257 {
00258 ROS_DEBUG("An IK solution could not be found");
00259 error_code = kinematics::NO_IK_SOLUTION;
00260 return false;
00261 }
00262 }
00263
00264 bool KDLArmKinematicsPlugin::searchPositionIK(const geometry_msgs::Pose &ik_pose,
00265 const std::vector<double> &ik_seed_state,
00266 const double &timeout,
00267 std::vector<double> &solution,
00268 int &error_code)
00269 {
00270 if(!active_)
00271 {
00272 ROS_ERROR("kinematics not active");
00273 error_code = kinematics::INACTIVE;
00274 return false;
00275 }
00276 KDL::Frame pose_desired;
00277 tf::PoseMsgToKDL(ik_pose, pose_desired);
00278
00279
00280 KDL::JntArray jnt_pos_in;
00281 KDL::JntArray jnt_pos_out;
00282 jnt_pos_in.resize(dimension_);
00283 for(unsigned int i=0; i < dimension_; i++)
00284 {
00285 jnt_pos_in(i) = ik_seed_state[i];
00286 }
00287
00288 int ik_valid = ik_solver_pos_->CartToJnt(jnt_pos_in,pose_desired,jnt_pos_out);
00289
00290 if(ik_valid >= 0)
00291 {
00292 solution.resize(dimension_);
00293 for(unsigned int i=0; i < dimension_; i++)
00294 solution[i] = jnt_pos_out(i);
00295 error_code = kinematics::SUCCESS;
00296 return true;
00297 }
00298 else
00299 {
00300 ROS_DEBUG("An IK solution could not be found");
00301 error_code = kinematics::NO_IK_SOLUTION;
00302 return false;
00303 }
00304 }
00305
00306 bool KDLArmKinematicsPlugin::searchPositionIK(const geometry_msgs::Pose &ik_pose,
00307 const std::vector<double> &ik_seed_state,
00308 const double &timeout,
00309 std::vector<double> &solution,
00310 const boost::function<void(const geometry_msgs::Pose &ik_pose,const std::vector<double> &ik_solution,int &error_code)> &desired_pose_callback,
00311 const boost::function<void(const geometry_msgs::Pose &ik_pose,const std::vector<double> &ik_solution,int &error_code)> &solution_callback,
00312 int &error_code)
00313 {
00314 if(!active_)
00315 {
00316 ROS_ERROR("kinematics not active");
00317 error_code = kinematics::INACTIVE;
00318 return false;
00319 }
00320 KDL::Frame pose_desired;
00321 tf::PoseMsgToKDL(ik_pose, pose_desired);
00322
00323
00324 KDL::JntArray jnt_pos_in;
00325 KDL::JntArray jnt_pos_out;
00326 jnt_pos_in.resize(dimension_);
00327 for(unsigned int i=0; i < dimension_; i++)
00328 jnt_pos_in(i) = ik_seed_state[i];
00329
00330 if(!desired_pose_callback.empty())
00331 desired_pose_callback(ik_pose,ik_seed_state,error_code);
00332
00333 if(error_code < 0)
00334 {
00335 ROS_ERROR("Could not find inverse kinematics for desired end-effector pose since the pose may be in collision");
00336 return false;
00337 }
00338 for(int i=0; i < max_search_iterations_; i++)
00339 {
00340 jnt_pos_in = getRandomConfiguration();
00341 int ik_valid = ik_solver_pos_->CartToJnt(jnt_pos_in,pose_desired,jnt_pos_out);
00342 if(ik_valid < 0)
00343 continue;
00344 std::vector<double> solution_local;
00345 solution_local.resize(dimension_);
00346 for(unsigned int j=0; j < dimension_; j++)
00347 solution_local[j] = jnt_pos_out(j);
00348 solution_callback(ik_pose,solution_local,error_code);
00349 if(error_code == kinematics::SUCCESS)
00350 {
00351 solution = solution_local;
00352 return true;
00353 }
00354 }
00355 ROS_DEBUG("An IK that satisifes the constraints and is collision free could not be found");
00356 if (error_code == 0)
00357 error_code = kinematics::NO_IK_SOLUTION;
00358 return false;
00359 }
00360
00361 bool KDLArmKinematicsPlugin::getPositionFK(const std::vector<std::string> &link_names,
00362 const std::vector<double> &joint_angles,
00363 std::vector<geometry_msgs::Pose> &poses)
00364 {
00365 if(!active_)
00366 {
00367 ROS_ERROR("kinematics not active");
00368 return false;
00369 }
00370
00371 KDL::Frame p_out;
00372 KDL::JntArray jnt_pos_in;
00373 geometry_msgs::PoseStamped pose;
00374 tf::Stamped<tf::Pose> tf_pose;
00375
00376 jnt_pos_in.resize(dimension_);
00377 for(unsigned int i=0; i < dimension_; i++)
00378 {
00379 jnt_pos_in(i) = joint_angles[i];
00380 }
00381
00382 poses.resize(link_names.size());
00383
00384 bool valid = true;
00385 for(unsigned int i=0; i < poses.size(); i++)
00386 {
00387 ROS_DEBUG("End effector index: %d",getKDLSegmentIndex(link_names[i]));
00388 if(fk_solver_->JntToCart(jnt_pos_in,p_out,getKDLSegmentIndex(link_names[i])) >=0)
00389 {
00390 tf::PoseKDLToMsg(p_out,poses[i]);
00391 }
00392 else
00393 {
00394 ROS_ERROR("Could not compute FK for %s",link_names[i].c_str());
00395 valid = false;
00396 }
00397 }
00398 return valid;
00399 }
00400
00401 std::string KDLArmKinematicsPlugin::getBaseFrame()
00402 {
00403 if(!active_)
00404 {
00405 ROS_ERROR("kinematics not active");
00406 return std::string("");
00407 }
00408 return root_name_;
00409 }
00410
00411 std::string KDLArmKinematicsPlugin::getToolFrame()
00412 {
00413 if(!active_ || chain_info_.link_names.empty())
00414 {
00415 ROS_ERROR("kinematics not active");
00416 return std::string("");
00417 }
00418 return chain_info_.link_names[0];
00419 }
00420
00421 std::vector<std::string> KDLArmKinematicsPlugin::getJointNames()
00422 {
00423 if(!active_)
00424 {
00425 std::vector<std::string> empty;
00426 ROS_ERROR("kinematics not active");
00427 return empty;
00428 }
00429 return chain_info_.joint_names;
00430 }
00431
00432 std::vector<std::string> KDLArmKinematicsPlugin::getLinkNames()
00433 {
00434 if(!active_)
00435 {
00436 std::vector<std::string> empty;
00437 ROS_ERROR("kinematics not active");
00438 return empty;
00439 }
00440 return chain_info_.link_names;
00441 }
00442
00443 }