JointGTSPSolver.cpp

Go to the documentation of this file.

#include <Eigen/Core>
#include <coverage_3d_planning/JointGTSPSolver.h>
#include <coverage_3d_planning/TSPSearch.h>
#include <coverage_3d_tools/conversions.h>
JointGTSPSolver::JointGTSPSolver() {
    dist_measure_ = "DIST";
    max_adjacency_ = 500000;
}

JointGTSPSolver::JointGTSPSolver(double max_adjacency, std::string tsp_type) {
    dist_measure_ = tsp_type;
    max_adjacency_ = max_adjacency;

}

void JointGTSPSolver::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("JointGTSPSolver: transforming graph to gtsp");
    Eigen::VectorXi multinode_mapping;
    Eigen::MatrixXf gtsp_weights;
    Eigen::MatrixXi gtsp_adjacency;
    transformGraphToGTSP(adjacency_, patches_, coll_free_ik_, multinode_mapping, gtsp_adjacency, gtsp_weights);

    ROS_INFO("JointGTSPSolver: transforming gtsp to tsp");
    transformGTSPToTSP(multinode_mapping, gtsp_adjacency, gtsp_weights);
    ROS_INFO_STREAM(
        "JointGTSPSolver: conversion result " << gtsp_adjacency.rows() << " " << gtsp_adjacency.cols() << " " << gtsp_weights.rows() << " " << gtsp_weights.cols());

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

    ROS_INFO("JointGTSPSolver: searching tsp solution ");
    TSPSearch tsp;
    tsp.writeToTSPFile(gtsp_start_weights);
    tsp.callTSPSolver();
    ROS_INFO("JointGTSPSolver: reading back the 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("JointGTSPSolver: mapping solution to the correct patches ");
    std::vector<unsigned int> multinode_traj;
    mapGTSPIndexToMultinode(multinode_mapping, traj, multinode_traj);

    //collect end-effector poses
    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);
                }
                pose_path.push_back(coll_free_poses_[curr_node][local_edge_nb]);
            } else {
                ROS_WARN("JointGTSPSolver::getSolution Path planning edge taken --> should usually not happen.");
                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]);
                }
            }
            pose_path.push_back(coll_free_poses_[curr_node][0]); // any edge just that we have a pose
        }
    }
    std::vector<std::vector<double> > path;
    ROS_INFO_STREAM(
        "JointGTSPSolver: size of adjacency matrix" << gtsp_adjacency.rows() << " " << gtsp_adjacency.cols() << " " << traj.size());
    getJointPoses(multinode_mapping, traj, path);
    path.insert(path.begin(), start_joint_);
    path.push_back(start_joint_);
    joint_path = path;

    std::vector<unsigned int> actions;
    getActions(multinode_traj, actions);
    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 JointGTSPSolver::transformGraphToGTSP(const Eigen::MatrixXi& adj_bool, const std::vector<SurfacePatch>& patches,
    const std::vector<std::vector<std::vector<std::vector<double> > > >& coll_free_ik,
    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 each edge is represented twice in the adjacency matrix
    int n_nodes = adj_bool.rows();

    std::vector<std::vector<std::vector<double> > > tmp_ik;
    tmp_ik.resize(n_nodes);
    Eigen::VectorXi row_sum = Eigen::VectorXi::Zero(n_nodes);
    int n_new_nodes = 0;
    for (unsigned int i = 0; i < coll_free_ik.size(); ++i) { //alle nodes
        for (unsigned int j = 0; j < coll_free_ik[i].size(); ++j) { //alle kanten eines node
            for (unsigned int k = 0; k < coll_free_ik[i][j].size(); ++k) {
                tmp_ik[i].push_back(coll_free_ik[i][j][k]);
                n_new_nodes++;
            }
        }
        row_sum[i] = tmp_ik[i].size();
    }

    //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);
    }
    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

    float sum_assigned_weights = 0;
    int curr_edge_index = 0;

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

        int local_edge_nb = -1;
        //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) {

                local_edge_nb++;
                //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];
                }

                //iterate over all possible ik configurations from patch i edge local_edge_nb
                for (unsigned int k = 0; k < coll_free_ik[i][local_edge_nb].size(); ++k) {
                    //link to all configurations in patch j
                    for (unsigned int l = 0; l < tmp_ik[j].size(); ++l) {

                        float cost = 0;
                        if (dist_measure_ == "DIST") {
                            cost = traj_planner_.absJointConfigurationDist(coll_free_ik[i][local_edge_nb][k],
                                tmp_ik[j][l]);
                        } else if (dist_measure_ == "TIME") {
                            cost = traj_planner_.jointConfigurationMinTravelTime(coll_free_ik[i][local_edge_nb][k],
                                tmp_ik[j][l]);
                        } else {
                            ROS_ERROR("JointGTSPSolver::transformGraphToGTSP Unknown TSP version");
                        }

                        sum_assigned_weights = sum_assigned_weights + cost;
                        new_adj_float(curr_edge_index, n2_start + l) = cost;
                        new_adj_bool(curr_edge_index, n2_start + l) = 1;

                    }
                    curr_edge_index++;
                }
            }
        }
    }

    //    //    //do all this for short jumps
    //    std::vector<std::vector<double> > tmp_ik2;
    //    for (unsigned int i = 0; i < coll_free_ik_.size(); ++i) {
    //        for (unsigned int j = 0; j < coll_free_ik_[i].size(); ++j) {
    //            for (unsigned int k = 0; k < coll_free_ik_[i][j].size(); ++k) {
    //                tmp_ik2.push_back(coll_free_ik_[i][j][k]);
    //            }
    //        }
    //    }
    //    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.0000001)) {
    //
    //                float cost = 0;
    //                if (dist_measure_ == "DIST") {
    //                    cost = traj_planner_.absJointConfigurationDist(tmp_ik2[i], tmp_ik2[j]);
    //                } else if (dist_measure_ == "TIME") {
    //                    cost = traj_planner_.jointConfigurationMinTravelTime(tmp_ik2[i], tmp_ik2[j]);
    //                } else {
    //                    ROS_ERROR("Unknown TSP version");
    //                }
    //                new_adj_float(i, j) = new_adj_float(i, j) + cost;
    //            }
    //        }
    //    }

    new_adj_float = new_adj_float + (Eigen::MatrixXf::Ones(n_new_nodes, n_new_nodes) * sum_assigned_weights);
}

