00001
00019 #include <urdf_traverser/Functions.h>
00020 #include <urdf_traverser/DependencyOrderedJoints.h>
00021
00022 #include <urdf2inventor/IVHelpers.h>
00023 #include <urdf2graspit/ContactFunctions.h>
00024 #include <urdf2graspit/ContactsGenerator.h>
00025 #include <urdf2inventor/Helpers.h>
00026
00027 #include <urdf2graspit/Types.h>
00028 #include <urdf2graspit/MarkerSelector.h>
00029
00030 #include <string>
00031 #include <ros/ros.h>
00032 #include <ros/package.h>
00033
00034 #include <map>
00035 #include <vector>
00036 #include <set>
00037
00038 #define RAD_TO_DEG 180/M_PI
00039
00040 using urdf2graspit::ContactsGenerator;
00041 using urdf2graspit::markerselector::MarkerSelector;
00042 using urdf2inventor::EigenTransform;
00043
00044 bool ContactsGenerator::transformToDHReferenceFrames(const std::vector<DHParam>& dh)
00045 {
00046 UrdfTraverserPtr trav = getTraverser();
00047 if (!trav)
00048 {
00049 ROS_ERROR("Traverser not set.");
00050 return false;
00051 }
00052 std::map<std::string,EigenTransform> transforms;
00053 if (!DHParam::getTransforms(dh, true, transforms))
00054 {
00055 ROS_ERROR("Could not get transforms from DH to URDF");
00056 return false;
00057 }
00058
00059 std::map<std::string,EigenTransform>::iterator it;
00060 for (it=transforms.begin(); it!=transforms.end(); ++it)
00061 {
00062 LinkPtr link=trav->getLink(it->first);
00063 if (!link.get())
00064 {
00065 ROS_ERROR("Link %s does not exist", it->first.c_str());
00066 return false;
00067 }
00068 bool preApply = true;
00069 applyTransformToContacts(link, it->second, preApply);
00070 }
00071 return true;
00072 }
00073
00074
00075 void ContactsGenerator::applyTransformToContacts(LinkPtr& link, const EigenTransform& trans, bool preMult)
00076 {
00077 std::map<std::string, std::vector<ContactPtr> >::iterator lCnt = linkContacts.find(link->name);
00078 if (lCnt != linkContacts.end())
00079 {
00080 for (std::vector<ContactPtr>::iterator it = lCnt->second.begin(); it != lCnt->second.end(); ++it)
00081 {
00082 ContactPtr c = *it;
00083 EigenTransform t = EigenTransform::Identity();
00084 t.translate(c->loc);
00085 t.rotate(c->ori);
00086
00087 if (preMult) t = trans * t;
00088 else t = t * trans;
00089 c->loc = t.translation();
00090 c->ori = Eigen::Quaterniond(t.rotation());
00091 c->norm = trans.rotation() * c->norm;
00092 }
00093 }
00094 }
00095
00096 void ContactsGenerator::scaleContacts(double scale_factor)
00097 {
00098 if (isContactsScaled) return;
00099 std::map<std::string, std::vector<ContactPtr> >::iterator linkItr;
00100 for (linkItr=linkContacts.begin(); linkItr!=linkContacts.end(); ++linkItr)
00101 {
00102 for (std::vector<ContactPtr>::iterator it = linkItr->second.begin(); it != linkItr->second.end(); ++it)
00103 {
00104 ContactPtr c = *it;
00105 c->loc *= scale_factor;
00106 }
00107 }
00108 isContactsScaled = true;
00109 }
00110
00111 bool ContactsGenerator::generateContactsForallVisuals(const std::string& linkName, const int linkNum, const int fingerNum,
00112 const float coefficient, const std::vector<MarkerSelector::Marker>& markers)
00113 {
00114 UrdfTraverserPtr trav = getTraverser();
00115 if (!trav)
00116 {
00117 ROS_ERROR("Traverser not set.");
00118 return false;
00119 }
00120
00121 LinkPtr link = trav->getLink(linkName);
00122
00123 std::map<std::string, std::vector<ContactPtr> >::iterator lCnt = linkContacts.find(linkName);
00124 if (lCnt == linkContacts.end())
00125 lCnt = linkContacts.insert(std::make_pair(linkName, std::vector<ContactPtr>())).first;
00126
00127 std::vector<MarkerSelector::Marker>::const_iterator m = markers.begin();
00128 int visualNum = -1;
00129 for (std::vector<VisualPtr >::iterator vit = link->visual_array.begin();
00130 vit != link->visual_array.end(); ++vit)
00131 {
00132 ++visualNum;
00133
00134 if ((m != markers.end()) && (m->visualNum > visualNum)) continue;
00135
00136 VisualPtr visual = *vit;
00137
00138 EigenTransform vTrans = urdf_traverser::getTransform(visual->origin);
00139
00140
00141 while (m != markers.end())
00142 {
00143
00144 if (m->visualNum == visualNum)
00145 {
00146 break;
00147 }
00148 ++m;
00149 }
00150
00151
00152
00153
00154
00155
00156 while ((m != markers.end()) && (m->visualNum == visualNum))
00157 {
00158 ContactPtr contact(new Contact());
00159 contact->linkNum = linkNum;
00160 contact->fingerNum = fingerNum;
00161
00162 setUpSoftFrictionEdges(contact->numFrictionEdges, contact->frictionEdges);
00163
00164
00165
00166
00167 Eigen::Vector3d pos(m->coords);
00168 Eigen::Vector3d norm(m->normal);
00169 norm.normalize();
00170
00171
00172
00173
00174
00175 EigenTransform toPos = EigenTransform::Identity();
00176 Eigen::Vector3d dNorm(0, 0, 1);
00177
00178 Eigen::Quaterniond rotNorm = Eigen::Quaterniond::FromTwoVectors(dNorm, norm);
00179 toPos.translate(pos);
00180 toPos.rotate(rotNorm);
00181
00182 EigenTransform jtToPos = vTrans * toPos;
00183 pos = jtToPos.translation();
00184 contact->loc = pos;
00185
00186 Eigen::Quaterniond ori(jtToPos.rotation());
00187 contact->ori = ori;
00188
00189
00190 norm = dNorm;
00191 norm.normalize();
00192 contact->norm = norm;
00193 contact->cof = coefficient;
00194
00195 contacts.push_back(contact);
00196 lCnt->second.push_back(contact);
00197
00198
00199
00200
00201 ++m;
00202 }
00203 }
00204
00205 return true;
00206 }
00207
00208
00209 bool ContactsGenerator::generateContacts(const std::vector<std::string>& rootFingerJoints,
00210 const std::string& palmLinkName,
00211 const float coefficient, const MarkerSelector::MarkerMap& markers,
00212 const std::vector<DHParam>& dh)
00213 {
00214 UrdfTraverserPtr trav = getTraverser();
00215 if (!trav)
00216 {
00217 ROS_ERROR("Traverser not set.");
00218 return false;
00219 }
00220
00221 LinkPtr palm_link = trav->getLink(palmLinkName);
00222 if (!palm_link.get())
00223 {
00224 ROS_ERROR_STREAM("Palm link "<<palmLinkName<<" not found.");
00225 return false;
00226 }
00227
00228 initOutStructure(trav->getModelName());
00229
00230
00231 std::string palmLinkNameClean = MarkerSelector::Marker::toSoBaseName(palmLinkName);
00232 MarkerSelector::MarkerMap::const_iterator palmM = markers.find(palmLinkNameClean);
00233 if (palmM != markers.end())
00234 {
00235 int linkNum = 0;
00236 int fingerNum = -1;
00237 if (!generateContactsForallVisuals(palmLinkName, linkNum, fingerNum, coefficient, palmM->second))
00238 {
00239 ROS_ERROR("Failed to generate contact for link %s", palmLinkName.c_str());
00240 return false;
00241 }
00242 }
00243
00244
00245 int fingerNum = -1;
00246 for (std::vector<std::string>::const_iterator it = rootFingerJoints.begin();
00247 it != rootFingerJoints.end(); ++it)
00248 {
00249 ++fingerNum;
00250
00251
00252 JointPtr root_joint = trav->getJoint(*it);
00253 if (!root_joint)
00254 {
00255 ROS_ERROR("Could not find joint %s for generating contact", it->c_str());
00256 trav->printJointNames("");
00257 return false;
00258 }
00259 std::vector<JointPtr> chain;
00260 bool onlyActive = true;
00261 if (!urdf_traverser::getDependencyOrderedJoints(*trav, chain, root_joint, false,
00262 onlyActive) || chain.empty())
00263 {
00264 ROS_ERROR("Could not get joint chain, joint %s", root_joint->name.c_str());
00265 return false;
00266 }
00267
00268 int linkNum = -1;
00269 for (std::vector<JointPtr>::iterator cit = chain.begin(); cit != chain.end(); ++cit)
00270 {
00271 ++linkNum;
00272 JointPtr chainJoint = *cit;
00273 std::string linkName = chainJoint->child_link_name;
00274
00275 std::string linkNameClean = MarkerSelector::Marker::toSoBaseName(linkName);
00276 MarkerSelector::MarkerMap::const_iterator markerIt = markers.find(linkNameClean);
00277 if (markerIt == markers.end())
00278 {
00279
00280
00281 continue;
00282 }
00283
00284
00285
00286 if (!generateContactsForallVisuals(linkName, linkNum, fingerNum, coefficient, markerIt->second))
00287 {
00288 ROS_ERROR("Failed to generate contact for link %s", linkName.c_str());
00289 return false;
00290 }
00291 }
00292 }
00293
00294 if (!transformToDHReferenceFrames(dh))
00295 {
00296 ROS_ERROR("Could not transform the contacts to DH parameter space");
00297 return false;
00298 }
00299 scaleContacts(getScaleFactor());
00300 return true;
00301 }
00302
00303
00304 std::string ContactsGenerator::getContactsFileContent(const std::string& robotName)
00305 {
00306 std::stringstream str;
00307
00308 str << "<?xml version=\"1.0\" ?> " << std::endl;
00309 str << "<virtual_contacts>" << std::endl;
00310 str << "<robot_name>";
00311 str << robotName;
00312 str << "</robot_name>" << std::endl;
00313
00314
00315 int contSize = contacts.size();
00316
00317
00318 for (std::vector<ContactPtr>::const_iterator cit = contacts.begin(); cit != contacts.end(); ++cit)
00319 {
00320 ContactPtr c = *cit;
00321
00322
00323 str << "<virtual_contact>" << std::endl;
00324 str << "<finger_number>";
00325 str << c->fingerNum;
00326 str << "</finger_number>" << std::endl;
00327
00328 str << "<link_number>";
00329 str << c->linkNum;
00330 str << "</link_number>" << std::endl;
00331
00332 str << "<num_friction_edges>";
00333 str << c->numFrictionEdges;
00334 str << "</num_friction_edges>" << std::endl;
00335
00336 str << "<friction_edges>" << std::endl;
00337
00338
00339
00340 for (unsigned int i = 0; i < c->numFrictionEdges; ++i)
00341 {
00342 str << "<friction_edge>";
00343 for (unsigned int j = 0; j < 6; ++j)
00344 {
00345 str << c->frictionEdges[i * 6 + j] << " ";
00346 }
00347 str << "</friction_edge>" << std::endl;
00348 }
00349
00350 str << "</friction_edges>" << std::endl;
00351
00352 str << "<location>" << std::endl;
00353 str << static_cast<float>(c->loc.x()) << " "
00354 << static_cast<float>(c->loc.y()) << " "
00355 << static_cast<float>(c->loc.z());
00356 str << "</location>" << std::endl;
00357
00358 str << "<rotation>" << std::endl;
00359 str << static_cast<float>(c->ori.w()) << " "
00360 << static_cast<float>(c->ori.x()) << " "
00361 << static_cast<float>(c->ori.y()) << " "
00362 << static_cast<float>(c->ori.z());
00363 str << "</rotation>" << std::endl;
00364
00365
00366 str << "<translation>";
00367 str << static_cast<float>(c->loc.x()) << " "
00368 << static_cast<float>(c->loc.y()) << " "
00369 << static_cast<float>(c->loc.z());
00370 str << "</translation>" << std::endl;
00371
00372 str << "<normal>";
00373 str << static_cast<float>(c->norm.x()) << " "
00374 << static_cast<float>(c->norm.y()) << " "
00375 << static_cast<float>(c->norm.z());
00376 str << "</normal>" << std::endl;
00377
00378 str << "<sCof>";
00379 str << static_cast<float>(c->cof);
00380 str << "</sCof>" << std::endl;;
00381 str << "</virtual_contact>" << std::endl;
00382 }
00383 str << "</virtual_contacts>" << std::endl;
00384
00385 return str.str();
00386 }
00387
00388
00389 SoNode * ContactsGenerator::getAxesAsInventor(
00390 const LinkPtr& from_link,
00391 const std::vector<DHParam>& dh,
00392 float _axesRadius, float _axesLength,
00393 bool linkIsRoot)
00394 {
00395
00396
00397 EigenTransform transform;
00398 if (!linkIsRoot)
00399 {
00400 JointPtr parentJoint = from_link->parent_joint;
00401 DHParam jointDH;
00402 if (!getDHParam(parentJoint->name, dh, jointDH))
00403 {
00404 ROS_ERROR_STREAM("Could not get DH parameter for "<<parentJoint->name);
00405 return NULL;
00406 }
00407
00408 ROS_INFO_STREAM("* Using DH params "<<jointDH);
00409
00410 transform.setIdentity();
00411 Eigen::Vector3d x(1,0,0);
00412 Eigen::Vector3d y(0,1,0);
00413 Eigen::Vector3d z(0,0,1);
00414 transform.rotate(Eigen::AngleAxisd(jointDH.theta,z));
00415 transform.translate(z*jointDH.d);
00416 transform.rotate(Eigen::AngleAxisd(jointDH.alpha,x));
00417 transform.translate(x*jointDH.r);
00418 }
00419 else
00420 {
00421 ROS_INFO_STREAM("* Transforming as root node ");
00422 }
00423
00424 SoSeparator * allVisuals = new SoSeparator();
00425 allVisuals->ref();
00426
00427
00428 addLocalAxes(from_link, allVisuals, false, _axesRadius, _axesLength);
00429
00430 UrdfTraverserPtr trav = getTraverser();
00431 if (!trav)
00432 {
00433 ROS_ERROR("Traverser not set.");
00434 return NULL;
00435 }
00436
00437
00438 for (std::vector<JointPtr>::const_iterator pj = from_link->child_joints.begin();
00439 pj != from_link->child_joints.end(); pj++)
00440 {
00441 JointPtr joint = *pj;
00442 LinkPtr childLink = trav->getLink(joint->child_link_name);
00443 SoNode * childNode = getAxesAsInventor(childLink, dh, _axesRadius, _axesLength, false);
00444 if (!childNode)
00445 {
00446 ROS_ERROR_STREAM("Could not get child node for "<<childLink->name);
00447 return NULL;
00448 }
00449 if (linkIsRoot)
00450 {
00451 transform = urdf_traverser::getTransform(joint);
00452 }
00453 urdf2inventor::addSubNode(childNode, allVisuals, transform);
00454 }
00455
00456 return allVisuals;
00457 }
00458
00459 bool ContactsGenerator::getDHParam(const std::string jointName, const std::vector<DHParam>& dh, DHParam& jointDH)
00460 {
00461 for (std::vector<DHParam>::const_iterator it=dh.begin(); it!=dh.end(); ++it)
00462 {
00463 if (it->joint->name == jointName)
00464 {
00465 jointDH=*it;
00466 return true;
00467 }
00468 }
00469 return false;
00470 }
00471
00472 bool ContactsGenerator::generateContactsWithViewer(const std::vector<std::string>& fingerRoots,
00473 const std::string& palmLinkName, float standard_coefficient,
00474 const std::vector<DHParam>& dh,
00475 bool _displayAxes, bool _axesFromDH,
00476 float _axesRadius, float _axesLength,
00477 const EigenTransform& addVisualTransform,
00478 bool facesCCW)
00479 {
00480 UrdfTraverserPtr trav = getTraverser();
00481 if (!trav)
00482 {
00483 ROS_ERROR("Traverser not set.");
00484 return false;
00485 }
00486
00487 LinkPtr palm = trav->getLink(palmLinkName);
00488 if (!palm.get())
00489 {
00490 ROS_ERROR_STREAM("Could not find palm link "<<palmLinkName);
00491 return false;
00492 }
00493
00494 if (!prepareModelForDenavitHartenberg(palmLinkName))
00495 {
00496 ROS_ERROR("Could not prepare for DH parameter compatible URDF model.");
00497 return false;
00498 }
00499
00500 SoNode * node = getAsInventor(palmLinkName, false,
00501 _displayAxes && !_axesFromDH, _axesRadius, _axesLength, addVisualTransform, NULL);
00502
00503 Eigen::Vector3d minBB, maxBB;
00504 urdf2inventor::getBoundingBox(node,minBB,maxBB);
00505 Eigen::Vector3d diagonal = maxBB-minBB;
00506 float markerFactor = 0.002;
00507
00508 bool success = true;
00509 MarkerSelector markerSelector(diagonal.norm()*markerFactor, facesCCW);
00510
00511 if (!node)
00512 {
00513 ROS_ERROR("Could not get inventor node");
00514 return false;
00515 }
00516
00517 if (_displayAxes && _axesFromDH)
00518 {
00519 SoNode * dhAxes = getAxesAsInventor (palm, dh, _axesRadius, _axesLength, true);
00520 if (!dhAxes)
00521 {
00522 ROS_ERROR("Could not the DH axes, so they won't be displayed");
00523 }
00524 else
00525 {
00526 SoSeparator * sep = dynamic_cast<SoSeparator*>(node);
00527 if (!sep)
00528 {
00529 ROS_ERROR("Inventor node parent is not a separator");
00530 }
00531 else
00532 {
00533 sep->addChild(dhAxes);
00534 }
00535 }
00536 }
00537
00538 ROS_INFO_STREAM("Model inventor files loaded, now loading into viewer...");
00539 markerSelector.init("Marker selector");
00540 markerSelector.loadModel(node);
00541 markerSelector.runViewer();
00542
00543 MarkerSelector::MarkerMap markers = markerSelector.getMarkers();
00544 ROS_INFO("Number of contacts: %lu",markers.size());
00545 ROS_INFO("Markers: %s",markerSelector.toString().c_str());
00546
00547
00548
00549 for (MarkerSelector::MarkerMap::iterator lit=markers.begin(); lit!=markers.end(); ++lit)
00550 {
00551 for (std::vector<MarkerSelector::Marker>::iterator mit=lit->second.begin(); mit!=lit->second.end(); ++mit)
00552 {
00553 MarkerSelector::Marker& m = *mit;
00554 m.coords = addVisualTransform*m.coords;
00555 m.normal = addVisualTransform*m.normal;
00556 }
00557 }
00558
00559 if (!generateContacts(fingerRoots, palmLinkName, standard_coefficient, markers, dh))
00560 {
00561 ROS_ERROR("could not generate contacts");
00562 return false;
00563 }
00564 return true;
00565 }
00566