TSPSolver.cpp

Go to the documentation of this file.

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

#include <coverage_3d_planning/TSPSolver.h>
#include <coverage_3d_planning/TSPSearch.h>
#include <coverage_3d_tools/conversions.h>

TSPSolver::TSPSolver() {
    dist_measure_="DIST";
    max_adjacency_=500;
}
TSPSolver::TSPSolver(double max_adjacency) {
    dist_measure_="DIST";
    max_adjacency_=max_adjacency;
}

void TSPSolver::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) {

    joint_path.clear();
    pose_path.clear();
    action.clear();

    //DISTANCE VERSION
    std::map<int, int> index_map;
    Eigen::MatrixXf weights;
    computeWeightMatrix(weights);

    TSPSearch tsp;
    tsp.writeToTSPFile(weights);
    tsp.callTSPSolver();
    std::vector<unsigned int> traj;
    tsp.readResultTSPFile(traj); // TODO:make sure we read trajectory without start

    traj.erase(traj.begin());
    std::vector<SurfacePatch> res_patch;
    for (unsigned int i = 0; i < traj.size(); ++i) {
        traj[i] = traj[i] - 1; // minus one as we have to consider the start which is not a patch
        res_patch.push_back(patches_[traj[i]]);
    }
    std::vector<unsigned int> actions;
    getActions(traj, actions);

    std::vector<std::vector<std::vector<double> > > ik_solutions_to_optimize;
    //collect end-effector poses and iksolutions for the trajectory
    for (unsigned int i = 0; i < traj.size(); ++i) {

        int curr_node = traj[i];
        int next_node = traj[i + 1];

        if (i < traj.size() - 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 {
                ROS_WARN("TSPSolver::getSolution Path planning edge taken --> should usually not happen.");
                std::vector<std::vector<std::vector<double> > > tmp = coll_free_ik_[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_[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
        }
    }
    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);

    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;

    res_traj = 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 TSPSolver::computeWeightMatrix(Eigen::MatrixXf& weigths) {
    //add one node for start pose

    Eigen::Vector3f start = PoseTF2EigenVector3(start_pose_);
    weigths = Eigen::MatrixXf::Ones(adjacency_.rows() + 1, adjacency_.rows() + 1) * max_adjacency_;
    for (int i = 1; i < weigths.rows(); ++i) {

        weigths(i, 0) = (start - surface_points_[i - 1].getVector3fMap()).norm();
        weigths(0, i) = weigths(i, 0);
    }

    for (int i = 1; i < weigths.rows(); ++i) {
        for (int j = 1; j < weigths.cols(); ++j) {
            if (adjacency_(i - 1, j - 1) == 1) {
                weigths(i, j)
                    = (surface_points_[i - 1].getVector3fMap() - surface_points_[j - 1].getVector3fMap()).norm();
            } else {
                weigths(i, j)
                    += (surface_points_[i - 1].getVector3fMap() - surface_points_[j - 1].getVector3fMap()).norm();

            }
        }
    }
}

void TSPSolver::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
        }
    }
}