void JointGTSPSolver::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(
                "JointGTSPSolver::transformGTSPToTSP  failure " << curr_n_nodes << " " << i << " " << start << " " << 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 JointGTSPSolver::mapGTSPIndexToMultinode(const Eigen::VectorXi& multinode_mapping,
    const std::vector<unsigned int>& traj, std::vector<unsigned int>& multinode_traj) {
    multinode_traj.clear();
    int counter = 0;
    BOOST_FOREACH(unsigned int i, traj) {
        counter++;
        unsigned int j = 0;

        while (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 JointGTSPSolver::mapGTSPIndexToMultinode(const Eigen::VectorXi& multinode_mapping, unsigned int node) {
    unsigned int j = 0;
    while (node >= multinode_mapping(j)) {
        j++;
    }
    return j;
}

void JointGTSPSolver::getJointPoses(const Eigen::VectorXi& multinode_mapping, const std::vector<unsigned int>& traj,
    std::vector<std::vector<double> >& joint_path) {
    joint_path.clear();
    std::vector<std::vector<double> > tmp_ik;
    for (unsigned int i = 0; i < coll_free_ik_.size(); ++i) {
        for (unsigned int j = 0; j < coll_free_ik_[i].size(); ++j) {
            for (unsigned int k = 0; k < coll_free_ik_[i][j].size(); ++k) {
                tmp_ik.push_back(coll_free_ik_[i][j][k]);
            }
        }
    }

    for (unsigned int i = 0; i < traj.size() - 1; ++i) {
        int curr_multinode = mapGTSPIndexToMultinode(multinode_mapping, traj[i]);

        if (i == 0) {
            joint_path.push_back(tmp_ik[traj[i]]);
        } else {
            int last_multinode = mapGTSPIndexToMultinode(multinode_mapping, traj[i - 1]);
            if (curr_multinode != last_multinode) {
                joint_path.push_back(tmp_ik[traj[i]]);
            }
        }
    }
    //push_back last node as this one must lead back to the start position
    //  joint_path.push_back(tmp_ik[traj[traj.size() - 1]]); TODO:check if correct

}

void JointGTSPSolver::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 JointGTSPSolver::insertStartIntoWeights(const Eigen::MatrixXf& weights, const Eigen::VectorXi& multinode_mapping,
    Eigen::MatrixXf& new_weights) {

    std::vector<std::vector<double> > tmp_ik;
    for (unsigned int i = 0; i < coll_free_ik_.size(); ++i) {
        for (unsigned int j = 0; j < coll_free_ik_[i].size(); ++j) {
            for (unsigned int k = 0; k < coll_free_ik_[i][j].size(); ++k) {
                tmp_ik.push_back(coll_free_ik_[i][j][k]);
            }
        }
    }

    new_weights = Eigen::MatrixXf::Ones(weights.rows() + 1, weights.rows() + 1) * max_adjacency_;
    for (int i = 1; i < new_weights.rows(); ++i) {

        if (dist_measure_ == "DIST") {
            double cost = traj_planner_.absJointConfigurationDist(start_joint_, tmp_ik[i - 1]);
            new_weights(i, 0) = cost;
            new_weights(0, i) = new_weights(i, 0);
        } else if (dist_measure_ == "TIME") {
            double cost = traj_planner_.jointConfigurationMinTravelTime(start_joint_, tmp_ik[i - 1]);
            new_weights(i, 0) = cost;
            new_weights(0, i) = new_weights(i, 0);
        } else {
            ROS_ERROR("JointGTSPSolver::transformGraphToGTSP Unknown TSP version");
        }
    }

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