00001 //---------------------------------------------------------------------- 00002 // The Motion Strategy Library (MSL) 00003 //---------------------------------------------------------------------- 00004 // 00005 // Copyright (c) 1998-2000 Iowa State University and Steve LaValle. 00006 // All Rights Reserved. 00007 // 00008 // Permission to use, copy, and distribute this software and its 00009 // documentation is hereby granted free of charge, provided that 00010 // (1) it is not a component of a commercial product, and 00011 // (2) this notice appears in all copies of the software and 00012 // related documentation. 00013 // 00014 // Iowa State University and the author make no representations 00015 // about the suitability or fitness of this software for any purpose. 00016 // It is provided "as is" without express or implied warranty. 00017 //---------------------------------------------------------------------- 00018 00019 #include <fstream.h> 00020 #include <math.h> 00021 00022 #include "problem.h" 00023 #include "defs.h" 00024 00025 #include <LEDA/REDEFINE_NAMES.h> 00026 00027 // Constructor 00028 Problem::Problem(Geom *geom, Model *model, string path = "") { 00029 00030 SetGeom(geom); 00031 SetModel(model); 00032 00033 if ((path.length() > 0)&&(path[path.length()-1] != '/')) 00034 path += "/"; 00035 00036 FilePath = path; 00037 00038 if (is_file(FilePath+"InitialState")) 00039 ReadInitialState(); 00040 else 00041 InitialState = M->LowerState + 00042 0.5*(M->UpperState - M->LowerState); 00043 00044 if (is_file(FilePath+"GoalState")) 00045 ReadGoalState(); 00046 else 00047 GoalState = M->LowerState; 00048 00049 StateDim = M->StateDim; 00050 InputDim = M->InputDim; 00051 LowerState = M->LowerState; 00052 UpperState = M->UpperState; 00053 00054 NumBodies = G->NumBodies; 00055 MaxDeviates = G->MaxDeviates; 00056 } 00057 00058 00059 void Problem::SetGeom(Geom *geom) { 00060 G = geom; 00061 NumBodies = G->NumBodies; 00062 MaxDeviates = G->MaxDeviates; 00063 GeomDim = G->GeomDim; 00064 } 00065 00066 00067 void Problem::SetModel(Model *model) { 00068 M = model; 00069 StateDim = M->StateDim; 00070 InputDim = M->InputDim; 00071 LowerState = M->LowerState; 00072 UpperState = M->UpperState; 00073 if (is_file(FilePath+"InitialState")) 00074 ReadInitialState(); 00075 else 00076 InitialState = M->LowerState + 00077 0.5*(M->UpperState - M->LowerState); 00078 00079 if (is_file(FilePath+"GoalState")) 00080 ReadGoalState(); 00081 else 00082 GoalState = M->LowerState; 00083 } 00084 00085 00086 void Problem::ReadInitialState() { 00087 file_istream fin(FilePath + "InitialState"); 00088 fin >> InitialState; 00089 } 00090 00091 void Problem::ReadGoalState() { 00092 file_istream fin(FilePath + "GoalState"); 00093 fin >> GoalState; 00094 } 00095 00096 00097 // In the base class, steal the following methods from Model 00098 00099 list<vector> Problem::GetInputs(const vector &x) { 00100 return M->GetInputs(x); 00101 } 00102 00103 list<vector> Problem::GetInputs() { 00104 vector x(StateDim); 00105 00106 return M->GetInputs(x); 00107 } 00108 00109 00110 vector Problem::InterpolateState(const vector &x1, const vector &x2, 00111 const double &a) { 00112 return M->LinearInterpolate(x1,x2,a); 00113 } 00114 00115 00116 point Problem::StateToLedaPoint(const vector &x) { 00117 return M->StateToLedaPoint(x); 00118 } 00119 00120 // By default, don't change anything 00121 vector Problem::StateToConfiguration(const vector &x) { 00122 return M->StateToConfiguration(x); 00123 } 00124 00125 // Default metric: use the metric from the model 00126 double Problem::Metric(const vector &x1, const vector &x2) { 00127 return M->Metric(x1,x2); 00128 } 00129 00130 vector Problem::StateDifference(const vector &x1, 00131 const vector &x2) { 00132 return M->StateDifference(x1,x2); 00133 } 00134 00135 bool Problem::Satisfied(const vector &x) { 00136 return ((G->CollisionFree(StateToConfiguration(x)))&& 00137 (M->Satisfied(x))); 00138 } 00139 00140 vector Problem::Integrate(const vector &x, const vector &u, 00141 const double &deltat) { 00142 return M->Integrate(x,u,deltat); 00143 } 00144 00145 // In the base class, steal the following methods from Geom 00146 00147 bool Problem::CollisionFree(const vector &q) { 00148 return G->CollisionFree(q); 00149 } 00150 00151 00152 double Problem::DistanceComp(const vector &q) { 00153 return G->DistanceComp(q); 00154 } 00155 00156 list<polygon> Problem::EnvironmentToLedaPolygons() { 00157 return G->EnvironmentToLedaPolygons(); 00158 } 00159 00160 list<polygon> Problem::RobotToLedaPolygons(const vector &q) { 00161 return G->RobotToLedaPolygons(q); 00162 } 00163 00164 vector Problem::ConfigurationDifference(const vector &q1, 00165 const vector &q2) { 00166 return G->ConfigurationDifference(q1,q2); 00167 } 00168 00169 #include <LEDA/UNDEFINE_NAMES.h>