footstep_graph.cpp
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 
37 #include <sstream>
38 
39 namespace jsk_footstep_planner
40 {
42  std::vector<Eigen::Affine3f> left_to_right_successors)
43  {
44  successors_from_left_to_right_ = left_to_right_successors;
45  for (size_t i = 0; i < successors_from_left_to_right_.size(); i++) {
46  Eigen::Affine3f transform = successors_from_left_to_right_[i];
47  float roll, pitch, yaw;
48  pcl::getEulerAngles(transform, roll, pitch, yaw);
49  Eigen::Vector3f translation = transform.translation();
50  Eigen::Affine3f flipped_transform
51  = Eigen::Translation3f(translation[0], -translation[1], translation[2])
52  * Eigen::Quaternionf(Eigen::AngleAxisf(-yaw, Eigen::Vector3f::UnitZ()));
53  successors_from_right_to_left_.push_back(flipped_transform);
54  }
55 
56  // max_successor_distance_
57  for (size_t i = 0; i < successors_from_left_to_right_.size(); i++) {
58  Eigen::Affine3f transform = successors_from_left_to_right_[i];
59  //double dist = transform.translation().norm();
60  double dist = transform.translation()[0]; // Only consider x
61  if (dist > max_successor_distance_) {
63  }
64  double rot = Eigen::AngleAxisf(transform.rotation()).angle();
65  if (rot > max_successor_rotation_) {
67  }
68  }
69  }
70 
71  bool FootstepGraph::isGoal(StatePtr state)
72  {
73  FootstepState::Ptr goal = getGoal(state->getLeg());
74  if (publish_progress_) {
75  jsk_footstep_msgs::FootstepArray msg;
76  msg.header.frame_id = "odom"; // TODO fixed frame_id
77  msg.header.stamp = ros::Time::now();
78  msg.footsteps.push_back(*state->toROSMsg());
80  }
81  Eigen::Affine3f pose = state->getPose();
82  Eigen::Affine3f goal_pose = goal->getPose();
83  Eigen::Affine3f transformation = pose.inverse() * goal_pose;
84 
85  if ((parameters_.goal_pos_thr > transformation.translation().norm()) &&
86  (parameters_.goal_rot_thr > std::abs(Eigen::AngleAxisf(transformation.rotation()).angle()))) {
87  // check collision
88  if (state->getLeg() == jsk_footstep_msgs::Footstep::LEFT) {
89  if (right_goal_state_->crossCheck(state)) {
90  return true;
91  }
92  } else if (state->getLeg() == jsk_footstep_msgs::Footstep::RIGHT) {
93  if (left_goal_state_->crossCheck(state)) {
94  return true;
95  }
96  }
97  }
98  return false;
99  }
100 
101  Eigen::Affine3f FootstepGraph::getRobotCoords(StatePtr current_state, StatePtr previous_state) const
102  {
103  Eigen::Affine3f mid = current_state->midcoords(*previous_state);
104  return mid * collision_bbox_offset_;
105  }
106 
107  pcl::PointIndices::Ptr FootstepGraph::getPointIndicesCollidingSphere(const Eigen::Affine3f& c)
108  {
109  pcl::PointXYZ center;
110  center.getVector3fMap() = Eigen::Vector3f(c.translation());
111  const double r = collision_bbox_size_.norm() / 2 + parameters_.obstacle_resolution;
112  pcl::PointIndices::Ptr near_indices(new pcl::PointIndices);
113  std::vector<float> distances;
114  obstacle_tree_model_->radiusSearch(center, r, near_indices->indices, distances);
115  return near_indices;
116  }
117 
118  bool FootstepGraph::isCollidingBox(const Eigen::Affine3f& c, pcl::PointIndices::Ptr candidates) const
119  {
120  const pcl::PointCloud<pcl::PointXYZ>::ConstPtr input_cloud = obstacle_tree_model_->getInputCloud();
121  Eigen::Affine3f inv_c = c.inverse();
122  for (size_t i = 0; i < candidates->indices.size(); i++) {
123  int index = candidates->indices[i];
124  const pcl::PointXYZ candidate_point = input_cloud->points[index];
125  // convert candidate_point into `c' local representation.
126  const Eigen::Vector3f local_p = inv_c * candidate_point.getVector3fMap();
127  if (std::abs(local_p[0]) < collision_bbox_size_[0] / 2 &&
128  std::abs(local_p[1]) < collision_bbox_size_[1] / 2 &&
129  std::abs(local_p[2]) < collision_bbox_size_[2] / 2) {
130  return true;
131  }
132  }
133  return false;
134  }
135 
136  // return true if colliding with obstacle
137  bool FootstepGraph::isColliding(StatePtr current_state, StatePtr previous_state)
138  {
139  // if not use obstacle model, always return false
140  // if use obstacle model and obstacle_model_ point cloud size is zero, always return false
141  // => to be collision-free always.
142  if (!use_obstacle_model_ || (obstacle_model_->size() == 0) ) {
143  return false;
144  }
145  // compute robot coorde
146  Eigen::Affine3f robot_coords = getRobotCoords(current_state, previous_state);
147  pcl::PointIndices::Ptr sphere_candidate = getPointIndicesCollidingSphere(robot_coords);
148  if (sphere_candidate->indices.size() == 0) {
149  return false;
150  }
151  return isCollidingBox(robot_coords, sphere_candidate);
152  }
153 
154  bool FootstepGraph::finalizeSteps(const StatePtr &last_1_Step, const StatePtr &lastStep,
155  std::vector<StatePtr> &finalizeSteps) {
156  // simple finalize (no check)
157  if (lastStep->getLeg() == jsk_footstep_msgs::Footstep::LEFT) {
158  finalizeSteps.push_back(right_goal_state_);
159  finalizeSteps.push_back(left_goal_state_);
160  } else if (lastStep->getLeg() == jsk_footstep_msgs::Footstep::RIGHT) {
161  finalizeSteps.push_back(left_goal_state_);
162  finalizeSteps.push_back(right_goal_state_);
163  }
164 
165  return true;
166  }
167 
168  std::string FootstepGraph::infoString() const
169  {
170  std::stringstream ss;
171  ss << "footstep_graph" << std::endl;
172  ss << " goal_pos_thr: " << parameters_.goal_pos_thr << std::endl;
173  ss << " goal_rot_thr: " << parameters_.goal_rot_thr << std::endl;
174  ss << " use_pointcloud_model: " << use_pointcloud_model_ << std::endl;
175  ss << " lazy_projection: " << lazy_projection_ << std::endl;
176  ss << " local_movement: " << local_movement_ << std::endl;
177  ss << " transition_limit: " << transition_limit_ << std::endl;
179  ss << " global_transition_limit: " << global_transition_limit_ << std::endl;
180  }
181  else {
182  ss << " global_transition_limit: None" << std::endl;
183  }
184  ss << " local_move_x: " << parameters_.local_move_x << std::endl;
185  ss << " local_move_y: " << parameters_.local_move_y << std::endl;
186  ss << " local_move_theta: " << parameters_.local_move_theta << std::endl;
187  ss << " local_move_x_num: " << parameters_.local_move_x_num << std::endl;
188  ss << " local_move_y_num: " << parameters_.local_move_y_num << std::endl;
189  ss << " local_move_theta_num: " << parameters_.local_move_theta_num << std::endl;
190  ss << " resolution: ["
191  << resolution_[0] << ", "
192  << resolution_[1] << ", "
193  << resolution_[2] << "]"
194  << std::endl;
195  ss << " plane_estimation_use_normal: " << parameters_.plane_estimation_use_normal << std::endl;
196  ss << " plane_estimation_normal_distance_weight: " << parameters_.plane_estimation_normal_distance_weight << std::endl;
197  ss << " plane_estimation_normal_opening_angle: " << parameters_.plane_estimation_normal_opening_angle << std::endl;
198  ss << " plane_estimation_min_ratio_of_inliers: " << parameters_.plane_estimation_min_ratio_of_inliers << std::endl;
199  ss << " plane_estimation_max_iterations: " << parameters_.plane_estimation_max_iterations << std::endl;
200  ss << " plane_estimation_min_inliers: " << parameters_.plane_estimation_min_inliers << std::endl;
201  ss << " plane_estimation_outlier_threshold: " << parameters_.plane_estimation_outlier_threshold << std::endl;
202 
203  ss << " support_check_x_sampling: " << parameters_.support_check_x_sampling << std::endl;
204  ss << " support_check_y_sampling: " << parameters_.support_check_y_sampling << std::endl;
205  ss << " support_check_vertex_neighbor_threshold: " << parameters_.support_check_vertex_neighbor_threshold << std::endl;
206  ss << " support_padding_x: " << parameters_.support_padding_x << std::endl;
207  ss << " support_padding_y: " << parameters_.support_padding_y << std::endl;
208  ss << " skip_cropping: " << parameters_.skip_cropping << std::endl;
209 
210  return ss.str();
211  }
212 
213  bool
214  FootstepGraph::isSuccessable(StatePtr current_state, StatePtr previous_state)
215  {
217  if (!global_transition_limit_->check(zero_state_, current_state)) {
218  return false;
219  }
220  }
221  if (transition_limit_) {
222  if (!transition_limit_->check(previous_state, current_state)) {
223  return false;
224  }
225  }
226  if (use_obstacle_model_) {
227  return !isColliding(current_state, previous_state);
228  }
229  return true;
230  }
231 
232  std::vector<FootstepState::Ptr>
234  {
235  std::vector<FootstepState::Ptr> moved_states;
236  moved_states.reserve((2*parameters_.local_move_x_num + 1)*(2*parameters_.local_move_y_num + 1)*(2*parameters_.local_move_theta_num +1) - 1);
237  int x_num = parameters_.local_move_x_num;
238  int y_num = parameters_.local_move_y_num;
239  int theta_num = parameters_.local_move_theta_num;
240  if(x_num == 0) x_num = 1;
241  if(y_num == 0) y_num = 1;
242  if(theta_num == 0) theta_num = 1;
243 
244  double move_x = parameters_.local_move_x;
245  double move_y = parameters_.local_move_y;
246  double move_t = parameters_.local_move_theta;
247  double offset_x = parameters_.local_move_x_offset;
248  double offset_y = (in->getLeg() == jsk_footstep_msgs::Footstep::LEFT) ?
250  double offset_t = parameters_.local_move_theta_offset;
251 
252  bool have_offset = ((offset_x != 0.0) || (offset_y != 0.0) || (offset_t != 0.0));
253  for (int xi = - parameters_.local_move_x_num; xi <= parameters_.local_move_x_num; xi++) {
254  for (int yi = - parameters_.local_move_y_num; yi <= parameters_.local_move_y_num; yi++) {
255  for (int thetai = - parameters_.local_move_theta_num; thetai <= parameters_.local_move_theta_num; thetai++) {
256  if ( have_offset || (xi != 0) || (yi != 0) || (thetai != 0) ) {
257  Eigen::Affine3f trans(Eigen::Translation3f((move_x / x_num * xi) + offset_x,
258  (move_y / y_num * yi) + offset_y,
259  0)
260  * Eigen::AngleAxisf((move_t / theta_num * thetai) + offset_t,
261  Eigen::Vector3f::UnitZ()));
262  moved_states.push_back(
263  FootstepState::Ptr(new FootstepState(in->getLeg(),
264  in->getPose() * trans,
265  in->getDimensions(),
266  in->getResolution())));
267  }
268  }
269  }
270  }
271  return moved_states;
272  }
273 
274  bool FootstepGraph::successors_original(StatePtr target_state, std::vector<FootstepGraph::StatePtr> &ret)
275  {
276  std::vector<Eigen::Affine3f> transformations;
277  int next_leg;
278  if (target_state->getLeg() == jsk_footstep_msgs::Footstep::LEFT) {
279  transformations = successors_from_left_to_right_;
280  next_leg = jsk_footstep_msgs::Footstep::RIGHT;
281  }
282  else if (target_state->getLeg() == jsk_footstep_msgs::Footstep::RIGHT) {
283  transformations = successors_from_right_to_left_;
284  next_leg = jsk_footstep_msgs::Footstep::LEFT;
285  }
286  else {
287  // TODO: error
288  }
289 
290  //std::vector<FootstepGraph::StatePtr> ret;
291  Eigen::Affine3f base_pose = target_state->getPose();
292  for (size_t i = 0; i < transformations.size(); i++) {
293  Eigen::Affine3f transform = transformations[i];
294  FootstepGraph::StatePtr next(new FootstepState(next_leg,
295  base_pose * transform,
296  target_state->getDimensions(),
297  resolution_));
299  // Update footstep position by projection
300  unsigned int error_state;
301  FootstepGraph::StatePtr tmpnext = projectFootstep(next, error_state);
302  if (!tmpnext && localMovement() && error_state == projection_state::close_to_success) {
303  std::vector<StatePtr> locally_moved_nodes = localMoveFootstepState(next);
304  for (size_t j = 0; j < locally_moved_nodes.size(); j++) {
305  if (isSuccessable(locally_moved_nodes[j], target_state)) {
306  FootstepGraph::StatePtr tmp = projectFootstep(locally_moved_nodes[j], error_state);
307  if(!!tmp) {
308  ret.push_back(tmp);
309  }
310  }
311  }
312  }
313  next = tmpnext;
314  }
315  if (!!next) {
316  if (isSuccessable(next, target_state)) {
317  ret.push_back(next);
318  }
319  }
320  }
321  return true;
322  }
323 
325  {
326  unsigned int error_state;
327  return projectFootstep(in, error_state);
328  }
329 
331  unsigned int& error_state)
332  {
333  if(!use_pointcloud_model_) {
334  return in;
335  }
337  FootstepState::Ptr projected_footstep = in->projectToCloud(
338  *tree_model_,
340  grid_search_,
343  Eigen::Vector3f(0, 0, 1),
344  error_state, parameters_);
345  ros::WallTime end_time = ros::WallTime::now();
347  return projected_footstep;
348  }
349 
351  {
352  unsigned int error_state;
355  if (left_projected && right_projected) {
357  if (!global_transition_limit_->check(zero_state_, left_projected) ||
358  !global_transition_limit_->check(zero_state_, right_projected)) {
359  return false;
360  }
361  }
362 
363  left_goal_state_ = left_projected;
364  right_goal_state_ = right_projected;
365  return true;
366  }
367  else {
368  return false;
369  }
370  }
371 
373  {
374  unsigned int error_state;
377  if (!global_transition_limit_->check(zero_state_, projected)) {
378  return false;
379  }
380  }
381  if (projected) {
382  start_state_ = projected;
383  return true;
384  }
385  return false;
386  }
387 
388  double footstepHeuristicZero(
390  {
391  // best-first search
392  return 0;
393  }
394 
397  {
398  // distance_to_goal / maxSuccessor
399  FootstepState::Ptr state = node->getState();
400  FootstepState::Ptr goal = graph->getGoal(state->getLeg());
401  Eigen::Vector3f state_pos(state->getPose().translation());
402  Eigen::Vector3f goal_pos(goal->getPose().translation());
403  return (std::abs((state_pos - goal_pos).norm() / graph->maxSuccessorDistance()));
404  }
405 
408  {
409  // distance_to_goal / maxSuccessor.trans + rotation_to_goal / maxSuccessor.rot
410  FootstepState::Ptr state = node->getState();
411  FootstepState::Ptr goal = graph->getGoal(state->getLeg());
412  Eigen::Affine3f transform = state->getPose().inverse() * goal->getPose();
413  return (std::abs(transform.translation().norm() / graph->maxSuccessorDistance()) +
414  std::abs(Eigen::AngleAxisf(transform.rotation()).angle()) / graph->maxSuccessorRotation());
415  }
416 
419  double first_rotation_weight,
420  double second_rotation_weight)
421  {
422  FootstepState::Ptr state = node->getState();
423  FootstepState::Ptr goal = graph->getGoal(state->getLeg());
424  Eigen::Vector3f goal_pos(goal->getPose().translation());
425  Eigen::Vector3f diff_pos(goal_pos - state->getPose().translation());
426  diff_pos[2] = 0.0; // Ignore z distance
427  Eigen::Quaternionf first_rot;
428  // Eigen::Affine3f::rotation is too slow because it calls SVD decomposition
429  first_rot.setFromTwoVectors(state->getPose().matrix().block<3, 3>(0, 0) * Eigen::Vector3f::UnitX(),
430  diff_pos.normalized());
431 
432  Eigen::Quaternionf second_rot;
433  second_rot.setFromTwoVectors(diff_pos.normalized(),
434  goal->getPose().matrix().block<3, 3>(0, 0) * Eigen::Vector3f::UnitX());
435  // is it correct??
436  double first_theta = acos(first_rot.w()) * 2;
437  double second_theta = acos(second_rot.w()) * 2;
438  if (isnan(first_theta)) {
439  first_theta = 0;
440  }
441  if (isnan(second_theta)) {
442  second_theta = 0;
443  }
444  // acos := [0, M_PI]
445  if (first_theta > M_PI) {
446  first_theta = 2.0 * M_PI - first_theta;
447  }
448  if (second_theta > M_PI) {
449  second_theta = 2.0 * M_PI - second_theta;
450  }
451  //return (Eigen::Vector2f(diff_pos[0], diff_pos[1]).norm() / graph->maxSuccessorDistance()) + std::abs(diff_pos[2]) / 0.2 +
452  return (diff_pos.norm() / graph->maxSuccessorDistance()) + std::abs(diff_pos[2]) / 0.2 +
453  (first_theta * first_rotation_weight + second_theta * second_rotation_weight) / graph->maxSuccessorRotation();
454  }
455 
458  {
459  FootstepState::Ptr state = node->getState();
460  FootstepState::Ptr goal = graph->getGoal(state->getLeg());
461 
462  Eigen::Vector3f goal_pos(goal->getPose().translation());
463  Eigen::Vector3f state_pos(state->getPose().translation());
464  Eigen::Vector3f state_mid_pos;
465  if (state->getLeg() == jsk_footstep_msgs::Footstep::LEFT) {
466  Eigen::Vector3f p(0, -0.1, 0);
467  state_mid_pos = state->getPose() * p;
468  } else { // Right
469  Eigen::Vector3f p(0, 0.1, 0);
470  state_mid_pos = state->getPose() * p;
471  }
472  double dist, to_goal, alp;
473  int idx;
474  Eigen::Vector3f foot;
475  dist = graph->heuristic_path_->distanceWithInfo(state_mid_pos, foot, to_goal, idx, alp);
476 
477  //jsk_recognition_utils::FiniteLine::Ptr ln = graph->heuristic_path_->at(idx);
478  Eigen::Vector3f dir = graph->heuristic_path_->getDirection(idx);
479 
480  Eigen::Quaternionf path_foot_rot;
481  path_foot_rot.setFromTwoVectors(state->getPose().matrix().block<3, 3>(0, 0) * Eigen::Vector3f::UnitX(),
482  dir);
483  double path_foot_theta = acos(path_foot_rot.w()) * 2;
484  if (path_foot_theta > M_PI) {
485  path_foot_theta = 2.0 * M_PI - path_foot_theta;
486  // foot_theta : [0, M_PI]
487  }
488 
489  double step_cost = to_goal / graph->maxSuccessorDistance();
490  double follow_cost = dist / 0.02; // ???
491  double path_foot_rot_cost = path_foot_theta / graph->maxSuccessorRotation();
492 
493  Eigen::Vector3f goal_diff = goal_pos - state_pos;
494  Eigen::Quaternionf goal_foot_rot;
495  goal_foot_rot.setFromTwoVectors(state->getPose().matrix().block<3, 3>(0, 0) * Eigen::Vector3f::UnitX(),
496  goal->getPose().matrix().block<3, 3>(0, 0) * Eigen::Vector3f::UnitX());
497  double goal_foot_theta = acos(goal_foot_rot.w()) * 2;
498  if (goal_foot_theta > M_PI) {
499  goal_foot_theta = 2.0 * M_PI - goal_foot_theta;
500  }
501  double goal_foot_rot_cost = goal_foot_theta / graph->maxSuccessorRotation();
502 
503  //return step_cost + follow_cost + (4.0 * goal_foot_rot_cost) + (0.5 * path_foot_rot_cost);
504  return 2*(step_cost + follow_cost + (0.5 * path_foot_rot_cost));
505  }
506 }
rot
rot
msg
msg
jsk_footstep_planner::FootstepGraph::right_goal_state_
FootstepState::Ptr right_goal_state_
Definition: footstep_graph.h:307
tmp
tmp
jsk_footstep_planner::FootstepParameters::goal_pos_thr
double goal_pos_thr
Definition: footstep_parameters.h:152
boost::shared_ptr< FootstepState >
jsk_footstep_planner::FootstepParameters::local_move_x_num
int local_move_x_num
Definition: footstep_parameters.h:130
i
int i
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
jsk_footstep_planner::FootstepGraph::getGoal
virtual FootstepState::Ptr getGoal(int leg)
Definition: footstep_graph.h:207
jsk_footstep_planner::FootstepParameters::plane_estimation_max_iterations
int plane_estimation_max_iterations
Definition: footstep_parameters.h:133
jsk_footstep_planner::FootstepGraph::grid_search_
ANNGrid::Ptr grid_search_
Definition: footstep_graph.h:303
M_PI
#define M_PI
jsk_footstep_planner::FootstepParameters::local_move_y
double local_move_y
Definition: footstep_parameters.h:138
jsk_footstep_planner::FootstepGraph::tree_model_
pcl::KdTreeFLANN< pcl::PointNormal >::Ptr tree_model_
Definition: footstep_graph.h:299
jsk_footstep_planner::FootstepGraph::pointcloud_model_
pcl::PointCloud< pcl::PointNormal >::Ptr pointcloud_model_
Definition: footstep_graph.h:296
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
jsk_footstep_planner::FootstepParameters::local_move_theta
double local_move_theta
Definition: footstep_parameters.h:139
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
c
c
jsk_footstep_planner::FootstepParameters::local_move_theta_num
int local_move_theta_num
Definition: footstep_parameters.h:132
transform
template Halfspace< double > transform(const Halfspace< double > &a, const Transform3< double > &tf)
jsk_footstep_planner::FootstepParameters::local_move_theta_offset
double local_move_theta_offset
Definition: footstep_parameters.h:142
jsk_footstep_planner::FootstepGraph::left_goal_state_
FootstepState::Ptr left_goal_state_
Definition: footstep_graph.h:306
ros::Publisher::publish
void publish(const boost::shared_ptr< M > &message) const
jsk_footstep_planner::FootstepGraph::transition_limit_
TransitionLimit::Ptr transition_limit_
Definition: footstep_graph.h:326
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
jsk_footstep_planner::FootstepParameters::support_check_vertex_neighbor_threshold
double support_check_vertex_neighbor_threshold
Definition: footstep_parameters.h:158
jsk_footstep_planner::FootstepGraph::projectFootstep
virtual FootstepState::Ptr projectFootstep(FootstepState::Ptr in)
Definition: footstep_graph.cpp:356
jsk_footstep_planner::FootstepParameters::goal_rot_thr
double goal_rot_thr
Definition: footstep_parameters.h:153
pose
pose
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::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
jsk_footstep_planner::FootstepParameters::plane_estimation_min_ratio_of_inliers
double plane_estimation_min_ratio_of_inliers
Definition: footstep_parameters.h:156
jsk_footstep_planner::FootstepGraph::pointcloud_model_2d_
pcl::PointCloud< pcl::PointNormal >::Ptr pointcloud_model_2d_
Definition: footstep_graph.h:298
jsk_footstep_planner::FootstepState
Definition: footstep_state.h:112
jsk_footstep_planner::FootstepGraph::Ptr
boost::shared_ptr< FootstepGraph > Ptr
Definition: footstep_graph.h:121
jsk_footstep_planner::FootstepParameters::plane_estimation_min_inliers
int plane_estimation_min_inliers
Definition: footstep_parameters.h:134
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
ros::WallTime::now
static WallTime now()
jsk_footstep_planner::FootstepParameters::support_padding_y
double support_padding_y
Definition: footstep_parameters.h:160
pose_array.p
p
Definition: pose_array.py:21
graph
FootstepGraph::Ptr graph
Definition: footstep_planning_2d_interactive_demo.cpp:51
jsk_footstep_planner::FootstepParameters::support_check_y_sampling
int support_check_y_sampling
Definition: footstep_parameters.h:136
jsk_footstep_planner::FootstepParameters::plane_estimation_normal_opening_angle
double plane_estimation_normal_opening_angle
Definition: footstep_parameters.h:155
jsk_footstep_planner::Graph< FootstepState >::StatePtr
boost::shared_ptr< StateT > StatePtr
Definition: graph.h:115
next
EndPoint * next[3]
jsk_footstep_planner::FootstepParameters::local_move_x_offset
double local_move_x_offset
Definition: footstep_parameters.h:140
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
start_time
start_time
ros::WallTime
jsk_footstep_planner::FootstepGraph::getPointIndicesCollidingSphere
virtual pcl::PointIndices::Ptr getPointIndicesCollidingSphere(const Eigen::Affine3f &c)
Definition: footstep_graph.cpp:139
r
S r
state
state
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::FootstepParameters::support_check_x_sampling
int support_check_x_sampling
Definition: footstep_parameters.h:135
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
footstep_graph.h
jsk_footstep_planner::FootstepParameters::plane_estimation_normal_distance_weight
double plane_estimation_normal_distance_weight
Definition: footstep_parameters.h:154
jsk_footstep_planner::FootstepParameters::support_padding_x
double support_padding_x
Definition: footstep_parameters.h:159
jsk_footstep_planner::FootstepParameters::local_move_x
double local_move_x
Definition: footstep_parameters.h:137
jsk_footstep_planner::FootstepGraph::collision_bbox_offset_
Eigen::Affine3f collision_bbox_offset_
Definition: footstep_graph.h:313
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
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::FootstepParameters::skip_cropping
bool skip_cropping
Definition: footstep_parameters.h:129
index
unsigned int index
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::footstepHeuristicStraight
double footstepHeuristicStraight(SolverNode< FootstepState, FootstepGraph >::Ptr node, FootstepGraph::Ptr graph)
Definition: footstep_graph.cpp:427
jsk_footstep_planner::FootstepParameters::local_move_y_offset
double local_move_y_offset
Definition: footstep_parameters.h:141
jsk_footstep_planner::FootstepParameters::obstacle_resolution
double obstacle_resolution
Definition: footstep_parameters.h:151
jsk_footstep_planner::FootstepGraph::pub_progress_
ros::Publisher pub_progress_
Definition: footstep_graph.h:318
jsk_footstep_planner::Graph< FootstepState >::start_state_
StatePtr start_state_
Definition: graph.h:134
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
jsk_footstep_planner::FootstepGraph::collision_bbox_size_
Eigen::Vector3f collision_bbox_size_
Definition: footstep_graph.h:314
jsk_footstep_planner::projection_state::close_to_success
const unsigned int close_to_success
Definition: footstep_state.h:129
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::FootstepParameters::plane_estimation_outlier_threshold
double plane_estimation_outlier_threshold
Definition: footstep_parameters.h:157
jsk_footstep_planner::FootstepParameters::plane_estimation_use_normal
bool plane_estimation_use_normal
Definition: footstep_parameters.h:128
jsk_footstep_planner::FootstepParameters::local_move_y_num
int local_move_y_num
Definition: footstep_parameters.h:131
ros::Time::now
static Time now()
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