00001
00002
00003
00004
00005
00006
00007
00008 #include <limits.h>
00009 #include <iostream>
00010 #include <fstream>
00011 #include <ros/ros.h>
00012 #include <angles/angles.h>
00013 #include <Eigen/Core>
00014 #include <coverage_3d_tools/conversions.h>
00015 #include <coverage_3d_arm_navigation/TrajectoryPlanner.h>
00016 #include <boost/config.hpp>
00017 #include <boost/graph/graph_traits.hpp>
00018 #include <boost/graph/adjacency_list.hpp>
00019 #include <boost/graph/dijkstra_shortest_paths.hpp>
00020
00021 TrajectoryPlanner::TrajectoryPlanner() {
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037
00038
00039
00040 max_velocities_.resize(7, 2.0);
00041
00042 left_limits_.resize(7);
00043 left_limits_[0] = -2.2853981634;
00044 left_limits_[1] = -0.5236;
00045 left_limits_[2] = -3.9;
00046 left_limits_[3] = -2.3213;
00047 left_limits_[4] = std::numeric_limits<double>::max();
00048 left_limits_[5] = -2.18;
00049 left_limits_[6] = std::numeric_limits<double>::max();
00050
00051 right_limits_.resize(7);
00052 right_limits_[0] = 0.714601836603;
00053 right_limits_[1] = 1.3963;
00054 right_limits_[2] = 0.8;
00055 right_limits_[3] = 0;
00056 right_limits_[4] = std::numeric_limits<double>::max();
00057 right_limits_[5] = 0;
00058 right_limits_[6] = std::numeric_limits<double>::max();
00059
00060 }
00061
00062 double TrajectoryPlanner::jointDist(double angle1, double angle2, unsigned int joint) {
00063 double diff = 0;
00064 if (left_limits_[joint] > M_PI) {
00065 diff = angles::shortest_angular_distance(angle1, angle2);
00066 } else {
00067 bool ok = angles::shortest_angular_distance_with_limits(angle1, angle1, left_limits_[joint],
00068 right_limits_[joint], diff);
00069 if (!ok) {
00070 ROS_ERROR("TrajectoryPlanner::jointDist: smallest angle could not be computed.");
00071 }
00072 }
00073 return diff;
00074 }
00075 double TrajectoryPlanner::absJointConfigurationDist(const std::vector<double>& p1, const std::vector<double>& p2) {
00076 ROS_ASSERT(p1.size()==p2.size());
00077
00078 double sum = 0;
00079 for (unsigned int i = 0; i < p1.size(); ++i) {
00080 sum += fabs(jointDist(p1[i], p2[i], i));
00081 }
00082 return sum;
00083 }
00084
00085 void TrajectoryPlanner::jointConfigurationDifference(const std::vector<double>& p1, const std::vector<double>& p2,
00086 std::vector<double>& diff) {
00087 ROS_ASSERT(p1.size()==p2.size());
00088 diff.clear();
00089 diff.resize(p1.size());
00090 for (unsigned int i = 0; i < p1.size(); ++i) {
00091 if (left_limits_[i] > M_PI) {
00092 diff[i] = angles::shortest_angular_distance(p1[i], p2[i]);
00093 } else {
00094 bool ok = angles::shortest_angular_distance_with_limits(p1[i], p2[i], left_limits_[i], right_limits_[i],
00095 diff[i]);
00096 if (!ok) {
00097 ROS_ERROR("TrajectoryPlanner::jointDist: smallest angle could not be computed.");
00098 }
00099 }
00100 }
00101 }
00102
00103 void TrajectoryPlanner::absJointConfigurationDifference(const std::vector<double>& p1, const std::vector<double>& p2,
00104 std::vector<double>& diff) {
00105 ROS_ASSERT(p1.size()==p2.size());
00106 diff.clear();
00107 diff.resize(p1.size());
00108 jointConfigurationDifference(p1, p2, diff);
00109
00110 for (unsigned int i = 0; i < diff.size(); ++i) {
00111 diff[i] = fabs(diff[i]);
00112 }
00113 }
00114
00115 double TrajectoryPlanner::jointConfigurationMinTravelTime(const std::vector<double> v1, const std::vector<double> v2) {
00116
00117
00118 Eigen::VectorXf q1 = StdVector2Eigen(v1);
00119 Eigen::VectorXf q2 = StdVector2Eigen(v2);
00120
00121
00122 std::vector<double> v_diff;
00123 absJointConfigurationDifference(v1, v2, v_diff);
00124 Eigen::VectorXf q_diff = StdVector2Eigen(v_diff);
00125 Eigen::VectorXf max_vel = StdVector2Eigen(max_velocities_);
00126
00127 Eigen::VectorXf min_travel_time_per_joint = q_diff.array() / max_vel.array();
00128 double min_time = min_travel_time_per_joint.array().maxCoeff();
00129 return min_time;
00130 }
00131
00132 void TrajectoryPlanner::getMaxJointVelocities(const std::vector<double> v1, const std::vector<double> v2,
00133 Eigen::VectorXf& velocities, double& min_time) {
00134
00135
00136 Eigen::VectorXf q1 = StdVector2Eigen(v1);
00137 Eigen::VectorXf q2 = StdVector2Eigen(v2);
00138
00139
00140 std::vector<double> v_diff;
00141 jointConfigurationDifference(v1, v2, v_diff);
00142 Eigen::VectorXf q_diff = StdVector2Eigen(v_diff);
00143 Eigen::VectorXf max_vel = StdVector2Eigen(max_velocities_);
00144
00145 Eigen::VectorXf min_travel_time_per_joint = q_diff.array().abs() / max_vel.array();
00146 min_time = min_travel_time_per_joint.array().maxCoeff();
00147
00148 Eigen::VectorXf actual_travel_time = Eigen::VectorXf::Ones(min_travel_time_per_joint.rows()) * min_time;
00149 Eigen::VectorXf actual_velocities = q_diff.array() / actual_travel_time.array();
00150 velocities = actual_velocities;
00151 }
00152 void TrajectoryPlanner::getMaxJointVelocities(const std::vector<double> v1, const std::vector<double> v2, std::vector<
00153 double>& velocities, double& min_time) {
00154
00155
00156 Eigen::VectorXf q1 = StdVector2Eigen(v1);
00157 Eigen::VectorXf q2 = StdVector2Eigen(v2);
00158
00159
00160 std::vector<double> v_diff;
00161 jointConfigurationDifference(v1, v2, v_diff);
00162 Eigen::VectorXf q_diff = StdVector2Eigen(v_diff);
00163 Eigen::VectorXf max_vel = StdVector2Eigen(max_velocities_);
00164
00165 Eigen::VectorXf min_travel_time_per_joint = q_diff.array().abs() / max_vel.array();
00166 min_time = min_travel_time_per_joint.array().maxCoeff();
00167 std::cout << "q_diff " << q_diff << std::endl;
00168 std::cout << "min travel time " << min_travel_time_per_joint << std::endl;
00169 Eigen::VectorXf actual_travel_time = Eigen::VectorXf::Ones(min_travel_time_per_joint.rows()) * min_time;
00170 Eigen::VectorXf actual_velocities = q_diff.array() / actual_travel_time.array();
00171
00172 velocities.clear();
00173 for (int i = 0; i < actual_velocities.rows(); ++i) {
00174 velocities.push_back(actual_velocities(i));
00175 }
00176 }
00177 void TrajectoryPlanner::planTrajectoryVelocity(const std::vector<std::vector<double> >& joint_traj, std::vector<
00178 std::vector<double> >&vel, std::vector<double>&time, const double scale_t, const double scale_v) {
00179 vel.clear();
00180 time.clear();
00181
00182 std::cout << "velocities " << std::endl;
00183 for (unsigned int i = 0; i < joint_traj.size() - 1; ++i) {
00184
00185 std::vector<double> velocities(max_velocities_.size(), 0);
00186 if (i == 0) {
00187 vel.push_back(velocities);
00188 time.push_back(0);
00189 double tmp_time;
00190 getMaxJointVelocities(joint_traj[i], joint_traj[i + 1], velocities, tmp_time);
00191 time.push_back(scale_t * tmp_time);
00192 } else {
00193 double tmp_time;
00194 getMaxJointVelocities(joint_traj[i], joint_traj[i + 1], velocities, tmp_time);
00195 for (unsigned int j = 0; j < velocities.size(); ++j) {
00196 std::cout << velocities[j] << " " << scale_v * velocities[j];
00197 velocities[j] = scale_v * velocities[j];
00198 }
00199 std::cout << std::endl;
00200 vel.push_back(velocities);
00201 tmp_time = time.back() + (scale_t * tmp_time);
00202 time.push_back(tmp_time);
00203 }
00204 }
00205 std::vector<double> velocities(max_velocities_.size(), 0);
00206 vel.push_back(velocities);
00207 }
00208
00209 void TrajectoryPlanner::planTrajectoryVelocity(const std::vector<std::vector<double> >& joint_traj, const std::vector<
00210 tf::Pose>& pose_traj, std::vector<std::vector<double> >&vel, std::vector<double>&time, const double scale_t,
00211 const double scale_v) {
00212 vel.clear();
00213 time.clear();
00214
00215 std::cout << "velocities " << std::endl;
00216 for (unsigned int i = 0; i < joint_traj.size() - 1; ++i) {
00217
00218 std::vector<double> velocities(max_velocities_.size(), 0);
00219 if (i == 0) {
00220 vel.push_back(velocities);
00221 time.push_back(0);
00222 double tmp_time;
00223 getMaxJointVelocities(joint_traj[i], joint_traj[i + 1], velocities, tmp_time);
00224 time.push_back(scale_t * tmp_time);
00225 } else {
00226 double tmp_time;
00227 getMaxJointVelocities(joint_traj[i], joint_traj[i + 1], velocities, tmp_time);
00228 for (unsigned int j = 0; j < velocities.size(); ++j) {
00229 std::cout << velocities[j] << " " << scale_v * velocities[j];
00230 velocities[j] = scale_v * velocities[j];
00231 }
00232 std::cout << std::endl;
00233 vel.push_back(velocities);
00234 tmp_time = time.back() + (scale_t * tmp_time);
00235 time.push_back(tmp_time);
00236 }
00237 }
00238 std::vector<double> velocities(max_velocities_.size(), 0);
00239 vel.push_back(velocities);
00240
00241
00242 std::cout << "cartesian velocities " << std::endl;
00243 double min_vel = std::numeric_limits<double>::max();
00244 for (unsigned int i = 0; i < vel.size() - 1; ++i) {
00245
00246
00247 double dist = (pose_traj[i + 1].getOrigin() - pose_traj[i].getOrigin()).length();
00248 double tmp_time;
00249 getMaxJointVelocities(joint_traj[i], joint_traj[i + 1], velocities, tmp_time);
00250 double tmp_vel = dist / tmp_time;
00251 std::cout << tmp_vel << " \t" << dist << " " << std::endl;
00252 if (tmp_vel < min_vel) {
00253 min_vel = tmp_vel;
00254 }
00255 }
00256
00257
00258
00259
00260
00261
00262
00263
00264
00265 std::cout << " Done " << std::endl;
00266
00267
00268
00269
00270
00271
00272
00273
00274
00275
00276
00277
00278
00279
00280
00281
00282
00283
00284
00285
00286
00287
00288
00289
00290
00291
00292
00293
00294
00295
00296
00297
00298
00299
00300
00301
00302
00303
00304
00305
00306
00307
00308
00309
00310
00311 }
00312 void TrajectoryPlanner::computeGreedyPath(const std::vector<std::vector<std::vector<double> > >& ik_solutions,
00313 const std::vector<bool>& valid_poses, std::vector<std::vector<double> >& path) {
00314
00315 ROS_ASSERT(ik_solutions.size()==valid_poses.size());
00316
00317 path.clear();
00318 int pose_count = 0;
00319
00320 for (unsigned int i = 0; i < ik_solutions.size(); ++i) {
00321 if (valid_poses[i]) {
00322 if (pose_count == 0) {
00323 path.push_back(ik_solutions[i][0]);
00324 pose_count++;
00325 } else {
00326 double min = std::numeric_limits<double>::max();
00327 int min_index = 0;
00328 for (unsigned int j = 0; j < ik_solutions[i].size(); ++j) {
00329 double dist = absJointConfigurationDist(path[path.size() - 1], ik_solutions[i][j]);
00330 if (dist < min) {
00331 min = dist;
00332 min_index = j;
00333 }
00334 }
00335 path.push_back(ik_solutions[i][min_index]);
00336 }
00337 }
00338 }
00339 }
00340
00341 void TrajectoryPlanner::computeDijkstraPath(const std::vector<std::vector<std::vector<double> > >& ik_solutions,
00342 const std::vector<bool>& valid_poses, std::vector<std::vector<double> >& path, std::string measure) {
00343
00344 path.clear();
00345 using namespace boost;
00346 typedef float Weight;
00347 typedef boost::property<boost::edge_weight_t, Weight> WeightProperty;
00348 typedef boost::property<boost::vertex_name_t, unsigned int> NameProperty;
00349 typedef boost::property<boost::vertex_index_t, int> IndexProperty;
00350 typedef boost::adjacency_list<boost::vecS, boost::vecS, boost::directedS, NameProperty, WeightProperty> Graph;
00351 typedef boost::graph_traits<Graph>::vertex_descriptor Vertex;
00352 typedef boost::graph_traits<Graph>::vertex_iterator Viter;
00353 typedef boost::property_map<Graph, boost::vertex_index_t>::type IndexMap;
00354 typedef boost::property_map<Graph, boost::vertex_name_t>::type NameMap;
00355 typedef boost::iterator_property_map<Vertex*, IndexMap, Vertex, Vertex&> PredecessorMap;
00356 typedef boost::iterator_property_map<Weight*, IndexMap, Weight, Weight&> DistanceMap;
00357
00358 Graph g;
00359
00360 std::vector<std::vector<int> > mapping;
00361 std::vector<std::vector<double> > lin_ik;
00362
00363 int index = 0;
00364 for (unsigned int i = 0; i < ik_solutions.size(); ++i) {
00365 std::vector<int> tmp_mapping;
00366 for (unsigned int j = 0; j < ik_solutions[i].size(); ++j) {
00367 tmp_mapping.push_back(index);
00368 lin_ik.push_back(ik_solutions[i][j]);
00369 index++;
00370 }
00371 mapping.push_back(tmp_mapping);
00372 }
00373
00374 for (unsigned int i = 0; i < ik_solutions.size() - 1; ++i) {
00375 for (unsigned int j = 0; j < ik_solutions[i].size(); ++j) {
00376 for (unsigned int k = 0; k < ik_solutions[i + 1].size(); ++k) {
00377
00378 unsigned int v1 = mapping[i][j];
00379 unsigned int v2 = mapping[i + 1][k];
00380 float dist = 0;
00381 if (measure == "TIME") {
00382 dist = jointConfigurationMinTravelTime(ik_solutions[i][j], ik_solutions[i + 1][k]);
00383 } else if (measure == "DIST") {
00384 dist = absJointConfigurationDist(ik_solutions[i][j], ik_solutions[i + 1][k]);
00385 }
00386 boost::add_edge(v1, v2, dist, g);
00387 }
00388 }
00389 }
00390
00391 typedef graph_traits<Graph>::vertex_descriptor vertex_descriptor;
00392 typedef graph_traits<Graph>::edge_descriptor edge_descriptor;
00393 typedef std::pair<unsigned int, int> Edge;
00394
00395 std::vector<Vertex> predecessors(boost::num_vertices(g));
00396 std::vector<Weight> distances(boost::num_vertices(g));
00397
00398 std::vector<vertex_descriptor> p(num_vertices(g));
00399 Vertex start = 0;
00400
00401 dijkstra_shortest_paths(g, start, predecessor_map(&predecessors[0]).distance_map(&distances[0]));
00402
00403
00404 std::cout << std::endl;
00405 typedef std::vector<Graph::edge_descriptor> PathType;
00406 PathType tmp_path;
00407 Vertex v = boost::num_vertices(g) - 1;
00408 for (Vertex u = predecessors[v]; u != v;
00409 v = u, u = predecessors[v])
00410 {
00411 std::pair<Graph::edge_descriptor, bool> edgePair = boost::edge(u, v, g);
00412 Graph::edge_descriptor edge = edgePair.first;
00413 tmp_path.push_back(edge);
00414 }
00415
00416
00417 for (PathType::reverse_iterator pathIterator = tmp_path.rbegin(); pathIterator != tmp_path.rend(); ++pathIterator) {
00418 if (pathIterator == tmp_path.rbegin()) {
00419 path.push_back(lin_ik[boost::source(*pathIterator, g)]);
00420 }
00421 path.push_back(lin_ik[boost::target(*pathIterator, g)]);
00422
00423
00424
00425 }
00426
00427
00428
00429
00430 }