footstep_graph.cpp
Go to the documentation of this file.
00001 // -*- mode: c++ -*-
00002 /*********************************************************************
00003  * Software License Agreement (BSD License)
00004  *
00005  *  Copyright (c) 2015, JSK Lab
00006  *  All rights reserved.
00007  *
00008  *  Redistribution and use in source and binary forms, with or without
00009  *  modification, are permitted provided that the following conditions
00010  *  are met:
00011  *
00012  *   * Redistributions of source code must retain the above copyright
00013  *     notice, this list of conditions and the following disclaimer.
00014  *   * Redistributions in binary form must reproduce the above
00015  *     copyright notice, this list of conditions and the following
00016  *     disclaimer in the documentation and/o2r other materials provided
00017  *     with the distribution.
00018  *   * Neither the name of the JSK Lab nor the names of its
00019  *     contributors may be used to endorse or promote products derived
00020  *     from this software without specific prior written permission.
00021  *
00022  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00023  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00024  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00025  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00026  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00027  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00028  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00029  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00030  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00031  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00032  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00033  *  POSSIBILITY OF SUCH DAMAGE.
00034  *********************************************************************/
00035 
00036 #include "jsk_footstep_planner/footstep_graph.h"
00037 #include <sstream>
00038 
00039 namespace jsk_footstep_planner
00040 {
00041   void FootstepGraph::setBasicSuccessors(
00042     std::vector<Eigen::Affine3f> left_to_right_successors)
00043   {
00044     successors_from_left_to_right_ = left_to_right_successors;
00045     for (size_t i = 0; i < successors_from_left_to_right_.size(); i++) {
00046       Eigen::Affine3f transform = successors_from_left_to_right_[i];
00047       float roll, pitch, yaw;
00048       pcl::getEulerAngles(transform, roll, pitch, yaw);
00049       Eigen::Vector3f translation = transform.translation();
00050       Eigen::Affine3f flipped_transform
00051         = Eigen::Translation3f(translation[0], -translation[1], translation[2])
00052         * Eigen::Quaternionf(Eigen::AngleAxisf(-yaw, Eigen::Vector3f::UnitZ()));
00053       successors_from_right_to_left_.push_back(flipped_transform);
00054     }
00055 
00056     // max_successor_distance_
00057     for (size_t i = 0; i < successors_from_left_to_right_.size(); i++) {
00058       Eigen::Affine3f transform = successors_from_left_to_right_[i];
00059       //double dist = transform.translation().norm();
00060       double dist = transform.translation()[0]; // Only consider x
00061       if (dist > max_successor_distance_) {
00062         max_successor_distance_ = dist;
00063       }
00064       double rot = Eigen::AngleAxisf(transform.rotation()).angle();
00065       if (rot > max_successor_rotation_) {
00066         max_successor_rotation_ = rot;
00067       }
00068     }
00069   }
00070 
00071   bool FootstepGraph::isGoal(StatePtr state)
00072   {
00073     FootstepState::Ptr goal = getGoal(state->getLeg());
00074     if (publish_progress_) {
00075       jsk_footstep_msgs::FootstepArray msg;
00076       msg.header.frame_id = "odom"; // TODO fixed frame_id
00077       msg.header.stamp = ros::Time::now();
00078       msg.footsteps.push_back(*state->toROSMsg());
00079       pub_progress_.publish(msg);
00080     }
00081     Eigen::Affine3f pose = state->getPose();
00082     Eigen::Affine3f goal_pose = goal->getPose();
00083     Eigen::Affine3f transformation = pose.inverse() * goal_pose;
00084 
00085     if ((parameters_.goal_pos_thr > transformation.translation().norm()) &&
00086         (parameters_.goal_rot_thr > std::abs(Eigen::AngleAxisf(transformation.rotation()).angle()))) {
00087       // check collision
00088       if (state->getLeg() == jsk_footstep_msgs::Footstep::LEFT) {
00089         if (right_goal_state_->crossCheck(state)) {
00090           return true;
00091         }
00092       } else if (state->getLeg() == jsk_footstep_msgs::Footstep::RIGHT) {
00093         if (left_goal_state_->crossCheck(state)) {
00094           return true;
00095         }
00096       }
00097     }
00098     return false;
00099   }
00100   
00101   Eigen::Affine3f FootstepGraph::getRobotCoords(StatePtr current_state, StatePtr previous_state) const
00102   {
00103     Eigen::Affine3f mid = current_state->midcoords(*previous_state);
00104     return mid * collision_bbox_offset_;
00105   }
00106 
00107   pcl::PointIndices::Ptr FootstepGraph::getPointIndicesCollidingSphere(const Eigen::Affine3f& c)
00108   {
00109     pcl::PointXYZ center;
00110     center.getVector3fMap() = Eigen::Vector3f(c.translation());
00111     const double r = collision_bbox_size_.norm() / 2 + parameters_.obstacle_resolution;
00112     pcl::PointIndices::Ptr near_indices(new pcl::PointIndices);
00113     std::vector<float> distances;
00114     obstacle_tree_model_->radiusSearch(center, r, near_indices->indices, distances);
00115     return near_indices;
00116   }
00117 
00118   bool FootstepGraph::isCollidingBox(const Eigen::Affine3f& c, pcl::PointIndices::Ptr candidates) const
00119   {
00120     const pcl::PointCloud<pcl::PointXYZ>::ConstPtr input_cloud = obstacle_tree_model_->getInputCloud();
00121     Eigen::Affine3f inv_c = c.inverse();
00122     for (size_t i = 0; i < candidates->indices.size(); i++) {
00123       int index = candidates->indices[i];
00124       const pcl::PointXYZ candidate_point = input_cloud->points[index];
00125       // convert candidate_point into `c' local representation.
00126       const Eigen::Vector3f local_p = inv_c * candidate_point.getVector3fMap();
00127       if (std::abs(local_p[0]) < collision_bbox_size_[0] / 2 &&
00128           std::abs(local_p[1]) < collision_bbox_size_[1] / 2 &&
00129           std::abs(local_p[2]) < collision_bbox_size_[2] / 2) {
00130         return true;
00131       }
00132     }
00133     return false;
00134   }
00135                                        
00136   // return true if colliding with obstacle
00137   bool FootstepGraph::isColliding(StatePtr current_state, StatePtr previous_state)
00138   {
00139     // if not use obstacle model, always return false
00140     // if use obstacle model and obstacle_model_ point cloud size is zero, always return false
00141     // => to be collision-free always.
00142     if (!use_obstacle_model_ || (obstacle_model_->size() == 0) ) {
00143       return false;
00144     }
00145     // compute robot coorde
00146     Eigen::Affine3f robot_coords = getRobotCoords(current_state, previous_state);
00147     pcl::PointIndices::Ptr sphere_candidate = getPointIndicesCollidingSphere(robot_coords);
00148     if (sphere_candidate->indices.size() == 0) {
00149       return false;
00150     }
00151     return isCollidingBox(robot_coords, sphere_candidate);
00152   }
00153 
00154   bool FootstepGraph::finalizeSteps(const StatePtr &last_1_Step, const StatePtr &lastStep,
00155                                     std::vector<StatePtr> &finalizeSteps) {
00156     // simple finalize (no check)
00157     if (lastStep->getLeg() == jsk_footstep_msgs::Footstep::LEFT) {
00158       finalizeSteps.push_back(right_goal_state_);
00159       finalizeSteps.push_back(left_goal_state_);
00160     } else if (lastStep->getLeg() == jsk_footstep_msgs::Footstep::RIGHT) {
00161       finalizeSteps.push_back(left_goal_state_);
00162       finalizeSteps.push_back(right_goal_state_);
00163     }
00164 
00165     return true;
00166   }
00167 
00168   std::string FootstepGraph::infoString() const
00169   {
00170     std::stringstream ss;
00171     ss << "footstep_graph" << std::endl;
00172     ss << "  goal_pos_thr: " << parameters_.goal_pos_thr << std::endl;
00173     ss << "  goal_rot_thr: " << parameters_.goal_rot_thr << std::endl;
00174     ss << "  use_pointcloud_model: " << use_pointcloud_model_ << std::endl;
00175     ss << "  lazy_projection: " << lazy_projection_ << std::endl;
00176     ss << "  local_movement: " << local_movement_ << std::endl;
00177     ss << "  transition_limit: " << transition_limit_ << std::endl;
00178     if (global_transition_limit_) {
00179       ss << "  global_transition_limit: " << global_transition_limit_ << std::endl;
00180     }
00181     else {
00182       ss << "  global_transition_limit: None" << std::endl;
00183     }
00184     ss << "  local_move_x: " << parameters_.local_move_x << std::endl;
00185     ss << "  local_move_y: " << parameters_.local_move_y << std::endl;
00186     ss << "  local_move_theta: " << parameters_.local_move_theta << std::endl;
00187     ss << "  local_move_x_num: " << parameters_.local_move_x_num << std::endl;
00188     ss << "  local_move_y_num: " << parameters_.local_move_y_num << std::endl;
00189     ss << "  local_move_theta_num: " << parameters_.local_move_theta_num << std::endl;
00190     ss << "  resolution: ["
00191        << resolution_[0] << ", "
00192        << resolution_[1] << ", "
00193        << resolution_[2] << "]"
00194        << std::endl;
00195     ss << "  plane_estimation_use_normal: "             << parameters_.plane_estimation_use_normal             << std::endl;
00196     ss << "  plane_estimation_normal_distance_weight: " << parameters_.plane_estimation_normal_distance_weight << std::endl;
00197     ss << "  plane_estimation_normal_opening_angle: "   << parameters_.plane_estimation_normal_opening_angle   << std::endl;
00198     ss << "  plane_estimation_min_ratio_of_inliers: "   << parameters_.plane_estimation_min_ratio_of_inliers   << std::endl;
00199     ss << "  plane_estimation_max_iterations: " << parameters_.plane_estimation_max_iterations << std::endl;
00200     ss << "  plane_estimation_min_inliers: " << parameters_.plane_estimation_min_inliers << std::endl;
00201     ss << "  plane_estimation_outlier_threshold: " << parameters_.plane_estimation_outlier_threshold << std::endl;
00202 
00203     ss << "  support_check_x_sampling: " << parameters_.support_check_x_sampling << std::endl;
00204     ss << "  support_check_y_sampling: " << parameters_.support_check_y_sampling << std::endl;
00205     ss << "  support_check_vertex_neighbor_threshold: " << parameters_.support_check_vertex_neighbor_threshold << std::endl;
00206     ss << "  support_padding_x: " << parameters_.support_padding_x << std::endl;
00207     ss << "  support_padding_y: " << parameters_.support_padding_y << std::endl;
00208     ss << "  skip_cropping: " << parameters_.skip_cropping << std::endl;
00209     
00210     return ss.str();
00211   }
00212 
00213   bool
00214   FootstepGraph::isSuccessable(StatePtr current_state, StatePtr previous_state)
00215   {
00216     if (global_transition_limit_) {
00217       if (!global_transition_limit_->check(zero_state_, current_state)) {
00218         return false;
00219       }
00220     }
00221     if (transition_limit_) {
00222       if (!transition_limit_->check(previous_state, current_state)) {
00223         return false;
00224       }
00225     }
00226     if (use_obstacle_model_) {
00227       return !isColliding(current_state, previous_state);
00228     }
00229     return true;
00230   }
00231   
00232   std::vector<FootstepState::Ptr>
00233   FootstepGraph::localMoveFootstepState(FootstepState::Ptr in)
00234   {
00235     std::vector<FootstepState::Ptr> moved_states;
00236     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);
00237     int x_num     = parameters_.local_move_x_num;
00238     int y_num     = parameters_.local_move_y_num;
00239     int theta_num = parameters_.local_move_theta_num;
00240     if(x_num == 0)     x_num = 1;
00241     if(y_num == 0)     y_num = 1;
00242     if(theta_num == 0) theta_num = 1;
00243 
00244     double move_x = parameters_.local_move_x;
00245     double move_y = parameters_.local_move_y;
00246     double move_t = parameters_.local_move_theta;
00247     double offset_x = parameters_.local_move_x_offset;
00248     double offset_y = (in->getLeg() == jsk_footstep_msgs::Footstep::LEFT) ?
00249       parameters_.local_move_y_offset : - parameters_.local_move_y_offset;
00250     double offset_t = parameters_.local_move_theta_offset;
00251 
00252     bool have_offset = ((offset_x != 0.0) || (offset_y != 0.0) || (offset_t != 0.0));
00253     for (int xi = - parameters_.local_move_x_num; xi <= parameters_.local_move_x_num; xi++) {
00254       for (int yi = - parameters_.local_move_y_num; yi <= parameters_.local_move_y_num; yi++) {
00255         for (int thetai = - parameters_.local_move_theta_num; thetai <= parameters_.local_move_theta_num; thetai++) {
00256           if ( have_offset || (xi != 0) || (yi != 0) || (thetai != 0) ) {
00257             Eigen::Affine3f trans(Eigen::Translation3f((move_x / x_num * xi) + offset_x,
00258                                                        (move_y / y_num * yi) + offset_y,
00259                                                        0)
00260                                   * Eigen::AngleAxisf((move_t / theta_num * thetai) + offset_t,
00261                                                       Eigen::Vector3f::UnitZ()));
00262             moved_states.push_back(
00263                                    FootstepState::Ptr(new FootstepState(in->getLeg(),
00264                                                                         in->getPose() * trans,
00265                                                                         in->getDimensions(),
00266                                                                         in->getResolution())));
00267           }
00268         }
00269       }
00270     }
00271     return moved_states;
00272   }
00273   
00274   bool FootstepGraph::successors_original(StatePtr target_state, std::vector<FootstepGraph::StatePtr> &ret)
00275   {
00276     std::vector<Eigen::Affine3f> transformations;
00277     int next_leg;
00278     if (target_state->getLeg() == jsk_footstep_msgs::Footstep::LEFT) {
00279       transformations = successors_from_left_to_right_;
00280       next_leg = jsk_footstep_msgs::Footstep::RIGHT;
00281     }
00282     else if (target_state->getLeg() == jsk_footstep_msgs::Footstep::RIGHT) {
00283       transformations = successors_from_right_to_left_;
00284       next_leg = jsk_footstep_msgs::Footstep::LEFT;
00285     }
00286     else {
00287       // TODO: error
00288     }
00289 
00290     //std::vector<FootstepGraph::StatePtr> ret;
00291     Eigen::Affine3f base_pose = target_state->getPose();
00292     for (size_t i = 0; i < transformations.size(); i++) {
00293       Eigen::Affine3f transform = transformations[i];
00294       FootstepGraph::StatePtr next(new FootstepState(next_leg,
00295                                                      base_pose * transform,
00296                                                      target_state->getDimensions(),
00297                                                      resolution_));
00298       if (use_pointcloud_model_ && !lazy_projection_) {
00299         // Update footstep position by projection
00300         unsigned int error_state;
00301         FootstepGraph::StatePtr tmpnext = projectFootstep(next, error_state);
00302         if (!tmpnext && localMovement() && error_state == projection_state::close_to_success) {
00303           std::vector<StatePtr> locally_moved_nodes = localMoveFootstepState(next);
00304           for (size_t j = 0; j < locally_moved_nodes.size(); j++) {
00305             if (isSuccessable(locally_moved_nodes[j], target_state)) {
00306               FootstepGraph::StatePtr tmp = projectFootstep(locally_moved_nodes[j], error_state);
00307               if(!!tmp) {
00308                 ret.push_back(tmp);
00309               }
00310             }
00311           }
00312         }
00313         next = tmpnext;
00314       }
00315       if (!!next) {
00316         if (isSuccessable(next, target_state)) {
00317           ret.push_back(next);
00318         }
00319       }
00320     }
00321     return true;
00322   }
00323 
00324   FootstepState::Ptr FootstepGraph::projectFootstep(FootstepState::Ptr in)
00325   {
00326     unsigned int error_state;
00327     return projectFootstep(in, error_state);
00328   }
00329   
00330   FootstepState::Ptr FootstepGraph::projectFootstep(FootstepState::Ptr in,
00331                                                     unsigned int& error_state)
00332   {
00333     if(!use_pointcloud_model_) {
00334       return in;
00335     }
00336     ros::WallTime start_time = ros::WallTime::now();
00337     FootstepState::Ptr projected_footstep = in->projectToCloud(
00338       *tree_model_,
00339       pointcloud_model_,
00340       grid_search_,
00341       *tree_model_2d_,
00342       pointcloud_model_2d_,
00343       Eigen::Vector3f(0, 0, 1),
00344       error_state, parameters_);
00345     ros::WallTime end_time = ros::WallTime::now();
00346     perception_duration_ = perception_duration_ + (end_time  - start_time);
00347     return projected_footstep;
00348   }
00349   
00350   bool FootstepGraph::projectGoal()
00351   {
00352     unsigned int error_state;
00353     FootstepState::Ptr left_projected = projectFootstep(left_goal_state_);
00354     FootstepState::Ptr right_projected = projectFootstep(right_goal_state_);
00355     if (left_projected && right_projected) {
00356       if (global_transition_limit_) {
00357         if (!global_transition_limit_->check(zero_state_, left_projected) ||
00358             !global_transition_limit_->check(zero_state_, right_projected)) {
00359           return false;
00360         }
00361       }
00362 
00363       left_goal_state_ = left_projected;
00364       right_goal_state_ = right_projected;
00365       return true;
00366     }
00367     else {
00368       return false;
00369     }
00370   }
00371   
00372   bool FootstepGraph::projectStart()
00373   {
00374     unsigned int error_state;
00375     FootstepState::Ptr projected = projectFootstep(start_state_);
00376     if (global_transition_limit_) {
00377       if (!global_transition_limit_->check(zero_state_, projected)) {
00378         return false;
00379       }
00380     }
00381     if (projected) {
00382       start_state_ = projected;
00383       return true;
00384     }
00385     return false;
00386   }
00387 
00388   double footstepHeuristicZero(
00389     SolverNode<FootstepState, FootstepGraph>::Ptr node, FootstepGraph::Ptr graph)
00390   {
00391     // best-first search
00392     return 0;
00393   }
00394   
00395   double footstepHeuristicStraight(
00396     SolverNode<FootstepState, FootstepGraph>::Ptr node, FootstepGraph::Ptr graph)
00397   {
00398     // distance_to_goal / maxSuccessor
00399     FootstepState::Ptr state = node->getState();
00400     FootstepState::Ptr goal = graph->getGoal(state->getLeg());
00401     Eigen::Vector3f state_pos(state->getPose().translation());
00402     Eigen::Vector3f goal_pos(goal->getPose().translation());
00403     return (std::abs((state_pos - goal_pos).norm() / graph->maxSuccessorDistance()));
00404   }
00405   
00406   double footstepHeuristicStraightRotation(
00407     SolverNode<FootstepState, FootstepGraph>::Ptr node, FootstepGraph::Ptr graph)
00408   {
00409     // distance_to_goal / maxSuccessor.trans + rotation_to_goal / maxSuccessor.rot
00410     FootstepState::Ptr state = node->getState();
00411     FootstepState::Ptr goal = graph->getGoal(state->getLeg());
00412     Eigen::Affine3f transform = state->getPose().inverse() * goal->getPose();
00413     return (std::abs(transform.translation().norm() / graph->maxSuccessorDistance()) +
00414                std::abs(Eigen::AngleAxisf(transform.rotation()).angle()) / graph->maxSuccessorRotation());
00415   }
00416 
00417   double footstepHeuristicStepCost(
00418     SolverNode<FootstepState, FootstepGraph>::Ptr node, FootstepGraph::Ptr graph,
00419     double first_rotation_weight,
00420     double second_rotation_weight)
00421   {
00422     FootstepState::Ptr state = node->getState();
00423     FootstepState::Ptr goal = graph->getGoal(state->getLeg());
00424     Eigen::Vector3f goal_pos(goal->getPose().translation());
00425     Eigen::Vector3f diff_pos(goal_pos - state->getPose().translation());
00426     diff_pos[2] = 0.0;          // Ignore z distance
00427     Eigen::Quaternionf first_rot;
00428     // Eigen::Affine3f::rotation is too slow because it calls SVD decomposition
00429     first_rot.setFromTwoVectors(state->getPose().matrix().block<3, 3>(0, 0) * Eigen::Vector3f::UnitX(),
00430                                 diff_pos.normalized());
00431 
00432     Eigen::Quaternionf second_rot;
00433     second_rot.setFromTwoVectors(diff_pos.normalized(),
00434                                  goal->getPose().matrix().block<3, 3>(0, 0) * Eigen::Vector3f::UnitX());
00435     // is it correct??
00436     double first_theta = acos(first_rot.w()) * 2;
00437     double second_theta = acos(second_rot.w()) * 2;
00438     if (isnan(first_theta)) {
00439       first_theta = 0;
00440     }
00441     if (isnan(second_theta)) {
00442       second_theta = 0;
00443     }
00444     // acos := [0, M_PI]
00445     if (first_theta > M_PI) {
00446       first_theta = 2.0 * M_PI - first_theta;
00447     }
00448     if (second_theta > M_PI) {
00449       second_theta = 2.0 * M_PI - second_theta;
00450     }
00451     //return (Eigen::Vector2f(diff_pos[0], diff_pos[1]).norm() / graph->maxSuccessorDistance()) + std::abs(diff_pos[2]) / 0.2 +
00452     return (diff_pos.norm() / graph->maxSuccessorDistance()) + std::abs(diff_pos[2]) / 0.2 + 
00453       (first_theta * first_rotation_weight + second_theta * second_rotation_weight) / graph->maxSuccessorRotation();
00454   }
00455 
00456   double footstepHeuristicFollowPathLine(
00457     SolverNode<FootstepState, FootstepGraph>::Ptr node, FootstepGraph::Ptr graph)
00458   {
00459     FootstepState::Ptr state = node->getState();
00460     FootstepState::Ptr goal = graph->getGoal(state->getLeg());
00461 
00462     Eigen::Vector3f goal_pos(goal->getPose().translation());
00463     Eigen::Vector3f state_pos(state->getPose().translation());
00464     Eigen::Vector3f state_mid_pos;
00465     if (state->getLeg() == jsk_footstep_msgs::Footstep::LEFT) {
00466       Eigen::Vector3f p(0, -0.1, 0);
00467       state_mid_pos = state->getPose() * p;
00468     } else { // Right
00469       Eigen::Vector3f p(0, 0.1, 0);
00470       state_mid_pos = state->getPose() * p;
00471     }
00472     double dist, to_goal, alp;
00473     int idx;
00474     Eigen::Vector3f foot;
00475     dist = graph->heuristic_path_->distanceWithInfo(state_mid_pos, foot, to_goal, idx, alp);
00476 
00477     //jsk_recognition_utils::FiniteLine::Ptr ln = graph->heuristic_path_->at(idx);
00478     Eigen::Vector3f dir = graph->heuristic_path_->getDirection(idx);
00479 
00480     Eigen::Quaternionf path_foot_rot;
00481     path_foot_rot.setFromTwoVectors(state->getPose().matrix().block<3, 3>(0, 0) * Eigen::Vector3f::UnitX(),
00482                                     dir);
00483     double path_foot_theta = acos(path_foot_rot.w()) * 2;
00484     if (path_foot_theta > M_PI) {
00485       path_foot_theta = 2.0 * M_PI - path_foot_theta;
00486       // foot_theta : [0, M_PI]
00487     }
00488 
00489     double step_cost = to_goal / graph->maxSuccessorDistance();
00490     double follow_cost = dist / 0.02; // ???
00491     double path_foot_rot_cost = path_foot_theta / graph->maxSuccessorRotation();
00492 
00493     Eigen::Vector3f goal_diff = goal_pos - state_pos;
00494     Eigen::Quaternionf goal_foot_rot;
00495     goal_foot_rot.setFromTwoVectors(state->getPose().matrix().block<3, 3>(0, 0) * Eigen::Vector3f::UnitX(),
00496                                     goal->getPose().matrix().block<3, 3>(0, 0) * Eigen::Vector3f::UnitX());
00497     double goal_foot_theta = acos(goal_foot_rot.w()) * 2;
00498     if (goal_foot_theta > M_PI) {
00499       goal_foot_theta = 2.0 * M_PI - goal_foot_theta;
00500     }
00501     double goal_foot_rot_cost = goal_foot_theta / graph->maxSuccessorRotation();
00502 
00503     //return step_cost + follow_cost + (4.0 * goal_foot_rot_cost) + (0.5 * path_foot_rot_cost);
00504     return 2*(step_cost + follow_cost + (0.5 * path_foot_rot_cost));
00505   }
00506 }


jsk_footstep_planner
Author(s): Ryohei Ueda
autogenerated on Wed Jul 19 2017 02:54:28