GTSPSolver.cpp

Go to the documentation of this file.

/*
 * GTSPSolver.cpp
 *
 *  Created on: Feb 18, 2012
 *      Author: hess
 */

#include <Eigen/Core>
#include <coverage_3d_planning/GTSPSolver.h>
#include <coverage_3d_planning/TSPSearch.h>
#include <coverage_3d_tools/conversions.h>
GTSPSolver::GTSPSolver() :
    alpha_(0.10) {
    dist_measure_ = "DIST";
    max_adjacency_ = 30000;
}
GTSPSolver::GTSPSolver(double max_adjacency) : alpha_(0.10) {
    dist_measure_ = "DIST";
    max_adjacency_ = max_adjacency;
}

void GTSPSolver::setAlpha(double alpha) {
    alpha_ = alpha;
}

void GTSPSolver::getSolution(std::vector<std::vector<double> >& joint_path, std::vector<tf::Pose>& pose_path,
    std::vector<unsigned int>& action, std::vector<unsigned int>& res_traj) {

    ROS_INFO("GTSPSolver::getSolution transforming graph to gtsp");
    Eigen::VectorXi multinode_mapping;
    Eigen::MatrixXf gtsp_weights;
    Eigen::MatrixXi gtsp_adjacency;
    transformGraphToGTSP(adjacency_, patches_, multinode_mapping, gtsp_adjacency, gtsp_weights);

    ROS_INFO("GTSPSolver::getSolution transforming gtsp to tsp");
    transformGTSPToTSP(multinode_mapping, gtsp_adjacency, gtsp_weights);

    //  //insert start as first point
    Eigen::MatrixXf gtsp_start_weights;
    insertStartIntoWeights(gtsp_weights, multinode_mapping, gtsp_start_weights);

    ROS_INFO("GTSPSolver::getSolution calling TSP solver");
    TSPSearch tsp;
    tsp.writeToTSPFile(gtsp_start_weights);
    tsp.callTSPSolver();
    ROS_DEBUG("GTSPSolver::getSolution reading back TSP result");
    std::vector<unsigned int> traj;
    tsp.readResultTSPFile(traj);

    traj.erase(traj.begin()); // remove first point again
    for (unsigned int i = 0; i < traj.size(); ++i) {
        traj[i] = traj[i] - 1; // correct indexes
    }

    ROS_INFO_STREAM("GTSPSolver::getSolution mapping to the correct patches ");
    std::vector<unsigned int> multinode_traj;
    mapGTSPIndexToMultinode(multinode_mapping, traj, multinode_traj);
    ROS_DEBUG_STREAM("GTSPSolver::getSolution trajectory size " << traj.size() << " multinode traj size " << multinode_traj.size());
    //collect end-effector poses and iksolutions for the trajectory
    std::vector<std::vector<std::vector<double> > > ik_solutions_to_optimize;
    for (unsigned int i = 0; i < multinode_traj.size(); ++i) {
        int curr_node = multinode_traj[i];

        if (i < multinode_traj.size() - 1) {
            int next_node = multinode_traj[i + 1];
            if ((adjacency_(curr_node, next_node) == 1)) {

                //get the number belonging to the edge we need the pose for
                int local_edge_nb = -1; // minus one: e.g. edge 1 is indexed on index 0
                for (int j = 0; j <= next_node; ++j) {
                    local_edge_nb += adjacency_(curr_node, j);
                }
                ik_solutions_to_optimize.push_back(coll_free_ik_[curr_node][local_edge_nb]);
                pose_path.push_back(coll_free_poses_[curr_node][local_edge_nb]);
            } else {
                //if this happens often, than your weight function is weired or the graph not well enough connected
                ROS_WARN("GTSPSolver::getSolution Path planning edge taken --> should usually not happen.");
                std::vector<std::vector<std::vector<double> > > tmp = coll_free_ik_[multinode_traj[i]];
                std::vector<std::vector<double> > tmp_joints;
                for (unsigned int j = 0; j < tmp.size(); ++j) {
                    for (unsigned int k = 0; k < tmp[j].size(); ++k) {
                        tmp_joints.push_back(tmp[j][k]);
                    }
                    //                          tmp_joints[j][5] = ik_solutions_to_optimize[ik_solutions_to_optimize.size() - 1][0][5]; // get the last node -> gripper orientation should be the same for all poses
                }
                ik_solutions_to_optimize.push_back(tmp_joints);
                pose_path.push_back(coll_free_poses_[curr_node][0]); // any edge just that we have a pose
            }
        } else {
            std::vector<std::vector<std::vector<double> > > tmp = coll_free_ik_[multinode_traj[i]];
            std::vector<std::vector<double> > tmp_joints;
            for (unsigned int j = 0; j < tmp.size(); ++j) {
                for (unsigned int k = 0; k < tmp[j].size(); ++k) {
                    tmp_joints.push_back(tmp[j][k]);
                }
                //                              tmp_joints[j][5] = ik_solutions_to_optimize[ik_solutions_to_optimize.size() - 1][0][5]; // get the last node -> gripper orientation should be the same for all poses
            }
            ik_solutions_to_optimize.push_back(tmp_joints);
            pose_path.push_back(coll_free_poses_[curr_node][0]); // any edge just that we have a pose
        }
    }

    //insert the start joint position
    std::vector<std::vector<double> > tmp_joint_start;
    tmp_joint_start.push_back(start_joint_);
    ik_solutions_to_optimize.insert(ik_solutions_to_optimize.begin(), tmp_joint_start);
    ik_solutions_to_optimize.push_back(tmp_joint_start);
   
    //computing the joint space path with dijktra
    std::vector<std::vector<double> > path;
    std::vector<bool> valid_poses(ik_solutions_to_optimize.size(), true);
    traj_planner_.computeDijkstraPath(ik_solutions_to_optimize, valid_poses, path, dist_measure_);
    joint_path = path;
    std::vector<unsigned int> actions;
    getActions(multinode_traj, actions);
        
    //save the result
    res_traj = multinode_traj;
    pose_path.push_back(start_pose_);
    pose_path.insert(pose_path.begin(), start_pose_);

    actions.push_back(1);
    actions.insert(actions.begin(), 1);
    action = actions;

}
void GTSPSolver::transformGraphToGTSP(const Eigen::MatrixXi& adj_bool, const std::vector<SurfacePatch>& patches,
    Eigen::VectorXi& multinode_mapping, Eigen::MatrixXi& new_adj_bool, Eigen::MatrixXf& new_adj_float) {

    // get number of new nodes --> introduce two nodes per edge --> note that
    int n_new_nodes = adj_bool.sum();
    int n_nodes = adj_bool.rows();
    
    //number of edges per node
    Eigen::VectorXi row_sum = adj_bool.rowwise().sum();

    std::vector<SurfacePatch> tmp_patches;
    //get cummulated number of edges --> use later as index
    Eigen::VectorXi row_cumsum = row_sum;
    for (int i = 1; i < row_sum.rows(); ++i) {
        row_cumsum(i) = row_sum(i) + row_cumsum(i - 1);
        for (int j = 0; j < row_sum(i); ++j) {
            tmp_patches.push_back(patches[i]);
        }
    }
    multinode_mapping = row_cumsum;

    //new matrices
    new_adj_bool = Eigen::MatrixXi::Zero(n_new_nodes, n_new_nodes);
    new_adj_float = Eigen::MatrixXf::Ones(n_new_nodes, n_new_nodes) * max_adjacency_; //all edges not computed have high cost --> not connected

    int curr_edge_index = 0;
    float sum_assigned_weights = 0;
    //iterate over all nodes - save the cost
    for (int i = 0; i < n_nodes; ++i) {

        //introduce new node for each edge of the current node
        for (int j = 0; j < n_nodes; ++j) {

            //if there is an edge between node i and node j
            if (adj_bool(i, j) == 1) {

                //get indexes of possible nodes in multinode b where edges could end
                int n2_start = 0;
                if (j != 0) {
                    n2_start = row_cumsum[j - 1];
                }

                //insert the cost
                Eigen::Vector3f edge1 = patches[j].getVector3fMap() - patches[i].getVector3fMap();
                float euclidean_cost = edge1.norm();
                int curr_edge_from_j = 0; //current edge count leading form j to another node
                //iterate over all possible ends
                for (int k = 0; k < n_nodes; ++k) {

                    if (adj_bool(j, k) == 1) {

                        //check where the end is going
                        Eigen::Vector3f edge2 = patches[k].getVector3fMap() - patches[j].getVector3fMap();

                        //if the result of the dot product is smaller -1 due to precision errors
                        float dotproduct = edge1.normalized().dot(edge2.normalized());
                        if (dotproduct < -1.0) {
                            dotproduct = -0.999999;
                        } else if (dotproduct > 1.0) {
                            dotproduct = 0.999999;
                        }
                 
                        //it is the pose difference --> thus we can always use the smaller angle
                        float angular_cost = acos(dotproduct) / M_PI;
                        
                        //compute weight as convex combination of translation and rotation
                        float cost = (1 - alpha_) * euclidean_cost + alpha_ * angular_cost;
                        
                        //contribute to the sum of assigned weights
                        sum_assigned_weights += cost;
                        
                        //save the weigts and adjacency
                        new_adj_float(curr_edge_index, n2_start + curr_edge_from_j) = cost;
                        new_adj_bool(curr_edge_index, n2_start + curr_edge_from_j) = 1;
                        curr_edge_from_j++;
                    }
                }
                curr_edge_index++;
            }
        }
    }

    //    for (unsigned int i = 0; i < new_adj_float.rows(); ++i) { //alle nodes
    //        for (unsigned int j = 0; j < new_adj_float.cols(); ++j) { //alle nodes
    //
    //            if (new_adj_float(i, j) > (max_adjacency_ - 0.001)) {
    //                Eigen::Vector3f edge1 = tmp_patches[j].getVector3fMap() - tmp_patches[i].getVector3fMap();
    //                float cost = edge1.norm();
    //                new_adj_float(i, j) = new_adj_float(i, j) + (1-alpha_)*cost;
    //            }
    //        }
    //    }


    new_adj_float = new_adj_float + (Eigen::MatrixXf::Ones(n_new_nodes, n_new_nodes) * sum_assigned_weights);
    ROS_DEBUG_STREAM("GTSPSolver::transformGraphToGTSP sum_assigned_weights " << sum_assigned_weights << "max_adjacency_ " << max_adjacency_ );
}

