TrajectoryPlanner.cpp

Go to the documentation of this file.

/*
 * TrajectoryPlanner.cpp
 *
 *  Created on: Feb 22, 2012
 *      Author: hess
 */
#include <limits.h>
#include <iostream>
#include <fstream>
#include <ros/ros.h>
#include <angles/angles.h>
#include <Eigen/Core>
#include <coverage_3d_tools/conversions.h>
#include <coverage_3d_arm_navigation/TrajectoryPlanner.h>
#include <boost/config.hpp>
#include <boost/graph/graph_traits.hpp>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/dijkstra_shortest_paths.hpp>

TrajectoryPlanner::TrajectoryPlanner() {

    //  r_arm_joints_.push_back("r_shoulder_pan_joint");
    //  r_arm_joints_.push_back("r_shoulder_lift_joint");
    //  r_arm_joints_.push_back("r_upper_arm_roll_joint");
    //  r_arm_joints_.push_back("r_elbow_flex_joint");
    //  r_arm_joints_.push_back("r_forearm_roll_joint");
    //  r_arm_joints_.push_back("r_wrist_flex_joint");
    //  r_arm_joints_.push_back("r_wrist_roll_joint");

    //    max_velocities_.resize(7);
    //    max_velocities_[0] = 2.088; //r_shoulder_pan_joint
    //    max_velocities_[1] = 2.082; //r_shoulder_lift_joint
    //    max_velocities_[2] = 3.27; //r_upper_arm_roll_joint
    //    max_velocities_[3] = 3.3; //r_elbow_flex_joint
    //    max_velocities_[4] = 3.6; //r_forearm_roll_joint
    //    max_velocities_[5] = 3.078; //r_wrist_flex_joint
    //    max_velocities_[6] = 3.6; // r_wrist_roll_joint

    max_velocities_.resize(7, 2.0);

    left_limits_.resize(7);
    left_limits_[0] = -2.2853981634; //r_shoulder_pan_joint
    left_limits_[1] = -0.5236; //r_shoulder_lift_joint
    left_limits_[2] = -3.9; //r_upper_arm_roll_joint
    left_limits_[3] = -2.3213; //r_elbow_flex_joint
    left_limits_[4] = std::numeric_limits<double>::max(); //r_forearm_roll_joint has no limit
    left_limits_[5] = -2.18; //r_wrist_flex_joint
    left_limits_[6] = std::numeric_limits<double>::max(); // r_wrist_roll_joint has no limit

    right_limits_.resize(7);
    right_limits_[0] = 0.714601836603; //r_shoulder_pan_joint
    right_limits_[1] = 1.3963; //r_shoulder_lift_joint
    right_limits_[2] = 0.8; //r_upper_arm_roll_joint
    right_limits_[3] = 0; //r_elbow_flex_joint
    right_limits_[4] = std::numeric_limits<double>::max(); //r_forearm_roll_joint has no limit
    right_limits_[5] = 0; //r_wrist_flex_joint
    right_limits_[6] = std::numeric_limits<double>::max(); // r_wrist_roll_joint has no limit

}

double TrajectoryPlanner::jointDist(double angle1, double angle2, unsigned int joint) {
    double diff = 0;
    if (left_limits_[joint] > M_PI) { // it is a continuous joint
        diff = angles::shortest_angular_distance(angle1, angle2);
    } else {
        bool ok = angles::shortest_angular_distance_with_limits(angle1, angle1, left_limits_[joint],
            right_limits_[joint], diff);
        if (!ok) {
            ROS_ERROR("TrajectoryPlanner::jointDist: smallest angle could not be computed.");
        }
    }
    return diff;
}
double TrajectoryPlanner::absJointConfigurationDist(const std::vector<double>& p1, const std::vector<double>& p2) {
    ROS_ASSERT(p1.size()==p2.size());

    double sum = 0;
    for (unsigned int i = 0; i < p1.size(); ++i) {
        sum += fabs(jointDist(p1[i], p2[i], i));
    }
    return sum;
}

