collision_robot.h
Go to the documentation of this file.
00001 /*********************************************************************
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2011, 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 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 
00035 /* Author: Ioan Sucan */
00036 
00037 #ifndef MOVEIT_COLLISION_DETECTION_COLLISION_ROBOT_
00038 #define MOVEIT_COLLISION_DETECTION_COLLISION_ROBOT_
00039 
00040 #include <moveit/collision_detection/collision_matrix.h>
00041 #include <moveit/robot_state/robot_state.h>
00042 #include <moveit_msgs/LinkPadding.h>
00043 #include <moveit_msgs/LinkScale.h>
00044 
00045 namespace collision_detection
00046 {
00047 
00051   class CollisionRobot
00052   {
00053   public:
00054 
00060     CollisionRobot(const robot_model::RobotModelConstPtr &model, double padding = 0.0, double scale = 1.0);
00061 
00063     CollisionRobot(const CollisionRobot &other);
00064 
00065     virtual ~CollisionRobot()
00066     {
00067     }
00068 
00074     virtual void checkSelfCollision(const CollisionRequest &req, CollisionResult &res, const robot_state::RobotState &state) const = 0;
00075 
00082     virtual void checkSelfCollision(const CollisionRequest &req, CollisionResult &res, const robot_state::RobotState &state, const AllowedCollisionMatrix &acm) const = 0;
00083 
00090     virtual void checkSelfCollision(const CollisionRequest &req, CollisionResult &res, const robot_state::RobotState &state1, const robot_state::RobotState &state2) const = 0;
00091 
00099     virtual void checkSelfCollision(const CollisionRequest &req, CollisionResult &res, const robot_state::RobotState &state1, const robot_state::RobotState &state2, const AllowedCollisionMatrix &acm) const = 0;
00100 
00108     virtual void checkOtherCollision(const CollisionRequest &req, CollisionResult &res,
00109                                      const robot_state::RobotState &state,
00110                                      const CollisionRobot &other_robot,
00111                                      const robot_state::RobotState &other_state) const = 0;
00112 
00121     virtual void checkOtherCollision(const CollisionRequest &req, CollisionResult &res,
00122                                      const robot_state::RobotState &state,
00123                                      const CollisionRobot &other_robot,
00124                                      const robot_state::RobotState &other_state,
00125                                      const AllowedCollisionMatrix &acm) const = 0;
00126 
00136     virtual void checkOtherCollision(const CollisionRequest &req, CollisionResult &res,
00137                                      const robot_state::RobotState &state1,
00138                                      const robot_state::RobotState &state2,
00139                                      const CollisionRobot &other_robot,
00140                                      const robot_state::RobotState &other_state1,
00141                                      const robot_state::RobotState &other_state2) const = 0;
00142 
00153     virtual void checkOtherCollision(const CollisionRequest &req, CollisionResult &res,
00154                                      const robot_state::RobotState &state1,
00155                                      const robot_state::RobotState &state2,
00156                                      const CollisionRobot &other_robot,
00157                                      const robot_state::RobotState &other_state1,
00158                                      const robot_state::RobotState &other_state2,
00159                                      const AllowedCollisionMatrix &acm) const = 0;
00160 
00162     virtual double distanceSelf(const robot_state::RobotState &state) const = 0;
00163 
00166     virtual double distanceSelf(const robot_state::RobotState &state,
00167                                 const AllowedCollisionMatrix &acm) const = 0;
00168 
00173     virtual double distanceOther(const robot_state::RobotState &state,
00174                                  const CollisionRobot &other_robot,
00175                                  const robot_state::RobotState &other_state) const = 0;
00176 
00182     virtual double distanceOther(const robot_state::RobotState &state,
00183                                  const CollisionRobot &other_robot,
00184                                  const robot_state::RobotState &other_state,
00185                                  const AllowedCollisionMatrix &acm) const = 0;
00186 
00188     const robot_model::RobotModelConstPtr& getRobotModel() const
00189     {
00190       return robot_model_;
00191     }
00192 
00197     void setLinkPadding(const std::string &link_name, double padding);
00198 
00200     double getLinkPadding(const std::string &link_name) const;
00201 
00203     void setLinkPadding(const std::map<std::string, double> &padding);
00204 
00206     const std::map<std::string, double> &getLinkPadding() const;
00207 
00209     void setLinkScale(const std::string &link_name, double scale);
00210 
00212     double getLinkScale(const std::string &link_name) const;
00213 
00215     void setLinkScale(const std::map<std::string, double> &scale);
00216 
00218     const std::map<std::string, double> &getLinkScale() const;
00219 
00221     void setPadding(double padding);
00222 
00224     void setScale(double scale);
00225 
00227     void setPadding(const std::vector<moveit_msgs::LinkPadding> &padding);
00228 
00230     void getPadding(std::vector<moveit_msgs::LinkPadding> &padding) const;
00231 
00233     void setScale(const std::vector<moveit_msgs::LinkScale> &scale);
00234 
00236     void getScale(std::vector<moveit_msgs::LinkScale> &scale) const;
00237 
00238   protected:
00239 
00244     virtual void updatedPaddingOrScaling(const std::vector<std::string> &links);
00245 
00247     robot_model::RobotModelConstPtr         robot_model_;
00248 
00250     std::map<std::string, double>           link_padding_;
00251 
00253     std::map<std::string, double>           link_scale_;
00254   };
00255 
00256   typedef boost::shared_ptr<CollisionRobot> CollisionRobotPtr;
00257   typedef boost::shared_ptr<const CollisionRobot> CollisionRobotConstPtr;
00258 }
00259 
00260 #endif


moveit_core
Author(s): Ioan Sucan , Sachin Chitta , Acorn Pooley
autogenerated on Thu Aug 27 2015 13:58:52