void GTSPSolver::transformGTSPToTSP(const Eigen::VectorXi& multinode_mapping, Eigen::MatrixXi& adj_bool,
    Eigen::MatrixXf& adj_float) {

    int n_multinodes = multinode_mapping.rows();
    int n_nodes = adj_bool.rows();

    //iterate over all multinodes
    //assign zero cost to move to successor node in multinode
    for (int i = 0; i < n_multinodes; ++i) {

        //find all nodes in multinode
        int start = 0;
        if (i != 0) {
            start = multinode_mapping(i - 1);
        }
        int end = multinode_mapping(i) - 1;

        int curr_n_nodes = end - start + 1;
        if (curr_n_nodes == 1 || curr_n_nodes == 0) {
           ROS_WARN_STREAM("GTSPSolver::transformGTSPToTSP: failure " << curr_n_nodes << " " << i << " " << multinode_mapping(i - 1) << " "
                << multinode_mapping(i));
            continue;
        }
        Eigen::MatrixXi tmp_bool = Eigen::MatrixXi::Zero(curr_n_nodes, n_nodes);
        Eigen::MatrixXf tmp_float = Eigen::MatrixXf::Zero(curr_n_nodes, n_nodes);

        //shift/rotate the cost --> last line is now first
        tmp_bool.block(curr_n_nodes - 1, 0, 1, n_nodes) = adj_bool.block(start, 0, 1, n_nodes);
        tmp_float.block(curr_n_nodes - 1, 0, 1, n_nodes) = adj_float.block(start, 0, 1, n_nodes);
        tmp_bool.block(0, 0, curr_n_nodes - 1, n_nodes) = adj_bool.block(start + 1, 0, curr_n_nodes - 1, n_nodes);
        tmp_float.block(0, 0, curr_n_nodes - 1, n_nodes) = adj_float.block(start + 1, 0, curr_n_nodes - 1, n_nodes);

        //save back to the original matrix
        adj_bool.block(start, 0, curr_n_nodes, n_nodes) = tmp_bool.block(0, 0, tmp_bool.rows(), tmp_bool.cols());
        adj_float.block(start, 0, curr_n_nodes, n_nodes) = tmp_float;

        for (int j = start; j < end; ++j) {
            adj_bool(j, j + 1) = 1;
            adj_float(j, j + 1) = 0;
        }
        //link from end to start to close the cycle
        adj_bool(end, start) = 1;
        adj_float(end, start) = 0;
    }
}

