planning_environment: planning_environment: collision

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<btTransform>& 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<btTransform>& poses,
00143                        bool mask_before_insertion=true);
00144   
00145   void remaskCollisionMap();
00146 
00147   void maskAndDeleteShapeVector(std::vector<shapes::Shape*>& shapes,
00148                                 std::vector<btTransform>& 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<btTransform>& 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 btTransform& 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 btTransform& 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   void getPlanningSceneGivenState(const planning_models::KinematicState& state,
00234                                   arm_navigation_msgs::PlanningScene& scene);
00235 
00236   void getAllCollisionsForState(const planning_models::KinematicState& state,
00237                                 std::vector<arm_navigation_msgs::ContactInformation>& contacts,
00238                                 unsigned int num_per_pair = 1);
00239 
00240   bool isKinematicStateValid(const planning_models::KinematicState& state,
00241                              const std::vector<std::string>& names,
00242                              arm_navigation_msgs::ArmNavigationErrorCodes& error_code,
00243                              const arm_navigation_msgs::Constraints goal_constraints,
00244                              const arm_navigation_msgs::Constraints path_constraints,
00245                              bool verbose = false);
00246 
00247   bool isJointTrajectoryValid(const arm_navigation_msgs::PlanningScene& planning_scene,
00248                               const trajectory_msgs::JointTrajectory &trajectory,
00249                               const arm_navigation_msgs::Constraints& goal_constraints,
00250                               const arm_navigation_msgs::Constraints& path_constraints,
00251                               arm_navigation_msgs::ArmNavigationErrorCodes& error_code,
00252                               std::vector<arm_navigation_msgs::ArmNavigationErrorCodes>& trajectory_error_codes,
00253                               const bool evaluate_entire_trajectory);
00254 
00255   bool isJointTrajectoryValid(planning_models::KinematicState& state,
00256                               const trajectory_msgs::JointTrajectory &trajectory,
00257                               const arm_navigation_msgs::Constraints& goal_constraints,
00258                               const arm_navigation_msgs::Constraints& path_constraints,
00259                               arm_navigation_msgs::ArmNavigationErrorCodes& error_code,
00260                               std::vector<arm_navigation_msgs::ArmNavigationErrorCodes>& trajectory_error_codes,
00261                               const bool evaluate_entire_trajectory);  
00262 
00263   // bool isRobotTrajectoryValid(const arm_navigation_msgs::PlanningScene& planning_scene,
00264   //                             const arm_navigation_msgs::RobotTrajectory &trajectory,
00265   //                             const arm_navigation_msgs::Constraints& goal_constraints,
00266   //                             const arm_navigation_msgs::Constraints& path_constraints,
00267   //                             arm_navigation_msgs::ArmNavigationErrorCodes& error_code,
00268   //                             std::vector<arm_navigation_msgs::ArmNavigationErrorCodes>& trajectory_error_codes,
00269   //                             const bool evaluate_entire_trajectory);
00270 
00271   // bool isRobotTrajectoryValid(planning_models::KinematicState& state,
00272   //                             const arm_navigation_msgs::RobotTrajectory &trajectory,
00273   //                             const arm_navigation_msgs::Constraints& goal_constraints,
00274   //                             const arm_navigation_msgs::Constraints& path_constraints,
00275   //                             arm_navigation_msgs::ArmNavigationErrorCodes& error_code,
00276   //                             std::vector<arm_navigation_msgs::ArmNavigationErrorCodes>& trajectory_error_codes,
00277   //                             const bool evaluate_entire_trajectory);  
00278 
00279 
00280   double getTotalTrajectoryJointLength(planning_models::KinematicState& state,
00281                                        const trajectory_msgs::JointTrajectory &trajectory) const;                         
00282 
00283   //
00284   // Visualization functions
00285   //
00286 
00287   void getCollisionMapAsMarkers(visualization_msgs::MarkerArray& arr,
00288                                 const std_msgs::ColorRGBA& color,
00289                                 const ros::Duration& lifetime);
00290 
00291   void getAllCollisionSpaceObjectMarkers(const planning_models::KinematicState& state,
00292                                          visualization_msgs::MarkerArray& arr,
00293                                          const std::string& ns, 
00294                                          const std_msgs::ColorRGBA& static_color,
00295                                          const std_msgs::ColorRGBA& attached_color,
00296                                          const ros::Duration& lifetime);
00297 
00298   void getAttachedCollisionObjectMarkers(const planning_models::KinematicState& state,
00299                                          visualization_msgs::MarkerArray& arr,
00300                                          const std::string& ns, 
00301                                          const std_msgs::ColorRGBA& color,
00302                                          const ros::Duration& lifetime,
00303                                          const bool show_padded = false,
00304                                          const std::vector<std::string>* link_names = NULL) const;
00305   
00306   void getStaticCollisionObjectMarkers(visualization_msgs::MarkerArray& arr,
00307                                        const std::string& ns, 
00308                                        const std_msgs::ColorRGBA& color,
00309                                        const ros::Duration& lifetime) const;
00310   
00311   //can't be const because it poses in state
00312   void getAllCollisionPointMarkers(const planning_models::KinematicState& state,
00313                                    visualization_msgs::MarkerArray& arr,
00314                                    const std_msgs::ColorRGBA& color,
00315                                    const ros::Duration& lifetime);
00316 
00317 
00318   void getRobotMarkersGivenState(const planning_models::KinematicState& state,
00319                                  visualization_msgs::MarkerArray& arr,
00320                                  const std_msgs::ColorRGBA& color,
00321                                  const std::string& name, 
00322                                  const ros::Duration& lifetime,
00323                                  const std::vector<std::string>* names = NULL,
00324                                  const double scale=1.0,
00325                                  const bool show_collision_models = true) const;
00326 
00327   void getRobotPaddedMarkersGivenState(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) const;
00333 
00334   void getGroupAndUpdatedJointMarkersGivenState(const planning_models::KinematicState& state,
00335                                                 visualization_msgs::MarkerArray& arr,
00336                                                 const std::string& group_name, 
00337                                                 const std_msgs::ColorRGBA& group_color,
00338                                                 const std_msgs::ColorRGBA& updated_color,
00339                                                 const ros::Duration& lifetime) const;
00343 
00344   void writePlanningSceneBag(const std::string& filename,
00345                              const arm_navigation_msgs::PlanningScene& planning_scene) const;
00346   
00347   bool readPlanningSceneBag(const std::string& filename,
00348                             arm_navigation_msgs::PlanningScene& planning_scene) const;
00349 
00350   bool appendMotionPlanRequestToPlanningSceneBag(const std::string& filename,
00351                                                  const std::string& topic_name,
00352                                                  const arm_navigation_msgs::MotionPlanRequest& req);
00353 
00354   bool loadMotionPlanRequestsInPlanningSceneBag(const std::string& filename,
00355                                                 const std::string& topic_name,
00356                                                 std::vector<arm_navigation_msgs::MotionPlanRequest>& motion_plan_reqs);
00357 
00358   bool loadJointTrajectoriesInPlanningSceneBag(const std::string& filename,
00359                                                const std::string& topic_name,
00360                                                std::vector<trajectory_msgs::JointTrajectory>& traj_vec);
00361 
00362   
00363   bool appendJointTrajectoryToPlanningSceneBag(const std::string& filename,
00364                                                const std::string& topic_name,
00365                                                const trajectory_msgs::JointTrajectory& traj);
00366 
00370 
00372   const collision_space::EnvironmentModel* getCollisionSpace() const {
00373     return ode_collision_model_;
00374   }
00375 
00377   double getDefaultScale(void) const
00378   {
00379     return default_scale_;
00380   }
00381         
00383   double getDefaultPadding(void) const
00384   {
00385     return default_padd_;
00386   }
00387 
00388   double getDefaultObjectPadding(void) const
00389   {
00390     return object_padd_;
00391   }
00392 
00393   void getDefaultOrderedCollisionOperations(std::vector<arm_navigation_msgs::CollisionOperation> &self_collision) const
00394   {
00395     self_collision = default_collision_operations_;
00396   }
00397       
00398   const std::map<std::string,double>& getDefaultLinkPaddingMap() const {
00399     return ode_collision_model_->getDefaultLinkPaddingMap();
00400   }
00401 
00402   std::map<std::string,double> getCurrentLinkPaddingMap() const {
00403     return ode_collision_model_->getCurrentLinkPaddingMap();
00404   }
00405   
00406   bool isPlanningSceneSet() const {
00407     return planning_scene_set_;
00408   }
00409 
00410   const std::map<std::string, geometry_msgs::TransformStamped>& getSceneTransformMap() const {
00411     return scene_transform_map_;
00412   }
00413 
00414   const std::vector<shapes::Shape*>& getCollisionMapShapes() const {
00415     return collision_map_shapes_;
00416   }
00417 
00418   const std::vector<btTransform>& getCollisionMapPoses() const {
00419     return collision_map_poses_;
00420   }
00421 
00422   void getCollisionObjectNames(std::vector<std::string>& o_strings) const {
00423     o_strings.clear();
00424     bodiesLock();
00425     for(std::map<std::string, bodies::BodyVector*>::const_iterator it = static_object_map_.begin();
00426         it != static_object_map_.end();
00427         it++) {
00428       o_strings.push_back(it->first);
00429     }
00430     o_strings.push_back(COLLISION_MAP_NAME);
00431     bodiesUnlock();
00432   }
00433 
00434   void getAttachedCollisionObjectNames(std::vector<std::string>& a_strings) const {
00435     a_strings.clear();
00436     bodiesLock();
00437     for(std::map<std::string, std::map<std::string, bodies::BodyVector*> >::const_iterator it = link_attached_objects_.begin();
00438         it != link_attached_objects_.end();
00439         it++) {
00440       for(std::map<std::string, bodies::BodyVector*>::const_iterator it2 = it->second.begin();
00441           it2 != it->second.end();
00442           it2++) {
00443         a_strings.push_back(it2->first);
00444       }    
00445     }
00446     bodiesUnlock();
00447   }
00448 
00449   void bodiesLock() const {
00450     bodies_lock_.lock();
00451   }
00452 
00453   void bodiesUnlock() const {
00454     bodies_lock_.unlock();
00455   }
00456 
00457 protected:
00458 
00459   mutable boost::recursive_mutex bodies_lock_;
00460 
00461   std::vector<shapes::Shape*> collision_map_shapes_;
00462   std::vector<btTransform> collision_map_poses_;
00463 
00464   std::map<std::string, bodies::BodyVector*> static_object_map_;
00465 
00466   std::map<std::string, std::map<std::string, bodies::BodyVector*> > link_attached_objects_;
00467         
00468   void loadCollisionFromParamServer();
00469   void setupModelFromParamServer(collision_space::EnvironmentModel* model);
00470         
00471   collision_space::EnvironmentModel* ode_collision_model_;
00472 
00473   bool planning_scene_set_;
00474 
00475   double default_scale_;
00476   double default_padd_;
00477   double object_padd_;
00478   double attached_padd_;
00479   std::vector<double> bounding_planes_;
00480 
00481   std::vector<arm_navigation_msgs::CollisionOperation> default_collision_operations_;
00482 
00483   std::map<std::string, geometry_msgs::TransformStamped> scene_transform_map_;
00484 
00485 };
00486     
00487         
00488 }
00489 
00490 #endif
00491
collision_models.h