void TrajectoryPlanner::jointConfigurationDifference(const std::vector<double>& p1, const std::vector<double>& p2,
    std::vector<double>& diff) {
    ROS_ASSERT(p1.size()==p2.size());
    diff.clear();
    diff.resize(p1.size());
    for (unsigned int i = 0; i < p1.size(); ++i) {
        if (left_limits_[i] > M_PI) { // it is a continuous joint
            diff[i] = angles::shortest_angular_distance(p1[i], p2[i]);
        } else {
            bool ok = angles::shortest_angular_distance_with_limits(p1[i], p2[i], left_limits_[i], right_limits_[i],
                diff[i]);
            if (!ok) {
                ROS_ERROR("TrajectoryPlanner::jointDist: smallest angle could not be computed.");
            }
        }
    }
}

void TrajectoryPlanner::absJointConfigurationDifference(const std::vector<double>& p1, const std::vector<double>& p2,
    std::vector<double>& diff) {
    ROS_ASSERT(p1.size()==p2.size());
    diff.clear();
    diff.resize(p1.size());
    jointConfigurationDifference(p1, p2, diff);

    for (unsigned int i = 0; i < diff.size(); ++i) {
        diff[i] = fabs(diff[i]);
    }
}

double TrajectoryPlanner::jointConfigurationMinTravelTime(const std::vector<double> v1, const std::vector<double> v2) {

    //convert double vectors to Eigen
    Eigen::VectorXf q1 = StdVector2Eigen(v1);
    Eigen::VectorXf q2 = StdVector2Eigen(v2);

    //get the minimal angle change for each joint of the joint vectors
    std::vector<double> v_diff;
    absJointConfigurationDifference(v1, v2, v_diff);
    Eigen::VectorXf q_diff = StdVector2Eigen(v_diff);
    Eigen::VectorXf max_vel = StdVector2Eigen(max_velocities_);

    Eigen::VectorXf min_travel_time_per_joint = q_diff.array() / max_vel.array();
    double min_time = min_travel_time_per_joint.array().maxCoeff();
    return min_time;
}

void TrajectoryPlanner::getMaxJointVelocities(const std::vector<double> v1, const std::vector<double> v2,
    Eigen::VectorXf& velocities, double& min_time) {

    //convert double vectors to Eigen
    Eigen::VectorXf q1 = StdVector2Eigen(v1);
    Eigen::VectorXf q2 = StdVector2Eigen(v2);

    //get the minimal angle change for each joint of the joint vectors
    std::vector<double> v_diff;
    jointConfigurationDifference(v1, v2, v_diff);
    Eigen::VectorXf q_diff = StdVector2Eigen(v_diff);
    Eigen::VectorXf max_vel = StdVector2Eigen(max_velocities_);

    Eigen::VectorXf min_travel_time_per_joint = q_diff.array().abs() / max_vel.array();
    min_time = min_travel_time_per_joint.array().maxCoeff();

    Eigen::VectorXf actual_travel_time = Eigen::VectorXf::Ones(min_travel_time_per_joint.rows()) * min_time;
    Eigen::VectorXf actual_velocities = q_diff.array() / actual_travel_time.array();
    velocities = actual_velocities;
}
void TrajectoryPlanner::getMaxJointVelocities(const std::vector<double> v1, const std::vector<double> v2, std::vector<
    double>& velocities, double& min_time) {

    //convert double vectors to Eigen
    Eigen::VectorXf q1 = StdVector2Eigen(v1);
    Eigen::VectorXf q2 = StdVector2Eigen(v2);

    //get the minimal angle change for each joint of the joint vectors
    std::vector<double> v_diff;
    jointConfigurationDifference(v1, v2, v_diff);
    Eigen::VectorXf q_diff = StdVector2Eigen(v_diff);
    Eigen::VectorXf max_vel = StdVector2Eigen(max_velocities_);

    Eigen::VectorXf min_travel_time_per_joint = q_diff.array().abs() / max_vel.array();
    min_time = min_travel_time_per_joint.array().maxCoeff();
    std::cout << "q_diff " << q_diff << std::endl;
    std::cout << "min travel time " << min_travel_time_per_joint << std::endl;
    Eigen::VectorXf actual_travel_time = Eigen::VectorXf::Ones(min_travel_time_per_joint.rows()) * min_time;
    Eigen::VectorXf actual_velocities = q_diff.array() / actual_travel_time.array();

    velocities.clear();
    for (int i = 0; i < actual_velocities.rows(); ++i) {
        velocities.push_back(actual_velocities(i));
    }
}
void TrajectoryPlanner::planTrajectoryVelocity(const std::vector<std::vector<double> >& joint_traj, std::vector<
    std::vector<double> >&vel, std::vector<double>&time, const double scale_t, const double scale_v) {
    vel.clear();
    time.clear();

    std::cout << "velocities " << std::endl;
    for (unsigned int i = 0; i < joint_traj.size() - 1; ++i) {

        std::vector<double> velocities(max_velocities_.size(), 0);
        if (i == 0) {
            vel.push_back(velocities);
            time.push_back(0);
            double tmp_time;
            getMaxJointVelocities(joint_traj[i], joint_traj[i + 1], velocities, tmp_time);
            time.push_back(scale_t * tmp_time);
        } else {
            double tmp_time;
            getMaxJointVelocities(joint_traj[i], joint_traj[i + 1], velocities, tmp_time);
            for (unsigned int j = 0; j < velocities.size(); ++j) {
                std::cout << velocities[j] << " " << scale_v * velocities[j];
                velocities[j] = scale_v * velocities[j];
            }
            std::cout << std::endl;
            vel.push_back(velocities);
            tmp_time = time.back() + (scale_t * tmp_time);
            time.push_back(tmp_time);
        }
    }
    std::vector<double> velocities(max_velocities_.size(), 0);
    vel.push_back(velocities);
}

