chomp_motion_planner: chomp_collision

00001 /*********************************************************************
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2009, 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 CHOMP_COLLISION_SPACE_H_
00038 #define CHOMP_COLLISION_SPACE_H_
00039 
00040 #include <mapping_msgs/CollisionMap.h>
00041 #include <mapping_msgs/CollisionObject.h>
00042 #include <motion_planning_msgs/RobotState.h>
00043 
00044 #include <tf/message_filter.h>
00045 #include <message_filters/subscriber.h>
00046 
00047 #include <tf/transform_listener.h>
00048 #include <ros/ros.h>
00049 #include <boost/thread/mutex.hpp>
00050 #include <chomp_motion_planner/chomp_collision_point.h>
00051 #include <chomp_motion_planner/chomp_robot_model.h>
00052 #include <Eigen/Core>
00053 #include <distance_field/distance_field.h>
00054 #include <distance_field/propagation_distance_field.h>
00055 #include <distance_field/pf_distance_field.h>
00056 #include <planning_environment/models/collision_models.h>
00057 #include <planning_environment/monitors/collision_space_monitor.h>
00058 
00059 namespace chomp
00060 {
00061 
00062 class ChompCollisionSpace
00063 {
00064 
00065 // public:
00066 
00067 //   struct KnownObject {
00068 
00069 //     KnownObject(void) 
00070 //     {
00071 //     }
00072 
00073 //     ~KnownObject() {
00074 //     }
00075     
00076 //     void deleteBodies() {
00077 //       for(unsigned int i = 0; i < bodies_.size(); i++) {
00078 //         delete bodies_[i];
00079 //       }
00080 //       bodies_.clear();
00081 //     }
00082 
00083 //     std::vector<bodies::Body*> bodies_;
00084 //     std::vector<btVector3> voxels_;
00085 
00086 //   };
00087 
00088 public:
00089   ChompCollisionSpace();
00090   virtual ~ChompCollisionSpace();
00091 
00095   //void collisionMapCallback(const mapping_msgs::CollisionMapConstPtr& collision_map);
00096 
00097   //void collisionObjectCallback(const mapping_msgs::CollisionObjectConstPtr &collisionObject);
00098 
00104   bool init(planning_environment::CollisionSpaceMonitor* monitor_, double max_radius_clearance, std::string& reference_frame);
00105 
00109   void lock();
00110 
00114   void unlock();
00115 
00116   double getDistanceGradient(double x, double y, double z,
00117       double& gradient_x, double& gradient_y, double& gradient_z) const;
00118 
00119   void setStartState(const ChompRobotModel::ChompPlanningGroup& planning_group, const motion_planning_msgs::RobotState& robot_state);
00120 
00121   inline void worldToGrid(btVector3 origin, double wx, double wy, double wz, int &gx, int &gy, int &gz) const;
00122 
00123   inline void gridToWorld(btVector3 origin, int gx, int gy, int gz, double &wx, double &wy, double &wz) const;
00124 
00125   template<typename Derived, typename DerivedOther>
00126   bool getCollisionPointPotentialGradient(const ChompCollisionPoint& collision_point, const Eigen::MatrixBase<Derived>& collision_point_pos,
00127       double& potential, Eigen::MatrixBase<DerivedOther>& gradient) const;
00128 
00129 private:
00130 
00131   std::vector<btVector3> interpolateTriangle(btVector3 v0, 
00132                                              btVector3 v1, 
00133                                              btVector3 v2, double min_res);
00134  
00135   double dist(const btVector3 &v0, const btVector3 &v1)
00136   {
00137     return sqrt( (v1.x()-v0.x())*(v1.x()-v0.x()) + 
00138                  (v1.y()-v0.y())*(v1.y()-v0.y()) +  
00139                  (v1.z()-v0.z())*(v1.z()-v0.z()) );
00140   }
00141  
00142   ros::NodeHandle node_handle_, root_handle_;
00143   distance_field::PropagationDistanceField* distance_field_;
00144 
00145   //  tf::MessageNotifier<mapping_msgs::CollisionMap> *collision_map_notifier_;
00146   //message_filters::Subscriber<mapping_msgs::CollisionMap> collision_map_subscriber_;
00147   //tf::MessageFilter<mapping_msgs::CollisionMap> *collision_map_filter_;
00148   //tf::MessageNotifier<mapping_msgs::CollisionMap> *collision_map_update_notifier_;
00149   //message_filters::Subscriber<mapping_msgs::CollisionObject>         *collision_object_subscriber_;
00150   //tf::MessageFilter<mapping_msgs::CollisionObject>                   *collision_object_filter_;
00151 
00152   std::string reference_frame_;
00153   boost::mutex mutex_;
00154   std::vector<btVector3> cuboid_points_;
00155 
00156   double max_expansion_;
00157   double resolution_;
00158   double field_bias_x_;
00159   double field_bias_y_;
00160   double field_bias_z_;
00161 
00162   planning_environment::CollisionSpaceMonitor *monitor_;
00163   std::map<std::string, std::vector<std::string> > planning_group_link_names_;
00164   std::map<std::string, std::vector<bodies::Body *> > planning_group_bodies_;
00165 
00166   void initCollisionCuboids();
00167   void addCollisionCuboid(const std::string param_name);
00168 
00169   planning_environment::CollisionModels* collision_models_;
00170 
00171   void loadRobotBodies();
00172   void updateRobotBodiesPoses(const planning_models::KinematicState& state);
00173   void getVoxelsInBody(const bodies::Body &body, std::vector<btVector3> &voxels);
00174   void addCollisionObjectsToPoints(std::vector<btVector3>& points, const btTransform& cur);
00175   void addBodiesInGroupToPoints(const std::string& group, std::vector<btVector3> &voxels);
00176   void addAllBodiesButExcludeLinksToPoints(std::string group_name, std::vector<btVector3>& body_points);  
00177 
00178   std::map<std::string, std::vector<std::string> > distance_exclude_links_;
00179   std::map<std::string, std::vector<std::string> > distance_include_links_;
00180 
00181 };
00182 
00184 
00185 inline void ChompCollisionSpace::lock()
00186 {
00187   mutex_.lock();
00188 }
00189 
00190 inline void ChompCollisionSpace::unlock()
00191 {
00192   mutex_.unlock();
00193 }
00194 
00195 inline double ChompCollisionSpace::getDistanceGradient(double x, double y, double z,
00196     double& gradient_x, double& gradient_y, double& gradient_z) const
00197 {
00198   return distance_field_->getDistanceGradient(x, y, z, gradient_x, gradient_y, gradient_z);
00199 }
00200 
00201 template<typename Derived, typename DerivedOther>
00202 bool ChompCollisionSpace::getCollisionPointPotentialGradient(const ChompCollisionPoint& collision_point, const Eigen::MatrixBase<Derived>& collision_point_pos,
00203     double& potential, Eigen::MatrixBase<DerivedOther>& gradient) const
00204 {
00205   Eigen::Vector3d field_gradient;
00206   double field_distance = getDistanceGradient(
00207       collision_point_pos(0), collision_point_pos(1), collision_point_pos(2),
00208       field_gradient(0), field_gradient(1), field_gradient(2));
00209 
00210   field_gradient(0) += field_bias_x_;
00211   field_gradient(1) += field_bias_y_;
00212   field_gradient(2) += field_bias_z_;
00213 
00214   double d = field_distance - collision_point.getRadius();
00215 
00216   // three cases below:
00217   if (d >= collision_point.getClearance())
00218   {
00219     potential = 0.0;
00220     gradient.setZero();
00221   }
00222   else if (d >= 0.0)
00223   {
00224     double diff = (d - collision_point.getClearance());
00225     double gradient_magnitude = diff * collision_point.getInvClearance(); // (diff / clearance)
00226     potential = 0.5*gradient_magnitude*diff;
00227     gradient = gradient_magnitude * field_gradient;
00228   }
00229   else // if d < 0.0
00230   {
00231     gradient = field_gradient;
00232     potential = -d + 0.5 * collision_point.getClearance();
00233   }
00234 
00235   return (field_distance <= collision_point.getRadius()); // true if point is in collision
00236 }
00237 
00238 inline void ChompCollisionSpace::worldToGrid(btVector3 origin, double wx, double wy, double wz, int &gx, int &gy, int &gz) const {
00239   gx = (int)((wx - origin.x()) * (1.0 / resolution_));
00240   gy = (int)((wy - origin.y()) * (1.0 / resolution_));
00241   gz = (int)((wz - origin.z()) * (1.0 / resolution_));
00242 }
00243 
00245 inline void ChompCollisionSpace::gridToWorld(btVector3 origin, int gx, int gy, int gz, double &wx, double &wy, double &wz) const {
00246   wx = gx * resolution_ + origin.x();
00247   wy = gy * resolution_ + origin.y();
00248   wz = gz * resolution_ + origin.z();
00249 }
00250 
00251 } // namespace chomp
00252 
00253 #endif /* CHOMP_COLLISION_SPACE_H_ */
chomp_collision_space.h
