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