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 // 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 
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.