Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
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
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
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
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
00305
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
00318 double closest_to_goal_;
00319 };
00320
00321
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
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
00344
00345
00346
00347
00348 }
00349
00350 #endif