planning_environment: planning_environment: model

00001 /*********************************************************************
00002 * Software License Agreement (BSD License)
00003 * 
00004 *  Copyright (c) 2010, Willow Garage, Inc.
00005 *  All rights reserved.
00006 * 
00007 *  Redistribution and use in source and binary forms, with or without
00008 *  modification, are permitted provided that the following conditions
00009 *  are met:
00010 * 
00011 *   * Redistributions of source code must retain the above copyright
00012 *     notice, this list of conditions and the following disclaimer.
00013 *   * Redistributions in binary form must reproduce the above
00014 *     copyright notice, this list of conditions and the following
00015 *     disclaimer in the documentation and/or other materials provided
00016 *     with the distribution.
00017 *   * Neither the name of the Willow Garage nor the names of its
00018 *     contributors may be used to endorse or promote products derived
00019 *     from this software without specific prior written permission.
00020 * 
00021 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032 *  POSSIBILITY OF SUCH DAMAGE.
00033 *********************************************************************/
00034 
00037 #include <planning_environment/models/model_utils.h>
00038 #include <geometric_shapes/bodies.h>
00039 #include <planning_environment/util/construct_object.h>
00040 
00041 //returns true if the joint_state_map sets all the joints in the state, 
00042 bool planning_environment::setRobotStateAndComputeTransforms(const arm_navigation_msgs::RobotState &robot_state,
00043                                                              planning_models::KinematicState& state)
00044 {
00045   if(robot_state.joint_state.name.size() != robot_state.joint_state.position.size()) {
00046     ROS_INFO_STREAM("Different number of names and positions: " << robot_state.joint_state.name.size() 
00047                     << " " << robot_state.joint_state.position.size());
00048     return false;
00049   }
00050   std::map<std::string, double> joint_state_map;
00051   for (unsigned int i = 0 ; i < robot_state.joint_state.name.size(); ++i)
00052   {
00053     joint_state_map[robot_state.joint_state.name[i]] = robot_state.joint_state.position[i];
00054   }
00055   std::vector<std::string> missing_states;
00056   bool complete = state.setKinematicState(joint_state_map, missing_states);
00057   if(!complete) {
00058     if(missing_states.empty()) {
00059       ROS_INFO("Incomplete, but no missing states");
00060     }
00061   }
00062   std::map<std::string, bool> has_missing_state_map;
00063   for(unsigned int i = 0; i < missing_states.size(); i++) {
00064     has_missing_state_map[missing_states[i]] = false;
00065   }
00066   bool need_to_update = false;
00067   if(robot_state.multi_dof_joint_state.joint_names.size() > 1) {
00068     ROS_INFO("More than 1 joint names");
00069   }
00070   for(unsigned int i = 0; i < robot_state.multi_dof_joint_state.joint_names.size(); i++) {
00071     std::string joint_name = robot_state.multi_dof_joint_state.joint_names[i];
00072     if(!state.hasJointState(joint_name)) {
00073       ROS_WARN_STREAM("No joint matching multi-dof joint " << joint_name);
00074       continue;
00075     }
00076     planning_models::KinematicState::JointState* joint_state = state.getJointState(joint_name);
00077     if(robot_state.multi_dof_joint_state.frame_ids.size() <= i) {
00078       ROS_INFO_STREAM("Robot state msg had bad multi_dof transform - not enough frame ids");
00079     } else if(robot_state.multi_dof_joint_state.child_frame_ids.size() <= i) {
00080       ROS_INFO_STREAM("Robot state msg had bad multi_dof transform - not enough child frame ids");
00081     } else if(robot_state.multi_dof_joint_state.frame_ids[i] != joint_state->getParentFrameId() ||
00082               robot_state.multi_dof_joint_state.child_frame_ids[i] != joint_state->getChildFrameId()) {
00083       ROS_WARN_STREAM("Robot state msg has bad multi_dof transform - frame ids or child frame_ids do not match up with joint");
00084     } else {
00085       tf::StampedTransform transf;
00086       tf::poseMsgToTF(robot_state.multi_dof_joint_state.poses[i], transf);
00087       joint_state->setJointStateValues(transf);
00088       need_to_update = true;
00089       //setting true for all individual joint names because we're setting the transform
00090       for(unsigned int i = 0; i < joint_state->getJointStateNameOrder().size(); i++) {
00091         has_missing_state_map[joint_state->getJointStateNameOrder()[i]] = true;
00092       }
00093     }
00094   }
00095   if(need_to_update) {
00096     state.updateKinematicLinks();
00097   }
00098   if(!complete) {
00099     for(std::map<std::string, bool>::iterator it = has_missing_state_map.begin();
00100         it != has_missing_state_map.end();
00101         it++) {
00102       if(!it->second) {
00103         return false;
00104       }
00105     }
00106     return true;
00107   }
00108   return true;
00109 }
00110 
00111 void planning_environment::convertKinematicStateToRobotState(const planning_models::KinematicState& kinematic_state,
00112                                                              const ros::Time& timestamp,
00113                                                              const std::string& header_frame,
00114                                                              arm_navigation_msgs::RobotState &robot_state)
00115 {
00116   robot_state.joint_state.position.clear();
00117   robot_state.joint_state.name.clear();
00118 
00119   robot_state.multi_dof_joint_state.joint_names.clear();
00120   robot_state.multi_dof_joint_state.frame_ids.clear();
00121   robot_state.multi_dof_joint_state.child_frame_ids.clear();
00122   robot_state.multi_dof_joint_state.poses.clear();
00123 
00124   const std::vector<planning_models::KinematicState::JointState*>& joints = kinematic_state.getJointStateVector();
00125   for(std::vector<planning_models::KinematicState::JointState*>::const_iterator it = joints.begin();
00126       it != joints.end();
00127       it++) {
00128     const std::vector<double>& joint_state_values = (*it)->getJointStateValues();
00129     const std::vector<std::string>& joint_state_value_names = (*it)->getJointStateNameOrder();
00130     for(unsigned int i = 0; i < joint_state_values.size(); i++) {
00131       robot_state.joint_state.name.push_back(joint_state_value_names[i]);
00132       robot_state.joint_state.position.push_back(joint_state_values[i]);
00133     }
00134     if(!(*it)->getParentFrameId().empty() && !(*it)->getChildFrameId().empty()) {
00135       robot_state.multi_dof_joint_state.stamp = ros::Time::now();
00136       robot_state.multi_dof_joint_state.joint_names.push_back((*it)->getName());
00137       robot_state.multi_dof_joint_state.frame_ids.push_back((*it)->getParentFrameId());
00138       robot_state.multi_dof_joint_state.child_frame_ids.push_back((*it)->getChildFrameId());
00139       tf::Pose p((*it)->getVariableTransform());
00140       geometry_msgs::Pose pose;
00141       tf::poseTFToMsg(p, pose);
00142       robot_state.multi_dof_joint_state.poses.push_back(pose);
00143     }
00144   }
00145   robot_state.joint_state.header.stamp = timestamp;
00146   robot_state.joint_state.header.frame_id = header_frame;
00147   return;
00148 }
00149 
00150 void planning_environment::applyOrderedCollisionOperationsToMatrix(const arm_navigation_msgs::OrderedCollisionOperations &ord,
00151                                                                    collision_space::EnvironmentModel::AllowedCollisionMatrix& acm) {
00152   if(ord.collision_operations.size() == 0) {
00153     ROS_INFO_STREAM("No allowed collision operations");
00154   }
00155   
00156   for(size_t i = 0; i < ord.collision_operations.size(); i++) {
00157     
00158     bool allowed = (ord.collision_operations[i].operation == arm_navigation_msgs::CollisionOperation::DISABLE);
00159 
00160     ROS_INFO_STREAM("Setting collision operation between " << ord.collision_operations[i].object1
00161                     << " and " << ord.collision_operations[i].object2 << " allowed " << allowed);
00162 
00163     if(ord.collision_operations[i].object1 == ord.collision_operations[i].COLLISION_SET_ALL &&
00164        ord.collision_operations[i].object2 == ord.collision_operations[i].COLLISION_SET_ALL) {
00165       acm.changeEntry(allowed);
00166       ROS_INFO_STREAM("Should be setting all");
00167     } else if(ord.collision_operations[i].object1 == ord.collision_operations[i].COLLISION_SET_ALL ||
00168               ord.collision_operations[i].object2 == ord.collision_operations[i].COLLISION_SET_ALL) {
00169       //second case - only one all
00170       std::string other;
00171       if(ord.collision_operations[i].object1 == ord.collision_operations[i].COLLISION_SET_ALL) {
00172         other = ord.collision_operations[i].object2;
00173       } else {
00174         other = ord.collision_operations[i].object1;
00175       }
00176       bool ok = acm.changeEntry(other,allowed);
00177       if(!ok) {
00178         ROS_WARN_STREAM("No allowed collision entry for " << other);
00179       }
00180     } else {
00181       //third case - must be both objects
00182       bool ok = acm.changeEntry(ord.collision_operations[i].object1,
00183                                 ord.collision_operations[i].object2,
00184                                 allowed);
00185       if(!ok) {
00186         ROS_WARN_STREAM("No entry in allowed collision matrix for at least one of " 
00187                         << ord.collision_operations[i].object1 << " and " 
00188                         << ord.collision_operations[i].object2);
00189       }
00190     }
00191   }
00192 }
00193 
00194 void planning_environment::convertFromACMToACMMsg(const collision_space::EnvironmentModel::AllowedCollisionMatrix& acm,
00195                                                   arm_navigation_msgs::AllowedCollisionMatrix& matrix) {
00196   if(!acm.getValid()) return;
00197   matrix.link_names.resize(acm.getSize());
00198   matrix.entries.resize(acm.getSize());
00199   const collision_space::EnvironmentModel::AllowedCollisionMatrix::entry_type& bm = acm.getEntriesBimap();
00200   for(collision_space::EnvironmentModel::AllowedCollisionMatrix::entry_type::right_const_iterator it = bm.right.begin();
00201       it != bm.right.end();
00202       it++) {
00203     matrix.link_names[it->first] = it->second;
00204     for(unsigned int i = 0; i < acm.getSize(); i++) {
00205       bool allowed;
00206       matrix.entries[i].enabled.resize(acm.getSize());
00207       acm.getAllowedCollision(it->first, i, allowed); 
00208       if(it->first >= matrix.entries[i].enabled.size()) {
00209         ROS_WARN_STREAM("Trouble size " << matrix.entries[i].enabled.size() << " ind " << it->first); 
00210       } else {
00211         matrix.entries[it->first].enabled[i] = allowed;
00212       }
00213     }
00214   }
00215 }
00216 
00217 collision_space::EnvironmentModel::AllowedCollisionMatrix 
00218 planning_environment::convertFromACMMsgToACM(const arm_navigation_msgs::AllowedCollisionMatrix& matrix)
00219 {
00220   std::map<std::string, unsigned int> indices;
00221   std::vector<std::vector<bool> > vecs;
00222   unsigned int ns = matrix.link_names.size();
00223   if(matrix.entries.size() != ns) {
00224     ROS_WARN_STREAM("Matrix messed up");
00225   }
00226   vecs.resize(ns);
00227   for(unsigned int i = 0; i < std::min(ns, (unsigned int) matrix.entries.size()); i++) {
00228     indices[matrix.link_names[i]] = i;
00229     if(matrix.entries[i].enabled.size() != ns) {
00230       ROS_WARN_STREAM("Matrix messed up");
00231     }
00232     vecs[i].resize(ns);
00233     for(unsigned int j = 0; j < std::min(ns, (unsigned int) matrix.entries[i].enabled.size()); j++) {
00234       vecs[i][j] = matrix.entries[i].enabled[j];
00235     }
00236   }
00237   collision_space::EnvironmentModel::AllowedCollisionMatrix acm(vecs,indices);
00238   return acm;
00239 }
00240 
00241 bool planning_environment::applyOrderedCollisionOperationsListToACM(const arm_navigation_msgs::OrderedCollisionOperations& ordered_coll,
00242                                                                     const std::vector<std::string>& object_names,
00243                                                                     const std::vector<std::string>& att_names,
00244                                                                     const planning_models::KinematicModel* model,
00245                                                                     collision_space::EnvironmentModel::AllowedCollisionMatrix& matrix)
00246 {
00247   bool all_ok = true;
00248   for(std::vector<arm_navigation_msgs::CollisionOperation>::const_iterator it = ordered_coll.collision_operations.begin();
00249       it != ordered_coll.collision_operations.end();
00250       it++) {
00251     bool op = (*it).operation != arm_navigation_msgs::CollisionOperation::ENABLE;
00252     std::vector<std::string> svec1, svec2;
00253     bool special1 = false;
00254     bool special2 = false;
00255     if((*it).object1 == (*it).COLLISION_SET_OBJECTS) {
00256       svec1 = object_names;
00257       special1 = true;
00258     }
00259     if((*it).object2 == (*it).COLLISION_SET_OBJECTS) {
00260       svec2 = object_names;
00261       special2 = true;
00262     }
00263     if((*it).object1 == (*it).COLLISION_SET_ATTACHED_OBJECTS) {
00264       svec1 = att_names;
00265       special1 = true;
00266     }
00267     if((*it).object2 == (*it).COLLISION_SET_ATTACHED_OBJECTS) {
00268       svec2 = att_names;
00269       special2 = true;
00270     }
00271     if(model->getModelGroup((*it).object1) != NULL) {
00272       svec1 = model->getModelGroup((*it).object1)->getGroupLinkNames();
00273       special1 = true;
00274     }
00275     if(model->getModelGroup((*it).object2) != NULL) {
00276       svec2 = model->getModelGroup((*it).object2)->getGroupLinkNames();
00277       special2 = true;
00278     }
00279     if(!special1) {
00280       svec1.push_back((*it).object1);
00281     }
00282     if(!special2) {
00283       svec2.push_back((*it).object2);
00284     }
00285 
00286     bool first_all = false;
00287     bool second_all = false;
00288     for(unsigned int j = 0; j < svec1.size(); j++) {
00289       if(svec1[j] == (*it).COLLISION_SET_ALL) {
00290         first_all = true;
00291         if(svec1.size() > 1) {
00292           ROS_WARN("Shouldn't have collision object all in multi-item list");
00293           all_ok = false;
00294         }
00295         break;
00296       }
00297     }
00298     for(unsigned int j = 0; j < svec2.size(); j++) {
00299       if(svec2[j] == (*it).COLLISION_SET_ALL) {
00300         second_all = true;
00301         if(svec2.size() > 1) {
00302           ROS_WARN("Shouldn't have collision object all in multi-item list");
00303           all_ok = false;
00304         }
00305         break;
00306       }
00307     }
00308 
00309     ROS_DEBUG_STREAM("Coll op first and second all " << first_all << " " << second_all);
00310     if(first_all && second_all) {
00311       matrix.changeEntry(op);
00312     } else if(first_all) {
00313       for(unsigned int j = 0; j < svec2.size(); j++) {
00314         matrix.changeEntry(svec2[j], op);
00315       }
00316     } else if(second_all) {
00317       for(unsigned int j = 0; j < svec1.size(); j++) {
00318         matrix.changeEntry(svec1[j], op);
00319       }
00320     } else {
00321       bool ok = matrix.changeEntry(svec1, svec2, (*it).operation != arm_navigation_msgs::CollisionOperation::ENABLE);
00322       if(!ok) {
00323         ROS_DEBUG_STREAM("No entry in acm for some member of " << (*it).object1 << " and " << (*it).object2);
00324         all_ok = false;
00325       }
00326     }
00327   }
00328   return all_ok;
00329 }               
00330 
00331 arm_navigation_msgs::AllowedCollisionMatrix 
00332 planning_environment::applyOrderedCollisionOperationsToCollisionsModel(const planning_environment::CollisionModels* cm,
00333                                                                        const arm_navigation_msgs::OrderedCollisionOperations& ordered_coll,
00334                                                                        const std::vector<std::string>& object_names,
00335                                                                        const std::vector<std::string>& att_names)
00336 {
00337   collision_space::EnvironmentModel::AllowedCollisionMatrix acm = cm->getDefaultAllowedCollisionMatrix();
00338   
00339   for(unsigned int i = 0; i < object_names.size(); i++) {
00340     if(!acm.hasEntry(object_names[i])) {
00341       acm.addEntry(object_names[i], false);
00342     } 
00343   }
00344 
00345   for(unsigned int i = 0; i < att_names.size(); i++) {
00346     if(!acm.hasEntry(att_names[i])) {
00347       acm.addEntry(att_names[i], false);
00348     } 
00349   }
00350 
00351   applyOrderedCollisionOperationsListToACM(ordered_coll, object_names, att_names, cm->getKinematicModel(), acm);
00352                          
00353   arm_navigation_msgs::AllowedCollisionMatrix ret_msg;
00354   convertFromACMToACMMsg(acm, ret_msg);
00355   return ret_msg;
00356 }
00357 
00358 bool planning_environment::doesKinematicStateObeyConstraints(const planning_models::KinematicState& state,
00359                                                              const arm_navigation_msgs::Constraints& constraints,
00360                                                              bool verbose) {
00361   planning_environment::KinematicConstraintEvaluatorSet constraint_evaluator;
00362   
00363   constraint_evaluator.add(constraints.joint_constraints);
00364   constraint_evaluator.add(constraints.position_constraints);
00365   constraint_evaluator.add(constraints.orientation_constraints);
00366   constraint_evaluator.add(constraints.visibility_constraints);
00367   return(constraint_evaluator.decide(&state, verbose));
00368 }
00369 
00370 void planning_environment::setMarkerShapeFromShape(const arm_navigation_msgs::Shape &obj, visualization_msgs::Marker &mk)
00371 {
00372   switch (obj.type)
00373   {
00374   case arm_navigation_msgs::Shape::SPHERE:
00375     mk.type = visualization_msgs::Marker::SPHERE;
00376     mk.scale.x = mk.scale.y = mk.scale.z = obj.dimensions[0] * 2.0;
00377     break;
00378     
00379   case arm_navigation_msgs::Shape::BOX:
00380     mk.type = visualization_msgs::Marker::CUBE;
00381     mk.scale.x = obj.dimensions[0];
00382     mk.scale.y = obj.dimensions[1];
00383     mk.scale.z = obj.dimensions[2];
00384     break;
00385     
00386   case arm_navigation_msgs::Shape::CYLINDER:
00387     mk.type = visualization_msgs::Marker::CYLINDER;
00388     mk.scale.x = obj.dimensions[0] * 2.0;
00389     mk.scale.y = obj.dimensions[0] * 2.0;
00390     mk.scale.z = obj.dimensions[1];
00391     break;
00392     
00393   case arm_navigation_msgs::Shape::MESH:
00394     mk.type = visualization_msgs::Marker::LINE_LIST;
00395     mk.scale.x = mk.scale.y = mk.scale.z = 0.001;
00396     {
00397       unsigned int nt = obj.triangles.size() / 3;
00398       for (unsigned int i = 0 ; i < nt ; ++i)
00399       {
00400         mk.points.push_back(obj.vertices[obj.triangles[3*i]]);
00401         mk.points.push_back(obj.vertices[obj.triangles[3*i+ 1]]);
00402         mk.points.push_back(obj.vertices[obj.triangles[3*i]]);
00403         mk.points.push_back(obj.vertices[obj.triangles[3*i+2]]);
00404         mk.points.push_back(obj.vertices[obj.triangles[3*i+1]]);
00405         mk.points.push_back(obj.vertices[obj.triangles[3*i+2]]);
00406       }
00407     }
00408     
00409     break;
00410     
00411   default:
00412     ROS_ERROR("Unknown object type: %d", (int)obj.type);
00413   }
00414 }
00415 
00416 void planning_environment::setMarkerShapeFromShape(const shapes::Shape *obj, visualization_msgs::Marker &mk, double padding)
00417 {
00418   switch (obj->type)
00419   {
00420   case shapes::SPHERE:
00421     mk.type = visualization_msgs::Marker::SPHERE;
00422     mk.scale.x = mk.scale.y = mk.scale.z = static_cast<const shapes::Sphere*>(obj)->radius * 2.0 + padding;
00423     break;
00424     
00425   case shapes::BOX:
00426     mk.type = visualization_msgs::Marker::CUBE;
00427     {
00428       const double *size = static_cast<const shapes::Box*>(obj)->size;
00429       mk.scale.x = size[0] + padding*2.0;
00430       mk.scale.y = size[1] + padding*2.0;
00431       mk.scale.z = size[2] + padding*2.0;
00432     }
00433     break;
00434     
00435   case shapes::CYLINDER:
00436     mk.type = visualization_msgs::Marker::CYLINDER;
00437     mk.scale.x = static_cast<const shapes::Cylinder*>(obj)->radius * 2.0 + padding;
00438     mk.scale.y = mk.scale.x;
00439     mk.scale.z = static_cast<const shapes::Cylinder*>(obj)->length + padding*2.0;
00440     break;
00441     
00442   case shapes::MESH:
00443     mk.type = visualization_msgs::Marker::LINE_LIST;
00444     mk.scale.x = mk.scale.y = mk.scale.z = 0.001;
00445     {      
00446       const shapes::Mesh *mesh = static_cast<const shapes::Mesh*>(obj);
00447       double* vertices = new double[mesh->vertexCount * 3];
00448       double sx = 0.0, sy = 0.0, sz = 0.0;
00449       for(unsigned int i = 0; i < mesh->vertexCount; ++i) {
00450         unsigned int i3 = i * 3;
00451         vertices[i3] = mesh->vertices[i3];
00452         vertices[i3 + 1] = mesh->vertices[i3 + 1];
00453         vertices[i3 + 2] = mesh->vertices[i3 + 2];
00454         sx += vertices[i3];
00455         sy += vertices[i3 + 1];
00456         sz += vertices[i3 + 2];
00457       }
00458       // the center of the mesh
00459       sx /= (double)mesh->vertexCount;
00460       sy /= (double)mesh->vertexCount;
00461       sz /= (double)mesh->vertexCount;
00462       
00463       for (unsigned int i = 0 ; i < mesh->vertexCount ; ++i)
00464       {
00465         unsigned int i3 = i * 3;
00466         
00467         // vector from center to the vertex
00468         double dx = vertices[i3] - sx;
00469         double dy = vertices[i3 + 1] - sy;
00470         double dz = vertices[i3 + 2] - sz;
00471         
00472         // length of vector
00473         //double norm = sqrt(dx * dx + dy * dy + dz * dz);
00474         
00475         double ndx = ((dx > 0) ? dx+padding : dx-padding);
00476         double ndy = ((dy > 0) ? dy+padding : dy-padding);
00477         double ndz = ((dz > 0) ? dz+padding : dz-padding);
00478         
00479         // the new distance of the vertex from the center
00480         //double fact = scale + padding/norm;
00481         vertices[i3] = sx + ndx; //dx * fact;
00482         vertices[i3 + 1] = sy + ndy; //dy * fact;
00483         vertices[i3 + 2] = sz + ndz; //dz * fact;                   
00484       }
00485       
00486       btTransform trans;
00487       tf::poseMsgToTF(mk.pose, trans);
00488 
00489       for (unsigned int j = 0 ; j < mesh->triangleCount; ++j) {
00490         unsigned int t1ind = mesh->triangles[3*j];
00491         unsigned int t2ind = mesh->triangles[3*j + 1];
00492         unsigned int t3ind = mesh->triangles[3*j + 2];
00493         
00494         btVector3 vec1(vertices[t1ind*3],
00495                        vertices[t1ind*3+1],
00496                        vertices[t1ind*3+2]);
00497         
00498         btVector3 vec2(vertices[t2ind*3],
00499                        vertices[t2ind*3+1],
00500                        vertices[t2ind*3+2]);
00501         
00502         btVector3 vec3(vertices[t3ind*3],
00503                        vertices[t3ind*3+1],
00504                        vertices[t3ind*3+2]);
00505         
00506         //vec1 = trans*vec1;
00507         //vec2 = trans*vec2;
00508         //vec3 = trans*vec3;
00509         
00510         geometry_msgs::Point pt1;
00511         pt1.x = vec1.x();
00512         pt1.y = vec1.y();
00513         pt1.z = vec1.z();
00514 
00515         geometry_msgs::Point pt2;
00516         pt2.x = vec2.x();
00517         pt2.y = vec2.y();
00518         pt2.z = vec2.z();
00519         
00520         geometry_msgs::Point pt3;
00521         pt3.x = vec3.x();
00522         pt3.y = vec3.y();
00523         pt3.z = vec3.z();
00524         
00525         mk.points.push_back(pt1);
00526         mk.points.push_back(pt2);
00527         
00528         mk.points.push_back(pt1);
00529         mk.points.push_back(pt3);
00530         
00531         mk.points.push_back(pt2);
00532         mk.points.push_back(pt3);
00533       }
00534       delete[] vertices;
00535     }
00536     break;
00537     
00538   default:
00539     ROS_ERROR("Unknown object type: %d", (int)obj->type);
00540   }
00541 }
00542 
00543 void planning_environment::convertFromLinkPaddingMapToLinkPaddingVector(const std::map<std::string, double>& link_padding_map,
00544                                                                         std::vector<arm_navigation_msgs::LinkPadding>& link_padding_vector)
00545 {
00546   for(std::map<std::string, double>::const_iterator it = link_padding_map.begin();
00547       it != link_padding_map.end();
00548       it++) {
00549     arm_navigation_msgs::LinkPadding lp;
00550     lp.link_name = it->first;
00551     lp.padding = it->second;
00552     link_padding_vector.push_back(lp);
00553   }
00554 }
00555 
00556 void planning_environment::getAllKinematicStateStampedTransforms(const planning_models::KinematicState& state,
00557                                                                  std::vector<geometry_msgs::TransformStamped>& trans_vector,
00558                                                                  const ros::Time& stamp)
00559 {
00560   trans_vector.clear();
00561   for(unsigned int i = 0; i < state.getLinkStateVector().size(); i++) {      
00562     const planning_models::KinematicState::LinkState* ls = state.getLinkStateVector()[i];
00563     geometry_msgs::TransformStamped ts;
00564     ts.header.stamp = stamp;
00565     ts.header.frame_id = state.getKinematicModel()->getRoot()->getParentFrameId();
00566     ts.child_frame_id = ls->getName();
00567     if(ts.header.frame_id == ts.child_frame_id) continue; 
00568     tf::transformTFToMsg(ls->getGlobalLinkTransform(),ts.transform);
00569     trans_vector.push_back(ts);
00570   }
00571 } 
00572 
00573 void planning_environment::convertAllowedContactSpecificationMsgToAllowedContactVector(const std::vector<arm_navigation_msgs::AllowedContactSpecification>& acmv,
00574                                                                                        std::vector<collision_space::EnvironmentModel::AllowedContact>& acv)
00575 {
00576   //assumes that poses are in the global frame
00577   acv.clear();
00578   for(unsigned int i = 0; i < acmv.size(); i++) {
00579     const arm_navigation_msgs::AllowedContactSpecification& acs = acmv[i];
00580     if(acs.link_names.size() != 2) {
00581       ROS_WARN_STREAM("Allowed collision specification has link_names size " << acs.link_names.size()
00582                       << " while the only supported size is 2");
00583       continue;
00584     }
00585     shapes::Shape* shape = constructObject(acs.shape);
00586     boost::shared_ptr<bodies::Body> bodysp(bodies::createBodyFromShape(shape));
00587     delete shape;
00588     btTransform trans;
00589     tf::poseMsgToTF(acs.pose_stamped.pose, trans);
00590     bodysp->setPose(trans);
00591 
00592     collision_space::EnvironmentModel::AllowedContact allc;
00593     allc.bound = bodysp;
00594     allc.body_name_1 = acs.link_names[0];
00595     allc.body_name_2 = acs.link_names[1];
00596     allc.depth = acs.penetration_depth;
00597     acv.push_back(allc);
00598   }
00599 }
model_utils.cpp
