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 // Constructor 00026 Problem::Problem(Geom *geom, Model *model, string path = "") { 00027 00028 SetGeom(geom); 00029 SetModel(model); 00030 00031 if ((path.length() > 0)&&(path[path.length()-1] != '/')) 00032 path += "/"; 00033 00034 FilePath = path; 00035 00036 READ_PARAMETER_OR_DEFAULT(InitialState,M->LowerState + \ 00037 0.5*(M->UpperState - M->LowerState)); 00038 00039 READ_PARAMETER_OR_DEFAULT(GoalState,M->LowerState); 00040 00041 StateDim = M->StateDim; 00042 InputDim = M->InputDim; 00043 LowerState = M->LowerState; 00044 UpperState = M->UpperState; 00045 00046 NumBodies = G->NumBodies; 00047 MaxDeviates = G->MaxDeviates; 00048 } 00049 00050 00051 void Problem::SetGeom(Geom *geom) { 00052 G = geom; 00053 NumBodies = G->NumBodies; 00054 MaxDeviates = G->MaxDeviates; 00055 GeomDim = G->GeomDim; 00056 } 00057 00058 00059 void Problem::SetModel(Model *model) { 00060 M = model; 00061 StateDim = M->StateDim; 00062 InputDim = M->InputDim; 00063 LowerState = M->LowerState; 00064 UpperState = M->UpperState; 00065 00066 READ_PARAMETER_OR_DEFAULT(InitialState,M->LowerState + \ 00067 0.5*(M->UpperState - M->LowerState)); 00068 00069 READ_PARAMETER_OR_DEFAULT(GoalState,M->LowerState); 00070 00071 } 00072 00073 00074 // In the base class, steal the following methods from Model 00075 00076 list<MSLVector> Problem::GetInputs(const MSLVector &x) { 00077 return M->GetInputs(x); 00078 } 00079 00080 list<MSLVector> Problem::GetInputs() { 00081 MSLVector x(StateDim); 00082 00083 return M->GetInputs(x); 00084 } 00085 00086 00087 MSLVector Problem::InterpolateState(const MSLVector &x1, const MSLVector &x2, 00088 const double &a) { 00089 return M->LinearInterpolate(x1,x2,a); 00090 } 00091 00092 // By default, don't change anything 00093 MSLVector Problem::StateToConfiguration(const MSLVector &x) { 00094 return M->StateToConfiguration(x); 00095 } 00096 00097 // Default metric: use the metric from the model 00098 double Problem::Metric(const MSLVector &x1, const MSLVector &x2) { 00099 return M->Metric(x1,x2); 00100 } 00101 00102 MSLVector Problem::StateDifference(const MSLVector &x1, 00103 const MSLVector &x2) { 00104 return M->StateDifference(x1,x2); 00105 } 00106 00107 bool Problem::Satisfied(const MSLVector &x) { 00108 return ((G->CollisionFree(StateToConfiguration(x)))&& 00109 (M->Satisfied(x))); 00110 } 00111 00112 MSLVector Problem::Integrate(const MSLVector &x, const MSLVector &u, 00113 const double &deltat) { 00114 return M->Integrate(x,u,deltat); 00115 } 00116 00117 // In the base class, steal the following methods from Geom 00118 00119 bool Problem::CollisionFree(const MSLVector &q) { 00120 return G->CollisionFree(q); 00121 } 00122 00123 00124 double Problem::DistanceComp(const MSLVector &q) { 00125 return G->DistanceComp(q); 00126 } 00127 00128 00129 MSLVector Problem::ConfigurationDifference(const MSLVector &q1, 00130 const MSLVector &q2) { 00131 return G->ConfigurationDifference(q1,q2); 00132 } 00133