void TrajectoryPlanner::planTrajectoryVelocity(const std::vector<std::vector<double> >& joint_traj, const std::vector<
    tf::Pose>& pose_traj, std::vector<std::vector<double> >&vel, std::vector<double>&time, const double scale_t,
    const double scale_v) {
    vel.clear();
    time.clear();

    std::cout << "velocities " << std::endl;
    for (unsigned int i = 0; i < joint_traj.size() - 1; ++i) {

        std::vector<double> velocities(max_velocities_.size(), 0);
        if (i == 0) {
            vel.push_back(velocities);
            time.push_back(0);
            double tmp_time;
            getMaxJointVelocities(joint_traj[i], joint_traj[i + 1], velocities, tmp_time);
            time.push_back(scale_t * tmp_time);
        } else {
            double tmp_time;
            getMaxJointVelocities(joint_traj[i], joint_traj[i + 1], velocities, tmp_time);
            for (unsigned int j = 0; j < velocities.size(); ++j) {
                std::cout << velocities[j] << " " << scale_v * velocities[j];
                velocities[j] = scale_v * velocities[j];
            }
            std::cout << std::endl;
            vel.push_back(velocities);
            tmp_time = time.back() + (scale_t * tmp_time);
            time.push_back(tmp_time);
        }
    }
    std::vector<double> velocities(max_velocities_.size(), 0);
    vel.push_back(velocities);

    //get min cartesian velocity
    std::cout << "cartesian velocities " << std::endl;
    double min_vel = std::numeric_limits<double>::max();
    for (unsigned int i = 0; i < vel.size() - 1; ++i) {

        //cartesian distance
        double dist = (pose_traj[i + 1].getOrigin() - pose_traj[i].getOrigin()).length();
        double tmp_time;
        getMaxJointVelocities(joint_traj[i], joint_traj[i + 1], velocities, tmp_time);
        double tmp_vel = dist / tmp_time;
        std::cout << tmp_vel << " \t" << dist << " " << std::endl;
        if (tmp_vel < min_vel) {
            min_vel = tmp_vel;
        }
    }
    //    for (int i = 1; i < vel.size()-1; ++i) {
    //
    //        Eigen::VectorXf p1 = StdVector2Eigen(vel[i-1]);
    //        Eigen::VectorXf p2 = StdVector2Eigen(vel[i]);
    //
    //        Eigen::VectorXf tmp= (p2.array()-p1.array())/(time[i+1]-time[i-1]);
    //        vel[i] = VectorEigen2Std(tmp);
    //        time[i] = time[i] + (0.8*(time[i+1]-time[i-1]));
    //  }
    std::cout << " Done " << std::endl;

    //  std::cout << "velocities " << std::endl;
    //    for (unsigned int i = 0; i < joint_traj.size() - 1; ++i) {
    //
    //        std::vector<double> velocities(max_velocities_.size(), 0);
    //        if (i == 0) {
    //            vel.push_back(velocities);
    //            time.push_back(0);
    //            double tmp_time;
    //            getMaxJointVelocities(joint_traj[i], joint_traj[i + 1], velocities, tmp_time);
    //            time.push_back(scale_t*tmp_time);
    //        } else {
    //            double tmp_time;
    //            getMaxJointVelocities(joint_traj[i], joint_traj[i + 1], velocities, tmp_time);
    //            for (unsigned int j = 0; j < velocities.size(); ++j) {
    //                  std::cout << velocities[j] << " " << scale_v*velocities[j];
    //                velocities[j]= scale_v*velocities[j];
    //            }
    //            std::cout << std::endl;
    //            vel.push_back(velocities);
    //            tmp_time = time.back() + (scale_t*tmp_time);
    //            time.push_back(tmp_time);
    //        }
    //    }
    //
    //    //convert double vectors to Eigen
    //    Eigen::VectorXf q1 = StdVector2Eigen(v1);
    //    Eigen::VectorXf q2 = StdVector2Eigen(v2);
    //
    //    //get the minimal angle change for each joint of the joint vectors
    //    std::vector<double> v_diff;
    //    jointConfigurationDifference(v1, v2, v_diff);
    //    Eigen::VectorXf q_diff = StdVector2Eigen(v_diff);
    //    Eigen::VectorXf max_vel = StdVector2Eigen(max_velocities_);
    //
    //    Eigen::VectorXf min_travel_time_per_joint = q_diff.array().abs() / max_vel.array();
    //    min_time = min_travel_time_per_joint.array().maxCoeff();
    //
    //    Eigen::VectorXf actual_travel_time = Eigen::VectorXf::Ones(min_travel_time_per_joint.rows());
    //    Eigen::VectorXf actual_velocities = q_diff.array() / actual_travel_time.array();
    //
    //    velocities.clear();
    //    for (int i = 0; i < actual_velocities.rows(); ++i) {
    //        velocities.push_back(actual_velocities(i));
    //    }
}
void TrajectoryPlanner::computeGreedyPath(const std::vector<std::vector<std::vector<double> > >& ik_solutions,
    const std::vector<bool>& valid_poses, std::vector<std::vector<double> >& path) {

    ROS_ASSERT(ik_solutions.size()==valid_poses.size());
    //get greedy path
    path.clear();
    int pose_count = 0;

    for (unsigned int i = 0; i < ik_solutions.size(); ++i) {
        if (valid_poses[i]) { // TODO:
            if (pose_count == 0) {
                path.push_back(ik_solutions[i][0]);
                pose_count++;
            } else {
                double min = std::numeric_limits<double>::max();
                int min_index = 0;
                for (unsigned int j = 0; j < ik_solutions[i].size(); ++j) {
                    double dist = absJointConfigurationDist(path[path.size() - 1], ik_solutions[i][j]);
                    if (dist < min) {
                        min = dist;
                        min_index = j;
                    }
                }
                path.push_back(ik_solutions[i][min_index]);
            }
        }
    }
}

