state_space.h

Go to the documentation of this file.

//=================================================================================================
// Copyright (c) 2018, Alexander Stumpf, TU Darmstadt
// Based on http://wiki.ros.org/footstep_planner by Johannes Garimort and Armin Hornung
// All rights reserved.

// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are met:
//     * Redistributions of source code must retain the above copyright
//       notice, this list of conditions and the following disclaimer.
//     * Redistributions in binary form must reproduce the above copyright
//       notice, this list of conditions and the following disclaimer in the
//       documentation and/or other materials provided with the distribution.
//     * Neither the name of the Simulation, Systems Optimization and Robotics
//       group, TU Darmstadt nor the names of its contributors may be used to
//       endorse or promote products derived from this software without
//       specific prior written permission.

// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
// DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER BE LIABLE FOR ANY
// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
//=================================================================================================

#ifndef VIGIR_FOOTSTEP_PLANNING_STATE_SPACE_H__
#define VIGIR_FOOTSTEP_PLANNING_STATE_SPACE_H__

#include <ros/ros.h>

#include <boost/thread/mutex.hpp>

#include <tr1/unordered_set>
#include <tr1/hashtable.h>

#include <sbpl/headers.h>

#include <vigir_footstep_planning_lib/modeling/footstep.h>
#include <vigir_footstep_planning_lib/modeling/planning_state.h>

#include <vigir_footstep_planner/environment_parameters.h>



namespace vigir_footstep_planning
{
// specialization of hash<int,int>, similar to standard boost::hash on pairs?
struct IntPairHash{
public:
  size_t operator()(std::pair<int, int> x) const throw() {
    size_t seed = std::tr1::hash<int>()(x.first);
    return std::tr1::hash<int>()(x.second) + 0x9e3779b9 + (seed<<6) + (seed>>2);
  }
};

struct StepCostPair {
  StepCostPair(const PlanningState* state, const int cost)
    : state(state)
    , cost(cost)
  {}

  bool operator < (const StepCostPair& other)
  {
    return cost < other.cost;
  }

  const PlanningState* state;
  const int cost;
};

typedef std::vector<int> exp_states_t;
typedef exp_states_t::const_iterator exp_states_iter_t;
typedef std::tr1::unordered_set<std::pair<int,int>, IntPairHash > exp_states_2d_t;
typedef exp_states_2d_t::const_iterator exp_states_2d_iter_t;



class StateSpace
{
public:
  StateSpace(const EnvironmentParameters& params, std::vector<int*>& state_ID2_index_mapping);

  virtual ~StateSpace();

  void reset();

  // typedefs
  typedef boost::shared_ptr<StateSpace> Ptr;
  typedef boost::shared_ptr<StateSpace> ConstPtr;

  void setFrameId(const std::string& frame_id);

  std::pair<int, int> updateGoal(const State& foot_left, const State& foot_right);
  std::pair<int, int> updateStart(const State& foot_left, const State& right_right);
  void setPlannerStartAndGoal(unsigned int start_foot_selection);

  bool getState(unsigned int id, State &s) const;

  bool getStartState(State &left, State &right) const;
  bool getStartState(State &robot) const;
  bool getGoalState(State &left, State &right) const;
  bool getGoalState(State &robot) const;

  exp_states_iter_t getRandomStatesStart()
  {
    return ivRandomStates.begin();
  }

  exp_states_iter_t getRandomStatesEnd()
  {
    return ivRandomStates.end();
  }


  PlanningState* createNewHashEntry(const State& s);

  PlanningState *createNewHashEntry(const PlanningState& s);

  PlanningState* getHashEntry(const State& s);

  PlanningState* getHashEntry(const PlanningState& s);

  PlanningState *createHashEntryIfNotExists(const PlanningState& s);

  bool closeToGoal(const PlanningState& from) const;

  bool closeToStart(const PlanningState& from) const;


  bool getStepCost(const State& stand_foot, const State& swing_foot_before, const State& swing_foot_after, double& cost, double& risk) const;
  bool getStepCost(const State& stand_foot, const State& swing_foot_before, const State& swing_foot_after, int& cost, int& risk) const;
  bool getStepCost(const State& stand_foot, const State& swing_foot_before, const State& swing_foot_after, int& cost) const;

  const EnvironmentParameters& params;
  std::string frame_id;

  int ivIdPlanningStart;
  int ivIdPlanningGoal;

  PlanningState* start_foot_left;
  PlanningState* start_foot_right;

  int ivIdStartFootLeft;
  int ivIdStartFootRight;

  PlanningState* goal_foot_left;
  PlanningState* goal_foot_right;

  int ivIdGoalFootLeft;
  int ivIdGoalFootRight;

  std::vector<int> ivStateArea;

  std::vector<int*>& state_ID2_index_mapping;

  std::vector<const PlanningState*> ivStateId2State;

  std::vector<PlanningState*>* ivpStateHash2State;

  bool ivHeuristicExpired;

  const int ivRandomNodeDist;

  exp_states_t ivRandomStates;  

  mutable boost::shared_mutex hash_table_shared_mutex;
};
}

#endif