void GTSPSolver::mapGTSPIndexToMultinode(const Eigen::VectorXi& multinode_mapping,
    const std::vector<unsigned int>& traj, std::vector<unsigned int>& multinode_traj) {
    multinode_traj.clear();
    BOOST_FOREACH(unsigned int i, traj)
                {
                    unsigned int j = 0;
                    while ((int) i >= multinode_mapping(j)) {
                        j++;
                    }

                    if (multinode_traj.size() == 0) {
                        multinode_traj.push_back(j);
                    } else {
                        //check if multinode has already been pushed in the last step
                        if (multinode_traj[multinode_traj.size() - 1] != j) {
                            multinode_traj.push_back(j);
                        }
                    }
                }
}

unsigned int GTSPSolver::mapGTSPIndexToMultinode(const Eigen::VectorXi& multinode_mapping, unsigned int node) {
    unsigned int j = 0;
    while ((int) node >= multinode_mapping(j)) {
        j++;
    }
    return j;
}

void GTSPSolver::getActions(const std::vector<unsigned int>& traj, std::vector<unsigned int>& actions) {

    actions.clear();

    for (unsigned int i = 0; i < traj.size(); ++i) {

        if (i < (traj.size() - 1)) {

            if (adjacency_(traj[i], traj[i + 1]) == 1) {
                actions.push_back(0);
            } else {
                actions.push_back(1);
            }
        } else {
            actions.push_back(1); // we are at the last node - thus use planning to get to the final pose
        }
    }
}
void GTSPSolver::insertStartIntoWeights(const Eigen::MatrixXf& weights, const Eigen::VectorXi& multinode_mapping,
    Eigen::MatrixXf& new_weights) {

    Eigen::Vector3f start = PoseTF2EigenVector3(start_pose_);
    new_weights = Eigen::MatrixXf::Ones(weights.rows() + 1, weights.rows() + 1) * max_adjacency_;
    for (int i = 1; i < new_weights.rows(); ++i) {
        unsigned int multinode_index = mapGTSPIndexToMultinode(multinode_mapping, i - 1);
        Eigen::Vector3f tmp = start - surface_points_[multinode_index].getVector3fMap();
        new_weights(i, 0) = tmp.norm();
        new_weights(0, i) = new_weights(i, 0);
    }

    new_weights.block(1, 1, weights.rows(), weights.cols()) = weights.block(0, 0, weights.rows(), weights.cols());
}