void TrajectoryPlanner::computeDijkstraPath(const std::vector<std::vector<std::vector<double> > >& ik_solutions,
    const std::vector<bool>& valid_poses, std::vector<std::vector<double> >& path, std::string measure) {

    path.clear();
    using namespace boost;
    typedef float Weight;
    typedef boost::property<boost::edge_weight_t, Weight> WeightProperty;
    typedef boost::property<boost::vertex_name_t, unsigned int> NameProperty;
    typedef boost::property<boost::vertex_index_t, int> IndexProperty;
    typedef boost::adjacency_list<boost::vecS, boost::vecS, boost::directedS, NameProperty, WeightProperty> Graph;
    typedef boost::graph_traits<Graph>::vertex_descriptor Vertex;
    typedef boost::graph_traits<Graph>::vertex_iterator Viter;
    typedef boost::property_map<Graph, boost::vertex_index_t>::type IndexMap;
    typedef boost::property_map<Graph, boost::vertex_name_t>::type NameMap;
    typedef boost::iterator_property_map<Vertex*, IndexMap, Vertex, Vertex&> PredecessorMap;
    typedef boost::iterator_property_map<Weight*, IndexMap, Weight, Weight&> DistanceMap;

    Graph g;

    std::vector<std::vector<int> > mapping;
    std::vector<std::vector<double> > lin_ik;
    //get node mapping
    int index = 0;
    for (unsigned int i = 0; i < ik_solutions.size(); ++i) {
        std::vector<int> tmp_mapping;
        for (unsigned int j = 0; j < ik_solutions[i].size(); ++j) {
            tmp_mapping.push_back(index);
            lin_ik.push_back(ik_solutions[i][j]);
            index++;
        }
        mapping.push_back(tmp_mapping);
    }

    for (unsigned int i = 0; i < ik_solutions.size() - 1; ++i) {
        for (unsigned int j = 0; j < ik_solutions[i].size(); ++j) {
            for (unsigned int k = 0; k < ik_solutions[i + 1].size(); ++k) {

                unsigned int v1 = mapping[i][j];
                unsigned int v2 = mapping[i + 1][k];
                float dist = 0;
                if (measure == "TIME") {
                    dist = jointConfigurationMinTravelTime(ik_solutions[i][j], ik_solutions[i + 1][k]);
                } else if (measure == "DIST") {
                    dist = absJointConfigurationDist(ik_solutions[i][j], ik_solutions[i + 1][k]);
                }
                boost::add_edge(v1, v2, dist, g);
            }
        }
    }

    typedef graph_traits<Graph>::vertex_descriptor vertex_descriptor;
    typedef graph_traits<Graph>::edge_descriptor edge_descriptor;
    typedef std::pair<unsigned int, int> Edge;

    std::vector<Vertex> predecessors(boost::num_vertices(g)); // To store parents
    std::vector<Weight> distances(boost::num_vertices(g)); // To store distances

    std::vector<vertex_descriptor> p(num_vertices(g));
    Vertex start = 0;

    dijkstra_shortest_paths(g, start, predecessor_map(&predecessors[0]).distance_map(&distances[0]));

    // Extract a shortest path
    std::cout << std::endl;
    typedef std::vector<Graph::edge_descriptor> PathType;
    PathType tmp_path;
    Vertex v = boost::num_vertices(g) - 1;
    for (Vertex u = predecessors[v]; u != v; // Keep tracking the path until we get to the source
    v = u, u = predecessors[v]) // Set the current vertex to the current predecessor,     and the predecessor to one level up
    {
        std::pair<Graph::edge_descriptor, bool> edgePair = boost::edge(u, v, g);
        Graph::edge_descriptor edge = edgePair.first;
        tmp_path.push_back(edge);
    }

    // Push the trajectory
    for (PathType::reverse_iterator pathIterator = tmp_path.rbegin(); pathIterator != tmp_path.rend(); ++pathIterator) {
        if (pathIterator == tmp_path.rbegin()) {
            path.push_back(lin_ik[boost::source(*pathIterator, g)]);
        }
        path.push_back(lin_ik[boost::target(*pathIterator, g)]);
        //        std::cout << boost::source(*pathIterator, g) << " -> " << boost::target(*pathIterator, g) << " = "
        //            << boost::get(boost::edge_weight, g, *pathIterator) << std::endl;

    }

    //    std::cout << std::endl;
    //    Vertex last_vertex = boost::num_vertices(g) - 1;
    //    std::cout << "Distance: " << distances[last_vertex] << std::endl;
}