42 std::vector<Eigen::Affine3f> left_to_right_successors)
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()));
60 double dist = transform.translation()[0];
64 double rot = Eigen::AngleAxisf(transform.rotation()).
angle();
75 jsk_footstep_msgs::FootstepArray
msg;
76 msg.header.frame_id =
"odom";
78 msg.footsteps.push_back(*state->toROSMsg());
81 Eigen::Affine3f
pose = state->getPose();
82 Eigen::Affine3f goal_pose = goal->getPose();
83 Eigen::Affine3f transformation = pose.inverse() * goal_pose;
88 if (state->getLeg() == jsk_footstep_msgs::Footstep::LEFT) {
92 }
else if (state->getLeg() == jsk_footstep_msgs::Footstep::RIGHT) {
103 Eigen::Affine3f mid = current_state->midcoords(*previous_state);
109 pcl::PointXYZ center;
110 center.getVector3fMap() = Eigen::Vector3f(c.translation());
112 pcl::PointIndices::Ptr near_indices(
new pcl::PointIndices);
113 std::vector<float> distances;
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];
126 const Eigen::Vector3f local_p = inv_c * candidate_point.getVector3fMap();
146 Eigen::Affine3f robot_coords =
getRobotCoords(current_state, previous_state);
148 if (sphere_candidate->indices.size() == 0) {
157 if (lastStep->getLeg() == jsk_footstep_msgs::Footstep::LEFT) {
160 }
else if (lastStep->getLeg() == jsk_footstep_msgs::Footstep::RIGHT) {
170 std::stringstream ss;
171 ss <<
"footstep_graph" << std::endl;
182 ss <<
" global_transition_limit: None" << std::endl;
190 ss <<
" resolution: [" 227 return !
isColliding(current_state, previous_state);
232 std::vector<FootstepState::Ptr>
235 std::vector<FootstepState::Ptr> moved_states;
240 if(x_num == 0) x_num = 1;
241 if(y_num == 0) y_num = 1;
242 if(theta_num == 0) theta_num = 1;
248 double offset_y = (in->getLeg() == jsk_footstep_msgs::Footstep::LEFT) ?
252 bool have_offset = ((offset_x != 0.0) || (offset_y != 0.0) || (offset_t != 0.0));
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,
260 * Eigen::AngleAxisf((move_t / theta_num * thetai) + offset_t,
261 Eigen::Vector3f::UnitZ()));
262 moved_states.push_back(
264 in->getPose() * trans,
266 in->getResolution())));
276 std::vector<Eigen::Affine3f> transformations;
278 if (target_state->getLeg() == jsk_footstep_msgs::Footstep::LEFT) {
280 next_leg = jsk_footstep_msgs::Footstep::RIGHT;
282 else if (target_state->getLeg() == jsk_footstep_msgs::Footstep::RIGHT) {
284 next_leg = jsk_footstep_msgs::Footstep::LEFT;
291 Eigen::Affine3f base_pose = target_state->getPose();
292 for (
size_t i = 0; i < transformations.size(); i++) {
293 Eigen::Affine3f transform = transformations[i];
295 base_pose * transform,
296 target_state->getDimensions(),
300 unsigned int error_state;
304 for (
size_t j = 0; j < locally_moved_nodes.size(); j++) {
326 unsigned int error_state;
331 unsigned int& error_state)
343 Eigen::Vector3f(0, 0, 1),
347 return projected_footstep;
352 unsigned int error_state;
355 if (left_projected && right_projected) {
374 unsigned int error_state;
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()));
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());
419 double first_rotation_weight,
420 double second_rotation_weight)
424 Eigen::Vector3f goal_pos(goal->getPose().translation());
425 Eigen::Vector3f diff_pos(goal_pos - state->getPose().translation());
427 Eigen::Quaternionf first_rot;
429 first_rot.setFromTwoVectors(state->getPose().matrix().block<3, 3>(0, 0) * Eigen::Vector3f::UnitX(),
430 diff_pos.normalized());
432 Eigen::Quaternionf second_rot;
433 second_rot.setFromTwoVectors(diff_pos.normalized(),
434 goal->getPose().matrix().block<3, 3>(0, 0) * Eigen::Vector3f::UnitX());
436 double first_theta =
acos(first_rot.w()) * 2;
437 double second_theta =
acos(second_rot.w()) * 2;
438 if (isnan(first_theta)) {
441 if (isnan(second_theta)) {
445 if (first_theta >
M_PI) {
446 first_theta = 2.0 *
M_PI - first_theta;
448 if (second_theta >
M_PI) {
449 second_theta = 2.0 *
M_PI - second_theta;
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();
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;
469 Eigen::Vector3f
p(0, 0.1, 0);
470 state_mid_pos = state->getPose() *
p;
472 double dist, to_goal, alp;
474 Eigen::Vector3f foot;
475 dist = graph->heuristic_path_->distanceWithInfo(state_mid_pos, foot, to_goal, idx, alp);
478 Eigen::Vector3f dir = graph->heuristic_path_->getDirection(idx);
480 Eigen::Quaternionf path_foot_rot;
481 path_foot_rot.setFromTwoVectors(state->getPose().matrix().block<3, 3>(0, 0) * Eigen::Vector3f::UnitX(),
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;
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();
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;
501 double goal_foot_rot_cost = goal_foot_theta / graph->maxSuccessorRotation();
504 return 2*(step_cost + follow_cost + (0.5 * path_foot_rot_cost));
void publish(const boost::shared_ptr< M > &message) const
INLINE Rall1d< T, V, S > acos(const Rall1d< T, V, S > &x)