collision_models.h
Go to the documentation of this file.
00001 /*********************************************************************
00002 * Software License Agreement (BSD License)
00003 * 
00004 *  Copyright (c) 2008, 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 #ifndef PLANNING_ENVIRONMENT_MODELS_COLLISION_MODELS_
00038 #define PLANNING_ENVIRONMENT_MODELS_COLLISION_MODELS_
00039 
00040 #include "planning_environment/models/robot_models.h"
00041 #include <tf/tf.h>
00042 #include <collision_space/environmentODE.h>
00043 #include <arm_navigation_msgs/PlanningScene.h>
00044 #include <arm_navigation_msgs/Shape.h>
00045 #include <geometric_shapes/bodies.h>
00046 #include <trajectory_msgs/JointTrajectory.h>
00047 #include <arm_navigation_msgs/Constraints.h>
00048 #include <arm_navigation_msgs/ArmNavigationErrorCodes.h>
00049 #include <arm_navigation_msgs/MotionPlanRequest.h>
00050 #include <arm_navigation_msgs/ContactInformation.h>
00051 #include <visualization_msgs/Marker.h>
00052 #include <visualization_msgs/MarkerArray.h>
00053 #include <arm_navigation_msgs/OrderedCollisionOperations.h>
00054 
00055 static const std::string COLLISION_MAP_NAME="collision_map";
00056 
00057 namespace planning_environment
00058 {
00059 
00062 class CollisionModels : public RobotModels
00063 {
00064 public:
00065 
00066   //
00067   // Constructors
00068   //
00069         
00070   CollisionModels(const std::string &description);
00071 
00072   CollisionModels(boost::shared_ptr<urdf::Model> urdf,
00073                   planning_models::KinematicModel* kmodel,
00074                   collision_space::EnvironmentModel* ode_collision_model_);
00075 
00076   virtual ~CollisionModels(void);
00077 
00078   //
00079   // Planning scene functions
00080   //
00081   planning_models::KinematicState* setPlanningScene(const arm_navigation_msgs::PlanningScene& planning_scene);
00082 
00083   void revertPlanningScene(planning_models::KinematicState* state);
00084 
00085   // 
00086   // Planning scene and state based transform functions
00087   //
00088   bool convertAttachedCollisionObjectToNewWorldFrame(const planning_models::KinematicState& state,
00089                                                      arm_navigation_msgs::AttachedCollisionObject& att_obj) const;
00090   
00091   bool convertCollisionObjectToNewWorldFrame(const planning_models::KinematicState& state,
00092                                              arm_navigation_msgs::CollisionObject& obj) const;
00093   
00094   bool convertConstraintsGivenNewWorldTransform(const planning_models::KinematicState& state,
00095                                                 arm_navigation_msgs::Constraints& constraints,
00096                                                 const std::string& opt_frame="") const;
00097   
00098   bool convertPoseGivenWorldTransform(const planning_models::KinematicState& state,
00099                                       const std::string& des_frame_id,
00100                                       const std_msgs::Header& header,
00101                                       const geometry_msgs::Pose& pose,
00102                                       geometry_msgs::PoseStamped& ret_pose) const;
00103 
00104   bool convertPointGivenWorldTransform(const planning_models::KinematicState& state,
00105                                        const std::string& des_frame_id,
00106                                        const std_msgs::Header& header,
00107                                        const geometry_msgs::Point& point,
00108                                        geometry_msgs::PointStamped& ret_point) const;
00109   
00110   bool convertQuaternionGivenWorldTransform(const planning_models::KinematicState& state,
00111                                             const std::string& des_frame_id,
00112                                             const std_msgs::Header& header,
00113                                             const geometry_msgs::Quaternion& quat,
00114                                             geometry_msgs::QuaternionStamped& ret_quat) const;
00115 
00116   //
00117   // Functions for updating the position of attached objects
00118   //
00119 
00120   bool updateAttachedBodyPosesForLink(const planning_models::KinematicState& state,
00121                                       const std::string& link_name);
00122 
00123   bool updateAttachedBodyPoses(const planning_models::KinematicState& state);
00124 
00125   //this function will fail if the header is not in the world frame
00126   bool addStaticObject(const arm_navigation_msgs::CollisionObject& obj);
00127 
00128   void addStaticObject(const std::string& name,
00129                        std::vector<shapes::Shape*>& shapes,
00130                        const std::vector<tf::Transform>& poses,
00131                        double padding);
00132 
00133   void deleteStaticObject(const std::string& name);
00134   
00135   void deleteAllStaticObjects();
00136 
00137   //this function will fail if the header is not in the world frame
00138   void setCollisionMap(const arm_navigation_msgs::CollisionMap& map,
00139                        bool mask_before_insertion=true);
00140 
00141   void setCollisionMap(std::vector<shapes::Shape*>& shapes,
00142                        const std::vector<tf::Transform>& poses,
00143                        bool mask_before_insertion=true);
00144   
00145   void remaskCollisionMap();
00146 
00147   void maskAndDeleteShapeVector(std::vector<shapes::Shape*>& shapes,
00148                                 std::vector<tf::Transform>& poses);
00149   
00150   //this function will fail if the header is not in the world frame
00151   bool addAttachedObject(const arm_navigation_msgs::AttachedCollisionObject& att);
00152 
00153   //fails if the link_name is not a valid link
00154   bool addAttachedObject(const std::string& object_name,
00155                          const std::string& link_name,
00156                          std::vector<shapes::Shape*>& shapes,
00157                          const std::vector<tf::Transform>& poses,
00158                          const std::vector<std::string>& touch_links,
00159                          double padding);
00160 
00161   bool deleteAttachedObject(const std::string& object_id,
00162                             const std::string& link_name);
00163 
00164   void deleteAllAttachedObjects(const std::string& link_name="");
00165 
00166   bool convertStaticObjectToAttachedObject(const std::string& object_name,
00167                                            const std::string& link_name,
00168                                            const tf::Transform& link_pose,
00169                                            const std::vector<std::string>& touch_links);
00170   
00171   bool convertAttachedObjectToStaticObject(const std::string& object_name,
00172                                            const std::string& link_name,
00173                                            const tf::Transform& link_pose);
00174 
00175   const std::map<std::string, std::map<std::string, bodies::BodyVector*> >& getLinkAttachedObjects() const
00176   {
00177     return link_attached_objects_;
00178   }
00179 
00180   //
00181   // Handling collision space functions
00182   //
00183 
00184   void applyLinkPaddingToCollisionSpace(const std::vector<arm_navigation_msgs::LinkPadding>& link_padding);
00185 
00186   void getCurrentLinkPadding(std::vector<arm_navigation_msgs::LinkPadding>& link_padding);
00187 
00188   void revertCollisionSpacePaddingToDefault();
00189 
00190   void revertAllowedCollisionToDefault();
00191 
00192   bool applyOrderedCollisionOperationsToCollisionSpace(const arm_navigation_msgs::OrderedCollisionOperations &ord,
00193                                                        bool print=false);
00194   bool disableCollisionsForNonUpdatedLinks(const std::string& group_name,
00195                                            bool use_default=false);
00196 
00197   bool setAlteredAllowedCollisionMatrix(const collision_space::EnvironmentModel::AllowedCollisionMatrix& acm);
00198 
00199   void clearAllowedContacts() {
00200     ode_collision_model_->lock();
00201     ode_collision_model_->clearAllowedContacts();
00202     ode_collision_model_->unlock();
00203   }
00204 
00205   //
00206   // Collision space accessors
00207   //
00208 
00209   const collision_space::EnvironmentModel::AllowedCollisionMatrix& getCurrentAllowedCollisionMatrix() const;
00210 
00211   const collision_space::EnvironmentModel::AllowedCollisionMatrix& getDefaultAllowedCollisionMatrix() const;
00212 
00213   void getCollisionSpaceCollisionMap(arm_navigation_msgs::CollisionMap& cmap) const;
00214 
00215   void getLastCollisionMap(arm_navigation_msgs::CollisionMap& cmap) const;
00216   
00217   void getCollisionSpaceAllowedCollisions(arm_navigation_msgs::AllowedCollisionMatrix& matrix) const;
00218 
00219   void getCollisionSpaceCollisionObjects(std::vector<arm_navigation_msgs::CollisionObject>& omap) const;
00220 
00221   void getCollisionSpaceAttachedCollisionObjects(std::vector<arm_navigation_msgs::AttachedCollisionObject>& avec) const;
00222 
00223   //
00224   // Functions for checking collisions and validity
00225   //
00226   
00227   bool isKinematicStateInCollision(const planning_models::KinematicState& state);
00228 
00229   bool isKinematicStateInSelfCollision(const planning_models::KinematicState& state);
00230 
00231   bool isKinematicStateInEnvironmentCollision(const planning_models::KinematicState& state);
00232 
00233   bool isKinematicStateInObjectCollision(const planning_models::KinematicState &state, 
00234                                          const std::string& object_name);
00235 
00236   bool isObjectInCollision(const std::string& object_name);
00237 
00238   void getPlanningSceneGivenState(const planning_models::KinematicState& state,
00239                                   arm_navigation_msgs::PlanningScene& scene);
00240 
00241   void getAllCollisionsForState(const planning_models::KinematicState& state,
00242                                 std::vector<arm_navigation_msgs::ContactInformation>& contacts,
00243                                 unsigned int num_per_pair = 1);
00244 
00245   void getAllEnvironmentCollisionsForObject(const std::string& object_name, 
00246                                             std::vector<arm_navigation_msgs::ContactInformation>& contacts,
00247                                             unsigned int num_per_pair = 1);
00248 
00249   bool isKinematicStateValid(const planning_models::KinematicState& state,
00250                              const std::vector<std::string>& names,
00251                              arm_navigation_msgs::ArmNavigationErrorCodes& error_code,
00252                              const arm_navigation_msgs::Constraints goal_constraints,
00253                              const arm_navigation_msgs::Constraints path_constraints,
00254                              bool verbose = false);
00255 
00256   bool isJointTrajectoryValid(const arm_navigation_msgs::PlanningScene& planning_scene,
00257                               const trajectory_msgs::JointTrajectory &trajectory,
00258                               const arm_navigation_msgs::Constraints& goal_constraints,
00259                               const arm_navigation_msgs::Constraints& path_constraints,
00260                               arm_navigation_msgs::ArmNavigationErrorCodes& error_code,
00261                               std::vector<arm_navigation_msgs::ArmNavigationErrorCodes>& trajectory_error_codes,
00262                               const bool evaluate_entire_trajectory);
00263 
00264   bool isJointTrajectoryValid(planning_models::KinematicState& state,
00265                               const trajectory_msgs::JointTrajectory &trajectory,
00266                               const arm_navigation_msgs::Constraints& goal_constraints,
00267                               const arm_navigation_msgs::Constraints& path_constraints,
00268                               arm_navigation_msgs::ArmNavigationErrorCodes& error_code,
00269                               std::vector<arm_navigation_msgs::ArmNavigationErrorCodes>& trajectory_error_codes,
00270                               const bool evaluate_entire_trajectory);  
00271 
00272   // bool isRobotTrajectoryValid(const arm_navigation_msgs::PlanningScene& planning_scene,
00273   //                             const arm_navigation_msgs::RobotTrajectory &trajectory,
00274   //                             const arm_navigation_msgs::Constraints& goal_constraints,
00275   //                             const arm_navigation_msgs::Constraints& path_constraints,
00276   //                             arm_navigation_msgs::ArmNavigationErrorCodes& error_code,
00277   //                             std::vector<arm_navigation_msgs::ArmNavigationErrorCodes>& trajectory_error_codes,
00278   //                             const bool evaluate_entire_trajectory);
00279 
00280   // bool isRobotTrajectoryValid(planning_models::KinematicState& state,
00281   //                             const arm_navigation_msgs::RobotTrajectory &trajectory,
00282   //                             const arm_navigation_msgs::Constraints& goal_constraints,
00283   //                             const arm_navigation_msgs::Constraints& path_constraints,
00284   //                             arm_navigation_msgs::ArmNavigationErrorCodes& error_code,
00285   //                             std::vector<arm_navigation_msgs::ArmNavigationErrorCodes>& trajectory_error_codes,
00286   //                             const bool evaluate_entire_trajectory);  
00287 
00288 
00289   double getTotalTrajectoryJointLength(planning_models::KinematicState& state,
00290                                        const trajectory_msgs::JointTrajectory &trajectory) const;                         
00291 
00292   //
00293   // Visualization functions
00294   //
00295 
00296   void getCollisionMapAsMarkers(visualization_msgs::MarkerArray& arr,
00297                                 const std_msgs::ColorRGBA& color,
00298                                 const ros::Duration& lifetime);
00299 
00300   void getAllCollisionSpaceObjectMarkers(const planning_models::KinematicState& state,
00301                                          visualization_msgs::MarkerArray& arr,
00302                                          const std::string& ns, 
00303                                          const std_msgs::ColorRGBA& static_color,
00304                                          const std_msgs::ColorRGBA& attached_color,
00305                                          const ros::Duration& lifetime);
00306 
00307   void getAttachedCollisionObjectMarkers(const planning_models::KinematicState& state,
00308                                          visualization_msgs::MarkerArray& arr,
00309                                          const std::string& ns, 
00310                                          const std_msgs::ColorRGBA& color,
00311                                          const ros::Duration& lifetime,
00312                                          const bool show_padded = false,
00313                                          const std::vector<std::string>* link_names = NULL) const;
00314   
00315   void getStaticCollisionObjectMarkers(visualization_msgs::MarkerArray& arr,
00316                                        const std::string& ns, 
00317                                        const std_msgs::ColorRGBA& color,
00318                                        const ros::Duration& lifetime) const;
00319   
00320   //can't be const because it poses in state
00321   void getAllCollisionPointMarkers(const planning_models::KinematicState& state,
00322                                    visualization_msgs::MarkerArray& arr,
00323                                    const std_msgs::ColorRGBA& color,
00324                                    const ros::Duration& lifetime);
00325 
00326 
00327   void getRobotMarkersGivenState(const planning_models::KinematicState& state,
00328                                  visualization_msgs::MarkerArray& arr,
00329                                  const std_msgs::ColorRGBA& color,
00330                                  const std::string& name, 
00331                                  const ros::Duration& lifetime,
00332                                  const std::vector<std::string>* names = NULL,
00333                                  const double scale=1.0,
00334                                  const bool show_collision_models = true) const;
00335 
00336   void getRobotPaddedMarkersGivenState(const planning_models::KinematicState& state,
00337                                        visualization_msgs::MarkerArray& arr,
00338                                        const std_msgs::ColorRGBA& color,
00339                                        const std::string& name, 
00340                                        const ros::Duration& lifetime,
00341                                        const std::vector<std::string>* names = NULL) const;
00342 
00343   void getGroupAndUpdatedJointMarkersGivenState(const planning_models::KinematicState& state,
00344                                                 visualization_msgs::MarkerArray& arr,
00345                                                 const std::string& group_name, 
00346                                                 const std_msgs::ColorRGBA& group_color,
00347                                                 const std_msgs::ColorRGBA& updated_color,
00348                                                 const ros::Duration& lifetime) const;
00352 
00353   void writePlanningSceneBag(const std::string& filename,
00354                              const arm_navigation_msgs::PlanningScene& planning_scene) const;
00355   
00356   bool readPlanningSceneBag(const std::string& filename,
00357                             arm_navigation_msgs::PlanningScene& planning_scene) const;
00358 
00359   bool appendMotionPlanRequestToPlanningSceneBag(const std::string& filename,
00360                                                  const std::string& topic_name,
00361                                                  const arm_navigation_msgs::MotionPlanRequest& req);
00362 
00363   bool loadMotionPlanRequestsInPlanningSceneBag(const std::string& filename,
00364                                                 const std::string& topic_name,
00365                                                 std::vector<arm_navigation_msgs::MotionPlanRequest>& motion_plan_reqs);
00366 
00367   bool loadJointTrajectoriesInPlanningSceneBag(const std::string& filename,
00368                                                const std::string& topic_name,
00369                                                std::vector<trajectory_msgs::JointTrajectory>& traj_vec);
00370 
00371   
00372   bool appendJointTrajectoryToPlanningSceneBag(const std::string& filename,
00373                                                const std::string& topic_name,
00374                                                const trajectory_msgs::JointTrajectory& traj);
00375 
00379 
00381   const collision_space::EnvironmentModel* getCollisionSpace() const {
00382     return ode_collision_model_;
00383   }
00384 
00386   double getDefaultScale(void) const
00387   {
00388     return default_scale_;
00389   }
00390         
00392   double getDefaultPadding(void) const
00393   {
00394     return default_padd_;
00395   }
00396 
00397   double getDefaultObjectPadding(void) const
00398   {
00399     return object_padd_;
00400   }
00401 
00402   void getDefaultOrderedCollisionOperations(std::vector<arm_navigation_msgs::CollisionOperation> &self_collision) const
00403   {
00404     self_collision = default_collision_operations_;
00405   }
00406       
00407   const std::map<std::string,double>& getDefaultLinkPaddingMap() const {
00408     return ode_collision_model_->getDefaultLinkPaddingMap();
00409   }
00410 
00411   std::map<std::string,double> getCurrentLinkPaddingMap() const {
00412     return ode_collision_model_->getCurrentLinkPaddingMap();
00413   }
00414   
00415   bool isPlanningSceneSet() const {
00416     return planning_scene_set_;
00417   }
00418 
00419   const std::map<std::string, geometry_msgs::TransformStamped>& getSceneTransformMap() const {
00420     return scene_transform_map_;
00421   }
00422 
00423   const std::vector<shapes::Shape*>& getCollisionMapShapes() const {
00424     return collision_map_shapes_;
00425   }
00426 
00427   const std::vector<tf::Transform>& getCollisionMapPoses() const {
00428     return collision_map_poses_;
00429   }
00430 
00431   void getCollisionObjectNames(std::vector<std::string>& o_strings) const {
00432     o_strings.clear();
00433     bodiesLock();
00434     for(std::map<std::string, bodies::BodyVector*>::const_iterator it = static_object_map_.begin();
00435         it != static_object_map_.end();
00436         it++) {
00437       o_strings.push_back(it->first);
00438     }
00439     o_strings.push_back(COLLISION_MAP_NAME);
00440     bodiesUnlock();
00441   }
00442 
00443   void getAttachedCollisionObjectNames(std::vector<std::string>& a_strings) const {
00444     a_strings.clear();
00445     bodiesLock();
00446     for(std::map<std::string, std::map<std::string, bodies::BodyVector*> >::const_iterator it = link_attached_objects_.begin();
00447         it != link_attached_objects_.end();
00448         it++) {
00449       for(std::map<std::string, bodies::BodyVector*>::const_iterator it2 = it->second.begin();
00450           it2 != it->second.end();
00451           it2++) {
00452         a_strings.push_back(it2->first);
00453       }    
00454     }
00455     bodiesUnlock();
00456   }
00457 
00458   void bodiesLock() const {
00459     bodies_lock_.lock();
00460   }
00461 
00462   void bodiesUnlock() const {
00463     bodies_lock_.unlock();
00464   }
00465 
00466 protected:
00467 
00468   mutable boost::recursive_mutex bodies_lock_;
00469 
00470   std::vector<shapes::Shape*> collision_map_shapes_;
00471   std::vector<tf::Transform> collision_map_poses_;
00472 
00473   std::map<std::string, bodies::BodyVector*> static_object_map_;
00474 
00475   std::map<std::string, std::map<std::string, bodies::BodyVector*> > link_attached_objects_;
00476         
00477   void loadCollisionFromParamServer();
00478   void setupModelFromParamServer(collision_space::EnvironmentModel* model);
00479         
00480   collision_space::EnvironmentModel* ode_collision_model_;
00481 
00482   bool planning_scene_set_;
00483 
00484   double default_scale_;
00485   double default_padd_;
00486   double object_padd_;
00487   double attached_padd_;
00488   std::vector<double> bounding_planes_;
00489 
00490   std::vector<arm_navigation_msgs::CollisionOperation> default_collision_operations_;
00491 
00492   std::map<std::string, geometry_msgs::TransformStamped> scene_transform_map_;
00493 
00494 };
00495     
00496         
00497 }
00498 
00499 #endif
00500 


planning_environment
Author(s): Ioan Sucan
autogenerated on Mon Dec 2 2013 12:34:43