planning_graph.h

Go to the documentation of this file.

/*
 * Software License Agreement (Apache License)
 *
 * Copyright (c) 2014, Southwest Research Institute
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 * http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */
/*
 * Planning_graph.h
 *
 *  Created on: Jun 5, 2014
 *      Author: Dan Solomon
 */

#ifndef PLANNING_GRAPH_H_
#define PLANNING_GRAPH_H_

#include <boost/graph/adjacency_list.hpp>
#include <boost/function.hpp>
#include "descartes_core/trajectory_pt.h"
#include "descartes_trajectory/cart_trajectory_pt.h"
#include "descartes_trajectory/joint_trajectory_pt.h"

#include <map>
#include <vector>

namespace descartes_planner
{
struct JointVertex
{
  descartes_core::TrajectoryPt::ID id;
};
struct JointEdge
{
  descartes_core::TrajectoryPt::ID joint_start;
  descartes_core::TrajectoryPt::ID joint_end;
  double transition_cost;
};

struct CartesianPointRelationship
{
  descartes_core::TrajectoryPt::ID id;
  descartes_core::TrajectoryPt::ID id_previous;
  descartes_core::TrajectoryPt::ID id_next;
};

typedef boost::adjacency_list<boost::listS,          /*edge container*/
                              boost::vecS,           /*vertex_container*/
                              boost::bidirectionalS, /*allows in_edge and out_edge*/
                              JointVertex,           /*vertex structure*/
                              JointEdge              /*edge structure*/
                              > JointGraph;

typedef boost::graph_traits<JointGraph>::vertex_iterator VertexIterator;
typedef boost::graph_traits<JointGraph>::edge_iterator EdgeIterator;
typedef boost::graph_traits<JointGraph>::out_edge_iterator OutEdgeIterator;
typedef boost::graph_traits<JointGraph>::in_edge_iterator InEdgeIterator;

typedef boost::function<double(const std::vector<double> &, const std::vector<double> &)> CostFunction;

struct CartesianPointInformation
{
  CartesianPointRelationship links_;
  descartes_core::TrajectoryPtPtr source_trajectory_;
  std::vector<descartes_core::TrajectoryPt::ID> joints_;
};

typedef std::map<descartes_core::TrajectoryPt::ID, CartesianPointInformation> CartesianMap;
typedef std::map<descartes_core::TrajectoryPt::ID, descartes_trajectory::JointTrajectoryPt> JointMap;
typedef std::map<descartes_core::TrajectoryPt::ID, JointGraph::vertex_descriptor> VertexMap;

class PlanningGraph
{
public:
  // TODO: add constructor that takes RobotState as param
  PlanningGraph(descartes_core::RobotModelConstPtr model);

  PlanningGraph(descartes_core::RobotModelConstPtr model, CostFunction cost_function_callback);

  virtual ~PlanningGraph();

  void clear();

  bool insertGraph(const std::vector<descartes_core::TrajectoryPtPtr> *points);

  bool addTrajectory(descartes_core::TrajectoryPtPtr point, descartes_core::TrajectoryPt::ID previous_id,
                     descartes_core::TrajectoryPt::ID next_id);

  bool modifyTrajectory(descartes_core::TrajectoryPtPtr point);

  bool removeTrajectory(descartes_core::TrajectoryPtPtr point);

  bool getCartesianTrajectory(std::vector<descartes_core::TrajectoryPtPtr> &traj);

  bool getShortestPath(double &cost, std::list<descartes_trajectory::JointTrajectoryPt> &path);
  // TODO: 'overloaded' requests depending on source and destination
  // bool GetShortestPathJointToCartesian(int startIndex, int endIndex, double &cost,
  // std::vector<descartes_core::TrajectoryPt> &path);
  // bool GetShortestPathCartesianToCartesian(int startIndex, int endIndex, double &cost,
  // std::vector<descartes_core::TrajectoryPt> &path);

  void printGraph();
  void printMaps();

  descartes_core::RobotModelConstPtr getRobotModel();

  CartesianMap getCartesianMap() const;

  const JointMap &getJointMap() const;

  const JointGraph &getGraph() const;

  bool findStartVertices(std::vector<JointGraph::vertex_descriptor> &start_points) const;

  bool findEndVertices(std::vector<JointGraph::vertex_descriptor> &end_points) const;

protected:
  descartes_core::RobotModelConstPtr robot_model_;

  CostFunction custom_cost_function_;

  JointGraph dg_;

  int recalculateJointSolutionsVertexMap(
      std::map<descartes_core::TrajectoryPt::ID, JointGraph::vertex_descriptor> &joint_vertex_map) const;

  typedef std::pair<bool, double> EdgeWeightResult;

  EdgeWeightResult edgeWeight(const descartes_trajectory::JointTrajectoryPt &start,
                              const descartes_trajectory::JointTrajectoryPt &end) const;

  // NOTE: both Cartesian Points and Joint Points/solutions extend a base descartes_core::TrajectoryPt type
  //       and include an accessor to both formats

  // maintains the original (Cartesian) points list along with link information and associated joint trajectories per
  // point
  CartesianMap *cartesian_point_link_;

  // maintains a map of joint solutions with it's corresponding graph vertex_descriptor
  //   one or more of these will exist for each element in trajectory_point_map
  JointMap joint_solutions_map_;

  bool calculateJointSolutions(const std::vector<descartes_core::TrajectoryPtPtr> &points,
                               std::vector<std::vector<descartes_trajectory::JointTrajectoryPt>> &poses);

  bool populateGraphVertices(const std::vector<descartes_core::TrajectoryPtPtr> &points,
                             std::vector<std::vector<descartes_trajectory::JointTrajectoryPt>> &poses);

  bool calculateEdgeWeights(const std::vector<descartes_core::TrajectoryPt::ID> &start_joints,
                            const std::vector<descartes_core::TrajectoryPt::ID> &end_joints,
                            std::vector<JointEdge> &edge_results);

  bool calculateEdgeWeights(const std::vector<descartes_trajectory::JointTrajectoryPt> &start_joints,
                            const std::vector<descartes_trajectory::JointTrajectoryPt> &end_joints,
                            std::vector<JointEdge> &edge_results) const;

  bool calculateAllEdgeWeights(const std::vector<std::vector<descartes_trajectory::JointTrajectoryPt>> &poses,
                               std::vector<JointEdge> &edges);

  bool populateGraphEdges(const std::vector<JointEdge> &edges);
};

} /* namespace descartes_planner */

#endif /* PLANNING_GRAPH_H_ */