demos/Koules/KoulesStateSpace.cpp
00001 /*********************************************************************
00002 * Software License Agreement (BSD License)
00003 *
00004 *  Copyright (c) 2013, Rice University
00005 *  All rights reserved.
00006 *
00007 *  Redistribution and use in source and binary forms, with or without
00008 *  modification, are permitted provided that the following conditions
00009 *  are met:
00010 *
00011 *   * Redistributions of source code must retain the above copyright
00012 *     notice, this list of conditions and the following disclaimer.
00013 *   * Redistributions in binary form must reproduce the above
00014 *     copyright notice, this list of conditions and the following
00015 *     disclaimer in the documentation and/or other materials provided
00016 *     with the distribution.
00017 *   * Neither the name of the Rice University nor the names of its
00018 *     contributors may be used to endorse or promote products derived
00019 *     from this software without specific prior written permission.
00020 *
00021 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032 *  POSSIBILITY OF SUCH DAMAGE.
00033 *********************************************************************/
00034 
00035 /* Author: Beck Chen, Mark Moll */
00036 
00037 #include "KoulesConfig.h"
00038 #include "KoulesStateSpace.h"
00039 #include "KoulesProjection.h"
00040 
00041 namespace ob = ompl::base;
00042 
00043 KoulesStateSpace::KoulesStateSpace(unsigned int numKoules)
00044     : RealVectorStateSpace(4 * numKoules + 5), mass_(numKoules + 1, kouleMass),
00045     radius_(numKoules + 1, kouleRadius)
00046 {
00047     mass_[0] = shipMass;
00048     radius_[0] = shipRadius;
00049     setName("Koules" + boost::lexical_cast<std::string>(numKoules) + getName());
00050     // layout: (x_s y_s vx_s vy_s theta_s ... x_i y_i vx_i vy_i ... ),
00051     // where (x_i, y_i) is the position of koule i (i=1,..,numKoules),
00052     // (vx_i, vy_i) its velocity, (x_s, y_s) the position of the ship,
00053     // (vx_s, vy_s) its velocity, and theta_s its orientation.
00054 
00055     // create the bounds
00056     unsigned int j = 0;
00057     // set the bounds for the ship's position
00058     bounds_.setLow(j, shipRadius);
00059     bounds_.setHigh(j++, sideLength - shipRadius);
00060     bounds_.setLow(j, shipRadius);
00061     bounds_.setHigh(j++, sideLength - shipRadius);
00062     // set the bounds for the ship's velocity
00063     bounds_.setLow(j, -10.);
00064     bounds_.setHigh(j++, 10.);
00065     bounds_.setLow(j, -10.);
00066     bounds_.setHigh(j++, 10.);
00067     // set bounds on orientation
00068     bounds_.setLow(j, -boost::math::constants::pi<double>());
00069     bounds_.setHigh(j++, boost::math::constants::pi<double>());
00070     for (unsigned int i = 0; i < numKoules; ++i)
00071     {
00072         // set the bounds for koule i's position
00073         bounds_.setLow(j, -2. * kouleRadius);
00074         bounds_.setHigh(j++, sideLength + 2. * kouleRadius);
00075         bounds_.setLow(j, -2. * kouleRadius);
00076         bounds_.setHigh(j++, sideLength + 2. * kouleRadius);
00077         // set the bounds for koule i's velocity
00078         bounds_.setLow(j, -10);
00079         bounds_.setHigh(j++, 10.);
00080         bounds_.setLow(j, -10.);
00081         bounds_.setHigh(j++, 10.);
00082     }
00083 }
00084 
00085 void KoulesStateSpace::registerProjections(void)
00086 {
00087     registerDefaultProjection(ob::ProjectionEvaluatorPtr(new KoulesProjection(this, (getDimension() - 1) / 4 + 1)));
00088     registerProjection("PDSTProjection", ob::ProjectionEvaluatorPtr(
00089         new KoulesProjection(this, (getDimension() - 1) / 2 + 1)));
00090 }
00091 
00092 bool KoulesStateSpace::isDead(const ompl::base::State* state, unsigned int i) const
00093 {
00094     const StateType* s = static_cast<const StateType*>(state);
00095     return s->values[i ? 4 * i + 1 : 0] == -2. * kouleRadius;
00096 }
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Defines