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 <trac_ik/trac_ik.hpp>
00033 #include <boost/date_time.hpp>
00034 #include <Eigen/Geometry>
00035 #include <ros/ros.h>
00036 #include <limits>
00037 #include <kdl_parser/kdl_parser.hpp>
00038 #include <urdf/model.h>
00039
00040 namespace TRAC_IK
00041 {
00042
00043 TRAC_IK::TRAC_IK(const std::string& base_link, const std::string& tip_link, const std::string& URDF_param, double _maxtime, double _eps, SolveType _type) :
00044 initialized(false),
00045 eps(_eps),
00046 maxtime(_maxtime),
00047 solvetype(_type)
00048 {
00049
00050 ros::NodeHandle node_handle("~");
00051
00052 urdf::Model robot_model;
00053 std::string xml_string;
00054
00055 std::string urdf_xml, full_urdf_xml;
00056 node_handle.param("urdf_xml", urdf_xml, URDF_param);
00057 node_handle.searchParam(urdf_xml, full_urdf_xml);
00058
00059 ROS_DEBUG_NAMED("trac_ik", "Reading xml file from parameter server");
00060 if (!node_handle.getParam(full_urdf_xml, xml_string))
00061 {
00062 ROS_FATAL_NAMED("trac_ik", "Could not load the xml from parameter server: %s", urdf_xml.c_str());
00063 return;
00064 }
00065
00066 node_handle.param(full_urdf_xml, xml_string, std::string());
00067 robot_model.initString(xml_string);
00068
00069 ROS_DEBUG_STREAM_NAMED("trac_ik", "Reading joints and links from URDF");
00070
00071 KDL::Tree tree;
00072
00073 if (!kdl_parser::treeFromUrdfModel(robot_model, tree))
00074 ROS_FATAL("Failed to extract kdl tree from xml robot description");
00075
00076 if (!tree.getChain(base_link, tip_link, chain))
00077 ROS_FATAL("Couldn't find chain %s to %s", base_link.c_str(), tip_link.c_str());
00078
00079 std::vector<KDL::Segment> chain_segs = chain.segments;
00080
00081 urdf::JointConstSharedPtr joint;
00082
00083 std::vector<double> l_bounds, u_bounds;
00084
00085 lb.resize(chain.getNrOfJoints());
00086 ub.resize(chain.getNrOfJoints());
00087
00088 uint joint_num = 0;
00089 for (unsigned int i = 0; i < chain_segs.size(); ++i)
00090 {
00091 joint = robot_model.getJoint(chain_segs[i].getJoint().getName());
00092 if (joint->type != urdf::Joint::UNKNOWN && joint->type != urdf::Joint::FIXED)
00093 {
00094 joint_num++;
00095 float lower, upper;
00096 int hasLimits;
00097 if (joint->type != urdf::Joint::CONTINUOUS)
00098 {
00099 if (joint->safety)
00100 {
00101 lower = std::max(joint->limits->lower, joint->safety->soft_lower_limit);
00102 upper = std::min(joint->limits->upper, joint->safety->soft_upper_limit);
00103 }
00104 else
00105 {
00106 lower = joint->limits->lower;
00107 upper = joint->limits->upper;
00108 }
00109 hasLimits = 1;
00110 }
00111 else
00112 {
00113 hasLimits = 0;
00114 }
00115 if (hasLimits)
00116 {
00117 lb(joint_num - 1) = lower;
00118 ub(joint_num - 1) = upper;
00119 }
00120 else
00121 {
00122 lb(joint_num - 1) = std::numeric_limits<float>::lowest();
00123 ub(joint_num - 1) = std::numeric_limits<float>::max();
00124 }
00125 ROS_DEBUG_STREAM_NAMED("trac_ik", "IK Using joint " << joint->name << " " << lb(joint_num - 1) << " " << ub(joint_num - 1));
00126 }
00127 }
00128
00129 initialize();
00130 }
00131
00132
00133 TRAC_IK::TRAC_IK(const KDL::Chain& _chain, const KDL::JntArray& _q_min, const KDL::JntArray& _q_max, double _maxtime, double _eps, SolveType _type):
00134 initialized(false),
00135 chain(_chain),
00136 lb(_q_min),
00137 ub(_q_max),
00138 eps(_eps),
00139 maxtime(_maxtime),
00140 solvetype(_type)
00141 {
00142 initialize();
00143 }
00144
00145 void TRAC_IK::initialize()
00146 {
00147
00148 assert(chain.getNrOfJoints() == lb.data.size());
00149 assert(chain.getNrOfJoints() == ub.data.size());
00150
00151 jacsolver.reset(new KDL::ChainJntToJacSolver(chain));
00152 nl_solver.reset(new NLOPT_IK::NLOPT_IK(chain, lb, ub, maxtime, eps, NLOPT_IK::SumSq));
00153 iksolver.reset(new KDL::ChainIkSolverPos_TL(chain, lb, ub, maxtime, eps, true, true));
00154
00155 for (uint i = 0; i < chain.segments.size(); i++)
00156 {
00157 std::string type = chain.segments[i].getJoint().getTypeName();
00158 if (type.find("Rot") != std::string::npos)
00159 {
00160 if (ub(types.size()) >= std::numeric_limits<float>::max() &&
00161 lb(types.size()) <= std::numeric_limits<float>::lowest())
00162 types.push_back(KDL::BasicJointType::Continuous);
00163 else
00164 types.push_back(KDL::BasicJointType::RotJoint);
00165 }
00166 else if (type.find("Trans") != std::string::npos)
00167 types.push_back(KDL::BasicJointType::TransJoint);
00168 }
00169
00170 assert(types.size() == lb.data.size());
00171
00172 initialized = true;
00173 }
00174
00175 bool TRAC_IK::unique_solution(const KDL::JntArray& sol)
00176 {
00177
00178 for (uint i = 0; i < solutions.size(); i++)
00179 if (myEqual(sol, solutions[i]))
00180 return false;
00181 return true;
00182
00183 }
00184
00185 inline void normalizeAngle(double& val, const double& min, const double& max)
00186 {
00187 if (val > max)
00188 {
00189
00190 double diffangle = fmod(val - max, 2 * M_PI);
00191
00192 val = max + diffangle - 2 * M_PI;
00193 }
00194
00195 if (val < min)
00196 {
00197
00198 double diffangle = fmod(min - val, 2 * M_PI);
00199
00200 val = min - diffangle + 2 * M_PI;
00201 }
00202 }
00203
00204 inline void normalizeAngle(double& val, const double& target)
00205 {
00206 if (val > target + M_PI)
00207 {
00208
00209 double diffangle = fmod(val - target, 2 * M_PI);
00210
00211 val = target + diffangle - 2 * M_PI;
00212 }
00213
00214 if (val < target - M_PI)
00215 {
00216
00217 double diffangle = fmod(target - val, 2 * M_PI);
00218
00219 val = target - diffangle + 2 * M_PI;
00220 }
00221 }
00222
00223
00224 template<typename T1, typename T2>
00225 bool TRAC_IK::runSolver(T1& solver, T2& other_solver,
00226 const KDL::JntArray &q_init,
00227 const KDL::Frame &p_in)
00228 {
00229 KDL::JntArray q_out;
00230
00231 double fulltime = maxtime;
00232 KDL::JntArray seed = q_init;
00233
00234 boost::posix_time::time_duration timediff;
00235 double time_left;
00236
00237 while (true)
00238 {
00239 timediff = boost::posix_time::microsec_clock::local_time() - start_time;
00240 time_left = fulltime - timediff.total_nanoseconds() / 1000000000.0;
00241
00242 if (time_left <= 0)
00243 break;
00244
00245 solver.setMaxtime(time_left);
00246
00247 int RC = solver.CartToJnt(seed, p_in, q_out, bounds);
00248 if (RC >= 0)
00249 {
00250 switch (solvetype)
00251 {
00252 case Manip1:
00253 case Manip2:
00254 normalize_limits(q_init, q_out);
00255 break;
00256 default:
00257 normalize_seed(q_init, q_out);
00258 break;
00259 }
00260 mtx_.lock();
00261 if (unique_solution(q_out))
00262 {
00263 solutions.push_back(q_out);
00264 uint curr_size = solutions.size();
00265 errors.resize(curr_size);
00266 mtx_.unlock();
00267 double err, penalty;
00268 switch (solvetype)
00269 {
00270 case Manip1:
00271 penalty = manipPenalty(q_out);
00272 err = penalty * TRAC_IK::ManipValue1(q_out);
00273 break;
00274 case Manip2:
00275 penalty = manipPenalty(q_out);
00276 err = penalty * TRAC_IK::ManipValue2(q_out);
00277 break;
00278 default:
00279 err = TRAC_IK::JointErr(q_init, q_out);
00280 break;
00281 }
00282 mtx_.lock();
00283 errors[curr_size - 1] = std::make_pair(err, curr_size - 1);
00284 }
00285 mtx_.unlock();
00286 }
00287
00288 if (!solutions.empty() && solvetype == Speed)
00289 break;
00290
00291 for (unsigned int j = 0; j < seed.data.size(); j++)
00292 if (types[j] == KDL::BasicJointType::Continuous)
00293 seed(j) = fRand(q_init(j) - 2 * M_PI, q_init(j) + 2 * M_PI);
00294 else
00295 seed(j) = fRand(lb(j), ub(j));
00296 }
00297 other_solver.abort();
00298
00299 solver.setMaxtime(fulltime);
00300
00301 return true;
00302 }
00303
00304
00305 void TRAC_IK::normalize_seed(const KDL::JntArray& seed, KDL::JntArray& solution)
00306 {
00307
00308
00309
00310 bool improved = false;
00311
00312 for (uint i = 0; i < lb.data.size(); i++)
00313 {
00314
00315 if (types[i] == KDL::BasicJointType::TransJoint)
00316 continue;
00317
00318 double target = seed(i);
00319 double val = solution(i);
00320
00321 normalizeAngle(val, target);
00322
00323 if (types[i] == KDL::BasicJointType::Continuous)
00324 {
00325 solution(i) = val;
00326 continue;
00327 }
00328
00329 normalizeAngle(val, lb(i), ub(i));
00330
00331 solution(i) = val;
00332 }
00333 }
00334
00335 void TRAC_IK::normalize_limits(const KDL::JntArray& seed, KDL::JntArray& solution)
00336 {
00337
00338
00339
00340 bool improved = false;
00341
00342 for (uint i = 0; i < lb.data.size(); i++)
00343 {
00344
00345 if (types[i] == KDL::BasicJointType::TransJoint)
00346 continue;
00347
00348 double target = seed(i);
00349
00350 if (types[i] == KDL::BasicJointType::RotJoint && types[i] != KDL::BasicJointType::Continuous)
00351 target = (ub(i) + lb(i)) / 2.0;
00352
00353 double val = solution(i);
00354
00355 normalizeAngle(val, target);
00356
00357 if (types[i] == KDL::BasicJointType::Continuous)
00358 {
00359 solution(i) = val;
00360 continue;
00361 }
00362
00363 normalizeAngle(val, lb(i), ub(i));
00364
00365 solution(i) = val;
00366 }
00367
00368 }
00369
00370
00371 double TRAC_IK::manipPenalty(const KDL::JntArray& arr)
00372 {
00373 double penalty = 1.0;
00374 for (uint i = 0; i < arr.data.size(); i++)
00375 {
00376 if (types[i] == KDL::BasicJointType::Continuous)
00377 continue;
00378 double range = ub(i) - lb(i);
00379 penalty *= ((arr(i) - lb(i)) * (ub(i) - arr(i)) / (range * range));
00380 }
00381 return std::max(0.0, 1.0 - exp(-1 * penalty));
00382 }
00383
00384
00385 double TRAC_IK::ManipValue1(const KDL::JntArray& arr)
00386 {
00387 KDL::Jacobian jac(arr.data.size());
00388
00389 jacsolver->JntToJac(arr, jac);
00390
00391 Eigen::JacobiSVD<Eigen::MatrixXd> svdsolver(jac.data);
00392 Eigen::MatrixXd singular_values = svdsolver.singularValues();
00393
00394 double error = 1.0;
00395 for (unsigned int i = 0; i < singular_values.rows(); ++i)
00396 error *= singular_values(i, 0);
00397 return error;
00398 }
00399
00400 double TRAC_IK::ManipValue2(const KDL::JntArray& arr)
00401 {
00402 KDL::Jacobian jac(arr.data.size());
00403
00404 jacsolver->JntToJac(arr, jac);
00405
00406 Eigen::JacobiSVD<Eigen::MatrixXd> svdsolver(jac.data);
00407 Eigen::MatrixXd singular_values = svdsolver.singularValues();
00408
00409 return singular_values.minCoeff() / singular_values.maxCoeff();
00410 }
00411
00412
00413 int TRAC_IK::CartToJnt(const KDL::JntArray &q_init, const KDL::Frame &p_in, KDL::JntArray &q_out, const KDL::Twist& _bounds)
00414 {
00415
00416 if (!initialized)
00417 {
00418 ROS_ERROR("TRAC-IK was not properly initialized with a valid chain or limits. IK cannot proceed");
00419 return -1;
00420 }
00421
00422
00423 start_time = boost::posix_time::microsec_clock::local_time();
00424
00425 nl_solver->reset();
00426 iksolver->reset();
00427
00428 solutions.clear();
00429 errors.clear();
00430
00431 bounds = _bounds;
00432
00433 task1 = std::thread(&TRAC_IK::runKDL, this, q_init, p_in);
00434 task2 = std::thread(&TRAC_IK::runNLOPT, this, q_init, p_in);
00435
00436 task1.join();
00437 task2.join();
00438
00439 if (solutions.empty())
00440 {
00441 q_out = q_init;
00442 return -3;
00443 }
00444
00445 switch (solvetype)
00446 {
00447 case Manip1:
00448 case Manip2:
00449 std::sort(errors.rbegin(), errors.rend());
00450 break;
00451 default:
00452 std::sort(errors.begin(), errors.end());
00453 break;
00454 }
00455
00456 q_out = solutions[errors[0].second];
00457
00458 return solutions.size();
00459 }
00460
00461
00462 TRAC_IK::~TRAC_IK()
00463 {
00464 if (task1.joinable())
00465 task1.join();
00466 if (task2.joinable())
00467 task2.join();
00468 }
00469 }