environment_chain3d.h
Go to the documentation of this file.
00001 /*********************************************************************
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2008, Maxim Likhachev
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 Maxim Likhachev 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 
00038 #ifndef _ENVIRONMENT_CHAIN3D_H_
00039 #define _ENVIRONMENT_CHAIN3D_H_
00040 
00041 #include <time.h>
00042 #include <stdio.h>
00043 #include <sys/types.h>
00044 #include <sys/stat.h>
00045 #include <unistd.h>
00046 #include <fstream>
00047 #include <vector>
00048 #include <string>
00049 #include <list>
00050 #include <algorithm>
00051 
00052 #include <sbpl/headers.h>
00053 #include <sbpl_interface/bfs3d/BFS_3D.h>
00054 #include <planning_scene/planning_scene.h>
00055 #include <collision_distance_field/collision_robot_hybrid.h>
00056 #include <collision_distance_field/collision_world_hybrid.h>
00057 #include <sbpl_interface/environment_chain3d_types.h>
00058 #include <moveit_msgs/GetMotionPlan.h>
00059 
00060 #include <Eigen/Core>
00061 
00062 static const double DEFAULT_INTERPOLATION_DISTANCE=.05;
00063 static const double DEFAULT_JOINT_MOTION_PRIMITIVE_DISTANCE=.2;
00064 
00065 namespace sbpl_interface {
00066 
00067 struct PlanningStatistics {
00068 
00069   PlanningStatistics() :
00070     total_expansions_(0),
00071     coll_checks_(0)
00072   {
00073   }
00074 
00075   unsigned int total_expansions_;
00076   ros::WallDuration total_expansion_time_;
00077   ros::WallDuration total_coll_check_time_;
00078   unsigned int coll_checks_;
00079   ros::WallDuration total_planning_time_;
00080 };
00081 
00082 struct PlanningParameters {
00083 
00084   PlanningParameters() :
00085     use_bfs_(true),
00086     use_standard_collision_checking_(false),
00087     attempt_full_shortcut_(true),
00088     interpolation_distance_(DEFAULT_INTERPOLATION_DISTANCE),
00089     joint_motion_primitive_distance_(DEFAULT_JOINT_MOTION_PRIMITIVE_DISTANCE)
00090   {
00091   }
00092 
00093   bool use_bfs_;
00094   bool use_standard_collision_checking_;
00095   bool attempt_full_shortcut_;
00096   double interpolation_distance_;
00097   double joint_motion_primitive_distance_;
00098 };
00099 
00101 class EnvironmentChain3D: public DiscreteSpaceInformation
00102 {
00103 public:
00104 
00105   EIGEN_MAKE_ALIGNED_OPERATOR_NEW
00106 
00110   EnvironmentChain3D(const planning_scene::PlanningSceneConstPtr& planning_scene);
00111 
00115   ~EnvironmentChain3D();
00116 
00117   //
00118   // Pure virtual functions from DiscreteSpaceInformation
00119   //
00120 
00126   virtual bool InitializeEnv(const char* sEnvFile);
00127 
00132   virtual bool InitializeMDPCfg(MDPConfig *MDPCfg);
00133 
00140   virtual int GetFromToHeuristic(int FromStateID, int ToStateID);
00141 
00147   virtual int GetGoalHeuristic(int stateID);
00148 
00154   virtual int GetStartHeuristic(int stateID);
00155 
00167   virtual void GetSuccs(int SourceStateID, vector<int>* SuccIDV, vector<int>* CostV);
00168 
00170   virtual void GetPreds(int TargetStateID, vector<int>* PredIDV, vector<int>* CostV);
00171 
00173   virtual void SetAllActionsandAllOutcomes(CMDPSTATE* state);
00174 
00176   virtual void SetAllPreds(CMDPSTATE* state);
00177 
00182   virtual int SizeofCreatedEnv();
00183 
00190   virtual void PrintState(int stateID, bool bVerbose, FILE* fOut=NULL);
00191 
00193   virtual void PrintEnv_Config(FILE* fOut);
00194 
00195   //
00196   // Overloaded virtual functions from DiscreteSpaceInformation
00197   //
00198 
00206   virtual void getExpandedStates(std::vector<std::vector<double> >* ara_states) {};
00207 
00215   virtual bool AreEquivalent(int StateID1, int StateID2);
00216 
00217   bool setupForMotionPlan(const planning_scene::PlanningSceneConstPtr& planning_scene,
00218                           const moveit_msgs::GetMotionPlan::Request &req,
00219                           moveit_msgs::GetMotionPlan::Response& res,
00220                           const PlanningParameters& params);
00221 
00222   const EnvChain3DPlanningData& getPlanningData() const {
00223     return planning_data_;
00224   }
00225 
00226   bool populateTrajectoryFromStateIDSequence(const std::vector<int>& state_ids,
00227                                              trajectory_msgs::JointTrajectory& traj) const;
00228 
00229 
00230   const PlanningStatistics& getPlanningStatistics() const {
00231     return planning_statistics_;
00232   }
00233 
00234   bool getPlaneBFSMarker(visualization_msgs::Marker& plane_marker,
00235                          double z_val);
00236 
00237 
00238   const Eigen::Affine3d& getGoalPose() const {
00239     return goal_pose_;
00240   }
00241 
00242   void attemptShortcut(const trajectory_msgs::JointTrajectory& traj_in,
00243                        trajectory_msgs::JointTrajectory& traj_out);
00244 
00245 protected:
00246 
00247   bool getGridXYZInt(const Eigen::Affine3d& pose,
00248                      int(&xyz)[3]) const;
00249 
00250   void getMotionPrimitives(const std::string& group);
00251 
00252   planning_scene::PlanningSceneConstPtr planning_scene_;
00253 
00254   double angle_discretization_;
00255   BFS_3D *bfs_;
00256 
00257   std::vector<boost::shared_ptr<JointMotionWrapper> > joint_motion_wrappers_;
00258   std::vector<boost::shared_ptr<JointMotionPrimitive> > possible_actions_;
00259   planning_models::RobotState *state_;
00260   const collision_detection::CollisionWorldHybrid* hy_world_;
00261   const collision_detection::CollisionRobotHybrid* hy_robot_;
00262   planning_models::RobotState *::JointStateGroup* joint_state_group_;
00263   boost::shared_ptr<collision_detection::GroupStateRepresentation> gsr_;
00264   //boost::shared_ptr<kinematics::KinematicsBase> kinematics_solver_;
00265   const planning_models::RobotState *::LinkState* tip_link_state_;
00266   EnvChain3DPlanningData planning_data_;
00267   kinematic_constraints::KinematicConstraintSet goal_constraint_set_;
00268   kinematic_constraints::KinematicConstraintSet path_constraint_set_;
00269   std::string planning_group_;
00270   Eigen::Affine3d goal_pose_;
00271   PlanningStatistics planning_statistics_;
00272   PlanningParameters planning_parameters_;
00273   int maximum_distance_for_motion_;
00274 
00275   planning_models::RobotState *interpolation_state_1_;
00276   planning_models::RobotState *interpolation_state_2_;
00277   planning_models::RobotState *interpolation_state_temp_;
00278   planning_models::RobotState *::JointStateGroup* interpolation_joint_state_group_1_;
00279   planning_models::RobotState *::JointStateGroup* interpolation_joint_state_group_2_;
00280   planning_models::RobotState *::JointStateGroup* interpolation_joint_state_group_temp_;
00281   std::map<int, std::map<int, std::vector<std::vector<double> > > > generated_interpolations_map_;
00282 
00283   void setMotionPrimitives(const std::string& group_name);
00284   void determineMaximumEndEffectorTravel();
00285 
00286   void convertCoordToJointAngles(const std::vector<int> &coord, std::vector<double> &angles);
00287   void convertJointAnglesToCoord(const std::vector<double> &angle, std::vector<int> &coord);
00288 
00289   int calculateCost(EnvChain3DHashEntry* HashEntry1, EnvChain3DHashEntry* HashEntry2);
00290   int getBFSCostToGoal(int x, int y, int z) const;
00291   int getEndEffectorHeuristic(int FromStateID, int ToStateID);
00292 
00293   double getJointDistanceDoubleSum(const std::vector<double>& angles1,
00294                                    const std::vector<double>& angles2) const;
00295 
00296   int getJointDistanceIntegerSum(const std::vector<double>& angles1,
00297                                  const std::vector<double>& angles2,
00298                                  double delta) const;
00299 
00300   int getJointDistanceIntegerMax(const std::vector<double>& angles1,
00301                                  const std::vector<double>& angles2,
00302                                  double delta) const;
00303 
00304   // double getJointDistanceMax(const std::vector<double>& angles1,
00305   //                            const std::vector<double>& angles2);
00306 
00307 
00308   bool interpolateAndCollisionCheck(const std::vector<double> angles1,
00309                                     const std::vector<double> angles2,
00310                                     std::vector<std::vector<double> >& state_values);
00311 
00312   inline double getEuclideanDistance(double x1, double y1, double z1, double x2, double y2, double z2) const
00313   {
00314     return sqrt((x1-x2)*(x1-x2) + (y1-y2)*(y1-y2) + (z1-z2)*(z1-z2));
00315   }
00316 
00317   //temp
00318   double closest_to_goal_;
00319 };
00320 
00321 //angles are counterclockwise from 0 to 360 in radians, 0 is the center of bin 0, ...
00322 inline void EnvironmentChain3D::convertCoordToJointAngles(const std::vector<int> &coord, std::vector<double> &angles)
00323 {
00324   angles.resize(coord.size());
00325   for(size_t i = 0; i < coord.size(); i++)
00326     angles[i] = coord[i]*angle_discretization_;
00327 }
00328 
00329 inline void EnvironmentChain3D::convertJointAnglesToCoord(const std::vector<double> &angle, std::vector<int> &coord)
00330 {
00331   coord.resize(angle.size());
00332   for(unsigned int i = 0; i < angle.size(); i++)
00333   {
00334     //NOTE: Added 3/1/09
00335     double pos_angle = angle[i];
00336     if(pos_angle < 0.0)
00337       pos_angle += 2*M_PI;
00338 
00339     coord[i] = (int)((pos_angle + angle_discretization_*0.5)/angle_discretization_);
00340   }
00341 }
00342 
00343 // inline double EnvironmentChain3D::getEuclideanDistance(double x1, double y1, double z1, double x2, double y2, double z2) const
00344 // {
00345 //   return sqrt((x1-x2)*(x1-x2) + (y1-y2)*(y1-y2) + (z1-z2)*(z1-z2));
00346 // }
00347 
00348 } //namespace
00349 
00350 #endif


sbpl_interface
Author(s): Gil Jones
autogenerated on Sun Jan 17 2016 12:57:03