Main Page   Class Hierarchy   Compound List   File List   Compound Members   File Members  

problem.C

Go to the documentation of this file.
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>
Motion Strategy Library


Web page maintained by Steve LaValle
Partial support provided by NSF CAREER Award IRI-970228 (LaValle), Honda Research, and Iowa State University.
Contributors: Anna Atramentov, Peng Cheng, James Kuffner, Steve LaValle, and Libo Yang.