//void testGTSPConstruction() {
//    SurfacePatch p1;
//    p1.x = 0.0;
//    p1.y = 0.0;
//    p1.z = 0.0;
//    SurfacePatch p2;
//    p2.x = 1.1;
//    p2.y = 0.0;
//    p2.z = 0.0;
//    SurfacePatch p3;
//    p3.x = 2.0;
//    p3.y = 0.1;
//    p3.z = 0.0;
//    SurfacePatch p4;
//    p4.x = 1;
//    p4.y = 1;
//    p4.z = 0.0;
//    GTSPSolver gtsp;
//    std::vector<SurfacePatch> patches;
//    patches.push_back(p1);
//    patches.push_back(p2);
//    patches.push_back(p3);
//    patches.push_back(p4);
//
//    Eigen::MatrixXi adj_bool = Eigen::MatrixXi::Ones(4, 4);
//    adj_bool(0, 0) = 0;
//    adj_bool(1, 1) = 0;
//    adj_bool(2, 2) = 0;
//    adj_bool(3, 3) = 0;
//
//    Eigen::VectorXi mapping;
//    Eigen::MatrixXi new_adj_bool;
//    Eigen::MatrixXf new_adj_float;
//    gtsp.transformGraphToGTSP(adj_bool, patches, mapping, new_adj_bool, new_adj_float);
//
//    std::cout << new_adj_bool << std::endl;
//    std::cout << new_adj_float << std::endl;
//
//}
//
//void testGTSPToTSP() {
//
//    GTSPSolver gtsp;
//    Eigen::MatrixXi multinode_mapping = Eigen::MatrixXi::Zero(4, 1);
//    multinode_mapping(0) = 2;
//    multinode_mapping(1) = 3;
//    multinode_mapping(2) = 4;
//    multinode_mapping(3) = 7;
//
//    Eigen::MatrixXi adj_bool = Eigen::MatrixXi::Zero(7, 7);
//    Eigen::MatrixXf adj_float = Eigen::MatrixXf::Ones(7, 7) * 1000;
//
//    adj_bool(2, 0) = 1;
//    adj_bool(3, 0) = 1;
//    adj_bool(4, 0) = 1;
//    adj_bool(5, 0) = 1;
//    adj_bool(6, 0) = 1;
//
//    adj_bool(2, 1) = 1;
//    adj_bool(3, 1) = 1;
//    adj_bool(4, 1) = 1;
//    adj_bool(5, 1) = 1;
//    adj_bool(6, 1) = 1;
//
//    adj_bool(0, 2) = 1;
//    adj_bool(1, 2) = 1;
//    adj_bool(4, 2) = 1;
//    adj_bool(5, 2) = 1;
//    adj_bool(6, 2) = 1;
//
//    adj_bool(0, 3) = 1;
//    adj_bool(1, 3) = 1;
//    adj_bool(4, 3) = 1;
//    adj_bool(5, 3) = 1;
//    adj_bool(6, 3) = 1;
//
//    adj_bool(0, 4) = 1;
//    adj_bool(1, 4) = 1;
//    adj_bool(2, 4) = 1;
//    adj_bool(3, 4) = 1;
//
//    adj_bool(0, 5) = 1;
//    adj_bool(1, 5) = 1;
//    adj_bool(2, 5) = 1;
//    adj_bool(3, 5) = 1;
//
//    adj_bool(0, 6) = 1;
//    adj_bool(1, 6) = 1;
//    adj_bool(2, 6) = 1;
//    adj_bool(3, 6) = 1;
//
//    adj_float(2, 0) = 6;
//    adj_float(3, 0) = 2;
//    adj_float(4, 0) = 9;
//    adj_float(5, 0) = 8;
//    adj_float(6, 0) = 4;
//
//    adj_float(2, 1) = 3;
//    adj_float(3, 1) = 1;
//    adj_float(4, 1) = 7;
//    adj_float(5, 1) = 4;
//    adj_float(6, 1) = 7;
//
//    adj_float(0, 2) = 5;
//    adj_float(1, 2) = 8;
//    adj_float(4, 2) = 9;
//    adj_float(5, 2) = 6;
//    adj_float(6, 2) = 9;
//
//    adj_float(0, 3) = 7;
//    adj_float(1, 3) = 9;
//    adj_float(4, 3) = 3;
//    adj_float(5, 3) = 5;
//    adj_float(6, 3) = 7;
//
//    adj_float(0, 4) = 4;
//    adj_float(1, 4) = 3;
//    adj_float(2, 4) = 4;
//    adj_float(3, 4) = 5;
//
//    adj_float(0, 5) = 6;
//    adj_float(1, 5) = 1;
//    adj_float(2, 5) = 2;
//    adj_float(3, 5) = 6;
//
//    adj_float(0, 6) = 2;
//    adj_float(1, 6) = 5;
//    adj_float(2, 6) = 4;
//    adj_float(3, 6) = 1;
//    std::cout << "org transition matrix \n" << adj_float;
//    gtsp.transformGTSPToTSP(multinode_mapping, adj_bool, adj_float);
//
//    std::cout << "new transition matrix \n" << adj_float;
//}