footstep_graph.h
Go to the documentation of this file.
1 // -*- mode: c++ -*-
2 /*********************************************************************
3  * Software License Agreement (BSD License)
4  *
5  * Copyright (c) 2015, JSK Lab
6  * All rights reserved.
7  *
8  * Redistribution and use in source and binary forms, with or without
9  * modification, are permitted provided that the following conditions
10  * are met:
11  *
12  * * Redistributions of source code must retain the above copyright
13  * notice, this list of conditions and the following disclaimer.
14  * * Redistributions in binary form must reproduce the above
15  * copyright notice, this list of conditions and the following
16  * disclaimer in the documentation and/o2r other materials provided
17  * with the distribution.
18  * * Neither the name of the JSK Lab nor the names of its
19  * contributors may be used to endorse or promote products derived
20  * from this software without specific prior written permission.
21  *
22  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
23  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
24  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
25  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
26  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
27  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
28  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
29  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
30  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
31  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
32  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
33  * POSSIBILITY OF SUCH DAMAGE.
34  *********************************************************************/
35 
36 
37 #ifndef JSK_FOOTSTEP_PLANNER_FOOTSTEP_GRAPH_H_
38 #define JSK_FOOTSTEP_PLANNER_FOOTSTEP_GRAPH_H_
39 
40 #include <ros/ros.h>
41 #include <pcl_ros/point_cloud.h>
42 #include <jsk_footstep_msgs/FootstepArray.h>
43 
51 
52 namespace jsk_footstep_planner
53 {
54  class FootstepGraph: public Graph<FootstepState>
55  {
56  public:
58  typedef typename boost::function< bool(StatePtr target_state, std::vector<StatePtr> &) > SuccessorFunction;
59  typedef typename boost::function< double(StatePtr, StatePtr, double) > PathCostFunction;
60 
63  FootstepGraph(const Eigen::Vector3f& resolution,
64  const bool use_pointcloud_model = false,
65  const bool lazy_projection = true,
66  const bool local_movement = false,
67  const bool use_obstacle_model = false):
69  publish_progress_(false),
71  use_pointcloud_model_(use_pointcloud_model),
72  lazy_projection_(lazy_projection),
73  local_movement_(local_movement),
74  use_obstacle_model_(use_obstacle_model),
75  pointcloud_model_2d_(new pcl::PointCloud<pcl::PointNormal>),
76  tree_model_(new pcl::KdTreeFLANN<pcl::PointNormal>),
77  obstacle_tree_model_(new pcl::KdTreeFLANN<pcl::PointXYZ>),
78  tree_model_2d_(new pcl::search::Octree<pcl::PointNormal>(0.2)),
79  grid_search_(new ANNGrid(0.05)),
80  parameters_(),
81  zero_state_(new FootstepState(0,
82  Eigen::Affine3f::Identity(),
83  Eigen::Vector3f::UnitX(),
84  resolution_)),
86  {
87  }
88  virtual std::vector<StatePtr> successors(StatePtr target_state) {
89  std::vector<StatePtr> ret;
90  successor_func_(target_state, ret);
91  return ret;
92  }
93 
94  virtual bool isGoal(StatePtr state);
95 
96  virtual double pathCost(StatePtr from, StatePtr to, double prev_cost)
97  {
98  return path_cost_func_(from, to, prev_cost);
99  }
100 
105  virtual bool isColliding(StatePtr current_state, StatePtr previous_state);
106  virtual pcl::PointIndices::Ptr getPointIndicesCollidingSphere(const Eigen::Affine3f& c);
107  virtual bool isCollidingBox(const Eigen::Affine3f& c, pcl::PointIndices::Ptr candidates) const;
113  virtual Eigen::Affine3f getRobotCoords(StatePtr current_state, StatePtr previous_state) const;
114  virtual void setBasicSuccessors(
115  std::vector<Eigen::Affine3f> left_to_right_successors);
116 
117  virtual void setGoalState(
119  {
122  }
123 
124  virtual void setLeftGoalState(FootstepState::Ptr goal)
125  {
126  left_goal_state_ = goal;
127  }
128 
129  virtual void setRightGoalState(FootstepState::Ptr goal)
130  {
131  right_goal_state_ = goal;
132  }
133 
138  virtual std::string infoString() const;
139 
140  bool finalizeSteps(const StatePtr &last_1_Step, const StatePtr &lastStep,
141  std::vector<StatePtr> &finalizeSteps);
142 
143  virtual FootstepState::Ptr getGoal(int leg)
144  {
145  if (leg == jsk_footstep_msgs::Footstep::LEFT) {
146  return left_goal_state_;
147  }
148  else if (leg == jsk_footstep_msgs::Footstep::RIGHT) {
149  return right_goal_state_;
150  }
151  else { // TODO: error
152  return goal_state_;
153  }
154  }
155 
156  virtual double maxSuccessorDistance()
157  {
159  }
160  virtual double maxSuccessorRotation()
161  {
163  }
164 
165  virtual void setProgressPublisher(ros::NodeHandle& nh, std::string topic)
166  {
167  publish_progress_ = true;
168  pub_progress_ = nh.advertise<jsk_footstep_msgs::FootstepArray>(topic, 1);
169  }
170 
171  virtual void setPointCloudModel(pcl::PointCloud<pcl::PointNormal>::Ptr model)
172  {
174  tree_model_->setInputCloud(pointcloud_model_);
175  // Project point_cloud_model_
176  pcl::ProjectInliers<pcl::PointNormal> proj;
177  proj.setModelType(pcl::SACMODEL_PLANE);
178  pcl::ModelCoefficients::Ptr
179  plane_coefficients (new pcl::ModelCoefficients);
180  plane_coefficients->values.resize(4.0);
181  plane_coefficients->values[2] = 1.0; // TODO: configurable
182  proj.setModelCoefficients(plane_coefficients);
183  proj.setInputCloud(pointcloud_model_);
184  proj.filter(*pointcloud_model_2d_);
185  tree_model_2d_->setInputCloud(pointcloud_model_2d_);
186  grid_search_->build(*model);
187  }
188 
189  virtual void setObstacleModel(pcl::PointCloud<pcl::PointXYZ>::Ptr model)
190  {
192  obstacle_tree_model_->setInputCloud(obstacle_model_);
193  }
194 
195  virtual bool projectGoal();
196  virtual bool projectStart();
197  virtual bool isSuccessable(StatePtr current_state, StatePtr previous_state);
198  virtual bool useObstacleModel() const { return use_obstacle_model_; }
199  virtual bool usePointCloudModel() const { return use_pointcloud_model_; }
200  virtual bool lazyProjection() const { return lazy_projection_; }
201  virtual bool localMovement() const { return local_movement_; }
202 
203  virtual void setParameters (FootstepParameters &p) { parameters_ = p; }
204  virtual void setTransitionLimit(TransitionLimit::Ptr limit) { transition_limit_ = limit; }
209  virtual FootstepState::Ptr projectFootstep(FootstepState::Ptr in, unsigned int& state);
212  virtual std::vector<FootstepState::Ptr> localMoveFootstepState(FootstepState::Ptr in);
213  virtual void setCollisionBBoxOffset(const Eigen::Affine3f& offset) { collision_bbox_offset_ = offset; }
214  virtual void setCollisionBBoxSize(const Eigen::Vector3f& size) { collision_bbox_size_ = size; }
215 
217  successor_func_ = s;
218  }
219  virtual void setPathCostFunction(PathCostFunction p) {
221  }
222  bool successors_original(StatePtr target_state, std::vector<FootstepGraph::StatePtr> &ret);
223  double path_cost_original(StatePtr from, StatePtr to, double prev_cost) {
224  // path cost is a number of steps
225  return prev_cost + 1;
226  }
228  {
229  heuristic_path_.reset(new jsk_recognition_utils::PolyLine(path_line)); // copy ???
230  }
231  protected:
232  pcl::PointCloud<pcl::PointNormal>::Ptr pointcloud_model_;
233  pcl::PointCloud<pcl::PointXYZ>::Ptr obstacle_model_;
234  pcl::PointCloud<pcl::PointNormal>::Ptr pointcloud_model_2d_;
235  pcl::KdTreeFLANN<pcl::PointNormal>::Ptr tree_model_;
236  pcl::KdTreeFLANN<pcl::PointXYZ>::Ptr obstacle_tree_model_;
237  //pcl::KdTreeFLANN<pcl::PointNormal>::Ptr tree_model_2d_;
238  pcl::search::Octree<pcl::PointNormal>::Ptr tree_model_2d_;
240  std::vector<Eigen::Affine3f> successors_from_left_to_right_;
241  std::vector<Eigen::Affine3f> successors_from_right_to_left_;
249  Eigen::Affine3f collision_bbox_offset_;
250  Eigen::Vector3f collision_bbox_size_;
255  // const params
256  const bool use_pointcloud_model_;
257  const bool lazy_projection_;
258  const bool local_movement_;
259  const bool use_obstacle_model_;
260  const Eigen::Vector3f resolution_;
261  // params
265 
267 
268  // add hint for huristic
270  private:
273  };
274 
275  // heuristic function
276  double footstepHeuristicZero(
284  double first_rotation_weight,
285  double second_rotation_weight);
288 }
289 
290 #endif
jsk_footstep_planner::FootstepGraph::FootstepGraph
FootstepGraph(const Eigen::Vector3f &resolution, const bool use_pointcloud_model=false, const bool lazy_projection=true, const bool local_movement=false, const bool use_obstacle_model=false)
Definition: footstep_graph.h:127
pcl
jsk_footstep_planner::FootstepGraph::PathCostFunction
boost::function< double(StatePtr, StatePtr, double) > PathCostFunction
Definition: footstep_graph.h:123
Eigen
jsk_footstep_planner::FootstepParameters
Definition: footstep_parameters.h:74
footstep_state.h
ros::Publisher
jsk_footstep_planner::FootstepGraph::right_goal_state_
FootstepState::Ptr right_goal_state_
Definition: footstep_graph.h:307
astar_solver.h
boost::shared_ptr< FootstepGraph >
jsk_footstep_planner::FootstepGraph::infoString
virtual std::string infoString() const
return string about graph information.
Definition: footstep_graph.cpp:200
jsk_footstep_planner::footstepHeuristicStraightRotation
double footstepHeuristicStraightRotation(SolverNode< FootstepState, FootstepGraph >::Ptr node, FootstepGraph::Ptr graph)
Definition: footstep_graph.cpp:438
jsk_footstep_planner::FootstepGraph::zero_state_
FootstepState::Ptr zero_state_
zero_state is used only for global transition limit
Definition: footstep_graph.h:312
footstep_parameters.h
jsk_footstep_planner::Graph< FootstepState >::goal_state_
StatePtr goal_state_
Definition: graph.h:135
jsk_footstep_planner::FootstepGraph::getGoal
virtual FootstepState::Ptr getGoal(int leg)
Definition: footstep_graph.h:207
jsk_footstep_planner::FootstepGraph::setRightGoalState
virtual void setRightGoalState(FootstepState::Ptr goal)
Definition: footstep_graph.h:193
jsk_footstep_planner::FootstepGraph::grid_search_
ANNGrid::Ptr grid_search_
Definition: footstep_graph.h:303
ros.h
jsk_footstep_planner::FootstepGraph::setGlobalTransitionLimit
virtual void setGlobalTransitionLimit(TransitionLimit::Ptr limit)
Definition: footstep_graph.h:270
jsk_footstep_planner::FootstepGraph::tree_model_
pcl::KdTreeFLANN< pcl::PointNormal >::Ptr tree_model_
Definition: footstep_graph.h:299
jsk_footstep_planner::FootstepGraph::getTransitionLimit
virtual TransitionLimit::Ptr getTransitionLimit()
Definition: footstep_graph.h:269
jsk_footstep_planner::FootstepGraph::pointcloud_model_
pcl::PointCloud< pcl::PointNormal >::Ptr pointcloud_model_
Definition: footstep_graph.h:296
jsk_footstep_planner::FootstepGraph::maxSuccessorRotation
virtual double maxSuccessorRotation()
Definition: footstep_graph.h:224
jsk_footstep_planner::FootstepGraph::footstepHeuristicFollowPathLine
friend double footstepHeuristicFollowPathLine(SolverNode< FootstepState, FootstepGraph >::Ptr node, FootstepGraph::Ptr graph)
jsk_footstep_planner::FootstepGraph::publish_progress_
bool publish_progress_
Definition: footstep_graph.h:317
jsk_footstep_planner::SolverNode::Ptr
boost::shared_ptr< SolverNode > Ptr
Definition: solver_node.h:117
geo_util.h
jsk_footstep_planner::FootstepGraph::setPathCostFunction
virtual void setPathCostFunction(PathCostFunction p)
Definition: footstep_graph.h:283
left
left
resolution
const Eigen::Vector3f resolution(0.05, 0.05, 0.08)
jsk_footstep_planner::FootstepGraph::max_successor_rotation_
double max_successor_rotation_
Definition: footstep_graph.h:316
jsk_footstep_planner::FootstepGraph::successors_from_left_to_right_
std::vector< Eigen::Affine3f > successors_from_left_to_right_
Definition: footstep_graph.h:304
jsk_footstep_planner::FootstepGraph::setBasicSuccessors
virtual void setBasicSuccessors(std::vector< Eigen::Affine3f > left_to_right_successors)
Definition: footstep_graph.cpp:73
jsk_footstep_planner::FootstepGraph::tree_model_2d_
pcl::search::Octree< pcl::PointNormal >::Ptr tree_model_2d_
Definition: footstep_graph.h:302
jsk_footstep_planner::FootstepGraph::left_goal_state_
FootstepState::Ptr left_goal_state_
Definition: footstep_graph.h:306
jsk_footstep_planner::FootstepGraph::transition_limit_
TransitionLimit::Ptr transition_limit_
Definition: footstep_graph.h:326
ros::NodeHandle::advertise
Publisher advertise(AdvertiseOptions &ops)
jsk_footstep_planner::footstepHeuristicStepCost
double footstepHeuristicStepCost(SolverNode< FootstepState, FootstepGraph >::Ptr node, FootstepGraph::Ptr graph, double first_rotation_weight, double second_rotation_weight)
Definition: footstep_graph.cpp:449
point_cloud.h
jsk_footstep_planner::FootstepGraph::setObstacleModel
virtual void setObstacleModel(pcl::PointCloud< pcl::PointXYZ >::Ptr model)
Definition: footstep_graph.h:253
jsk_footstep_planner::FootstepGraph::projectFootstep
virtual FootstepState::Ptr projectFootstep(FootstepState::Ptr in)
Definition: footstep_graph.cpp:356
jsk_footstep_planner::FootstepGraph::useObstacleModel
virtual bool useObstacleModel() const
Definition: footstep_graph.h:262
jsk_footstep_planner::FootstepGraph::maxSuccessorDistance
virtual double maxSuccessorDistance()
Definition: footstep_graph.h:220
jsk_footstep_planner::FootstepGraph::heuristic_path_
jsk_recognition_utils::PolyLine::Ptr heuristic_path_
Definition: footstep_graph.h:333
jsk_footstep_planner::FootstepState::Ptr
boost::shared_ptr< FootstepState > Ptr
Definition: footstep_state.h:115
jsk_footstep_planner::FootstepGraph::use_pointcloud_model_
const bool use_pointcloud_model_
Definition: footstep_graph.h:320
jsk_footstep_planner::FootstepGraph::setGoalState
virtual void setGoalState(FootstepState::Ptr left, FootstepState::Ptr right)
Definition: footstep_graph.h:181
jsk_footstep_planner::FootstepGraph::resolution_
const Eigen::Vector3f resolution_
Definition: footstep_graph.h:324
jsk_footstep_planner::FootstepGraph::perception_duration_
ros::WallDuration perception_duration_
Definition: footstep_graph.h:330
jsk_footstep_planner
Definition: ann_grid.h:50
jsk_footstep_planner::FootstepGraph::localMoveFootstepState
virtual std::vector< FootstepState::Ptr > localMoveFootstepState(FootstepState::Ptr in)
Definition: footstep_graph.cpp:265
topic
string topic
jsk_footstep_planner::FootstepGraph::pointcloud_model_2d_
pcl::PointCloud< pcl::PointNormal >::Ptr pointcloud_model_2d_
Definition: footstep_graph.h:298
jsk_footstep_planner::FootstepGraph::setProgressPublisher
virtual void setProgressPublisher(ros::NodeHandle &nh, std::string topic)
Definition: footstep_graph.h:229
jsk_footstep_planner::FootstepGraph::setPointCloudModel
virtual void setPointCloudModel(pcl::PointCloud< pcl::PointNormal >::Ptr model)
Definition: footstep_graph.h:235
jsk_footstep_planner::FootstepGraph::lazyProjection
virtual bool lazyProjection() const
Definition: footstep_graph.h:264
jsk_footstep_planner::FootstepGraph::Ptr
boost::shared_ptr< FootstepGraph > Ptr
Definition: footstep_graph.h:121
jsk_footstep_planner::FootstepGraph::localMovement
virtual bool localMovement() const
Definition: footstep_graph.h:265
jsk_footstep_planner::FootstepGraph::max_successor_distance_
double max_successor_distance_
Definition: footstep_graph.h:315
plot_bench.right
right
Definition: plot_bench.py:90
Vector3< float >
pose_array.p
p
Definition: pose_array.py:21
graph
FootstepGraph::Ptr graph
Definition: footstep_planning_2d_interactive_demo.cpp:51
jsk_footstep_planner::FootstepGraph::path_cost_original
double path_cost_original(StatePtr from, StatePtr to, double prev_cost)
Definition: footstep_graph.h:287
search
ROSCPP_DECL bool search(const std::string &key, std::string &result)
Identity
Identity
jsk_footstep_planner::Graph< FootstepState >::StatePtr
boost::shared_ptr< StateT > StatePtr
Definition: graph.h:115
jsk_footstep_planner::FootstepGraph::local_movement_
const bool local_movement_
Definition: footstep_graph.h:322
jsk_footstep_planner::FootstepGraph::isSuccessable
virtual bool isSuccessable(StatePtr current_state, StatePtr previous_state)
Definition: footstep_graph.cpp:246
jsk_footstep_planner::FootstepGraph::usePointCloudModel
virtual bool usePointCloudModel() const
Definition: footstep_graph.h:263
jsk_footstep_planner::FootstepGraph::getPointIndicesCollidingSphere
virtual pcl::PointIndices::Ptr getPointIndicesCollidingSphere(const Eigen::Affine3f &c)
Definition: footstep_graph.cpp:139
jsk_footstep_planner::FootstepGraph::setParameters
virtual void setParameters(FootstepParameters &p)
Definition: footstep_graph.h:267
jsk_recognition_utils::PolyLine
jsk_footstep_planner::FootstepGraph::SuccessorFunction
boost::function< bool(StatePtr target_state, std::vector< StatePtr > &) > SuccessorFunction
Definition: footstep_graph.h:122
jsk_footstep_planner::FootstepGraph::projectStart
virtual bool projectStart()
Definition: footstep_graph.cpp:404
jsk_footstep_planner::FootstepGraph::lazy_projection_
const bool lazy_projection_
Definition: footstep_graph.h:321
jsk_footstep_planner::FootstepGraph::successors
virtual std::vector< StatePtr > successors(StatePtr target_state)
Definition: footstep_graph.h:152
jsk_footstep_planner::FootstepGraph::clearPerceptionDuration
virtual void clearPerceptionDuration()
Definition: footstep_graph.h:275
PointCloud
sensor_msgs::PointCloud2 PointCloud
jsk_footstep_planner::FootstepGraph::projectGoal
virtual bool projectGoal()
Definition: footstep_graph.cpp:382
jsk_footstep_planner::FootstepGraph::use_obstacle_model_
const bool use_obstacle_model_
Definition: footstep_graph.h:323
jsk_footstep_planner::FootstepGraph::obstacle_model_
pcl::PointCloud< pcl::PointXYZ >::Ptr obstacle_model_
Definition: footstep_graph.h:297
graph.h
jsk_footstep_planner::FootstepGraph::setCollisionBBoxSize
virtual void setCollisionBBoxSize(const Eigen::Vector3f &size)
Definition: footstep_graph.h:278
jsk_footstep_planner::FootstepGraph::pathCost
virtual double pathCost(StatePtr from, StatePtr to, double prev_cost)
Definition: footstep_graph.h:160
jsk_footstep_planner::FootstepGraph::collision_bbox_offset_
Eigen::Affine3f collision_bbox_offset_
Definition: footstep_graph.h:313
jsk_footstep_planner::FootstepGraph::getPerceptionDuration
virtual ros::WallDuration getPerceptionDuration()
Definition: footstep_graph.h:274
pose_array.s
s
Definition: pose_array.py:22
jsk_footstep_planner::FootstepGraph::isCollidingBox
virtual bool isCollidingBox(const Eigen::Affine3f &c, pcl::PointIndices::Ptr candidates) const
Definition: footstep_graph.cpp:150
jsk_footstep_planner::FootstepGraph::isGoal
virtual bool isGoal(StatePtr state)
Definition: footstep_graph.cpp:103
transition_limit.h
model
std::string model
Definition: pointcloud_model_generator_node.cpp:45
jsk_footstep_planner::FootstepGraph::successors_from_right_to_left_
std::vector< Eigen::Affine3f > successors_from_right_to_left_
Definition: footstep_graph.h:305
jsk_footstep_planner::FootstepGraph::isColliding
virtual bool isColliding(StatePtr current_state, StatePtr previous_state)
return True if current_state collides with obstacle.
Definition: footstep_graph.cpp:169
jsk_footstep_planner::FootstepGraph::finalizeSteps
bool finalizeSteps(const StatePtr &last_1_Step, const StatePtr &lastStep, std::vector< StatePtr > &finalizeSteps)
Definition: footstep_graph.cpp:186
jsk_footstep_planner::FootstepGraph::obstacle_tree_model_
pcl::KdTreeFLANN< pcl::PointXYZ >::Ptr obstacle_tree_model_
Definition: footstep_graph.h:300
jsk_footstep_planner::FootstepGraph::setLeftGoalState
virtual void setLeftGoalState(FootstepState::Ptr goal)
Definition: footstep_graph.h:188
jsk_footstep_planner::footstepHeuristicStraight
double footstepHeuristicStraight(SolverNode< FootstepState, FootstepGraph >::Ptr node, FootstepGraph::Ptr graph)
Definition: footstep_graph.cpp:427
jsk_footstep_planner::FootstepGraph::setCollisionBBoxOffset
virtual void setCollisionBBoxOffset(const Eigen::Affine3f &offset)
Definition: footstep_graph.h:277
ros::WallDuration
jsk_footstep_planner::FootstepGraph::setSuccessorFunction
virtual void setSuccessorFunction(SuccessorFunction s)
Definition: footstep_graph.h:280
jsk_footstep_planner::FootstepGraph::pub_progress_
ros::Publisher pub_progress_
Definition: footstep_graph.h:318
jsk_footstep_planner::FootstepGraph::getGlobalTransitionLimit
virtual TransitionLimit::Ptr getGlobalTransitionLimit()
Definition: footstep_graph.h:271
jsk_footstep_planner::FootstepGraph::setTransitionLimit
virtual void setTransitionLimit(TransitionLimit::Ptr limit)
Definition: footstep_graph.h:268
jsk_footstep_planner::FootstepGraph::successors_original
bool successors_original(StatePtr target_state, std::vector< FootstepGraph::StatePtr > &ret)
Definition: footstep_graph.cpp:306
jsk_footstep_planner::FootstepGraph::global_transition_limit_
TransitionLimit::Ptr global_transition_limit_
Definition: footstep_graph.h:327
jsk_footstep_planner::FootstepGraph::parameters_
FootstepParameters parameters_
Definition: footstep_graph.h:328
size
size
jsk_footstep_planner::FootstepGraph::collision_bbox_size_
Eigen::Vector3f collision_bbox_size_
Definition: footstep_graph.h:314
jsk_footstep_planner::footstepHeuristicZero
double footstepHeuristicZero(SolverNode< FootstepState, FootstepGraph >::Ptr node, FootstepGraph::Ptr graph)
Definition: footstep_graph.cpp:420
jsk_footstep_planner::FootstepGraph::getRobotCoords
virtual Eigen::Affine3f getRobotCoords(StatePtr current_state, StatePtr previous_state) const
Compute robot coords from current footstep and previous footstep. R_robot = midcoords(F_current,...
Definition: footstep_graph.cpp:133
jsk_footstep_planner::TransitionLimit::Ptr
boost::shared_ptr< TransitionLimit > Ptr
Definition: transition_limit.h:116
jsk_footstep_planner::FootstepGraph::path_cost_func_
PathCostFunction path_cost_func_
Definition: footstep_graph.h:336
ros::NodeHandle
ann_grid.h
jsk_footstep_planner::FootstepGraph::successor_func_
SuccessorFunction successor_func_
Definition: footstep_graph.h:335
jsk_footstep_planner::FootstepGraph::setHeuristicPathLine
void setHeuristicPathLine(jsk_recognition_utils::PolyLine &path_line)
Definition: footstep_graph.h:291
jsk_footstep_planner::footstepHeuristicFollowPathLine
double footstepHeuristicFollowPathLine(SolverNode< FootstepState, FootstepGraph >::Ptr node, FootstepGraph::Ptr graph)
Definition: footstep_graph.cpp:488


jsk_footstep_planner
Author(s): Ryohei Ueda
autogenerated on Wed Jan 24 2024 04:05:29