Go to the documentation of this file.00001
00002 #include "ConfigurationSpace.h"
00003 #include "PathPlanner.h"
00004 #include "TGT.h"
00005 #define _USE_MATH_DEFINES // for MSVC
00006 #include <math.h>
00007
00008 using namespace PathEngine;
00009
00010 inline double theta_diff(double from, double to)
00011 {
00012 double diff = to - from;
00013 if (diff > M_PI){
00014 return diff - 2*M_PI;
00015 }else if (diff < -M_PI){
00016 return diff + 2*M_PI;
00017 }else{
00018 return diff;
00019 }
00020 }
00021
00022 #if 0
00023 std::vector<Configuration> TGT::getPath(const Configuration &from, const Configuration &to) const
00024 {
00025 std::vector<Configuration> path;
00026
00027 double dx = to.value(0) - from.value(0);
00028 double dy = to.value(1) - from.value(1);
00029 double theta = atan2(dy, dx);
00030
00031 Configuration changedFrom(from.value(0), from.value(1), theta);
00032 Configuration changedTo(to.value(0), to.value(1), theta);
00033
00034 path.push_back(from);
00035
00036 unsigned int n;
00037 Configuration pos;
00038
00039 std::cout << 1 << std::endl;
00040
00041 n = (unsigned int)(distance(from, changedFrom)/interpolationDistance())+1;
00042 for (unsigned int i=1; i<=n; i++){
00043 pos = interpolate(from, changedFrom, ((double)i)/n);
00044 std::cout << pos << std::endl;
00045 path.push_back(pos);
00046 }
00047
00048 std::cout << 2 << std::endl;
00049
00050 n = (unsigned int)(distance(changedFrom, changedTo)/interpolationDistance())+1;
00051 for (unsigned int i=1; i<=n; i++){
00052 pos = interpolate(changedFrom, changedTo, ((double)i)/n);
00053 std::cout << pos << std::endl;
00054 path.push_back(pos);
00055 }
00056
00057 std::cout << 3 << std::endl;
00058
00059 n = (unsigned int)(distance(changedTo, to)/interpolationDistance())+1;
00060 for (unsigned int i=1; i<=n; i++){
00061 pos = interpolate(changedTo, to, ((double)i)/n);
00062 std::cout << pos << std::endl;
00063 path.push_back(pos);
00064 }
00065 getchar();
00066
00067 return path;
00068 }
00069 #endif
00070
00071 Configuration TGT::interpolate(const Configuration& from, const Configuration& to,
00072 double ratio) const
00073 {
00074 ConfigurationSpace *cspace = planner_->getConfigurationSpace();
00075 double dx = to.value(0) - from.value(0);
00076 double dy = to.value(1) - from.value(1);
00077
00078 if (dx == 0 && dy == 0){
00079 double dth = theta_diff(from.value(2), to.value(2));
00080 Configuration cfg(cspace->size());
00081 cfg.value(0) = from.value(0);
00082 cfg.value(1) = from.value(1);
00083 cfg.value(2) = from.value(2)+ratio*dth;
00084 return cfg;
00085 }else{
00086
00087 double theta = atan2(dy, dx);
00088
00089 double dth1 = theta_diff(from.value(2), theta);
00090 double d1 = cspace->weight(2)*fabs(dth1);
00091
00092 dx *= cspace->weight(0);
00093 dy *= cspace->weight(1);
00094 double d2 = sqrt(dx*dx + dy*dy);
00095
00096 double dth2 = theta_diff(theta, to.value(2));
00097 double d3 = cspace->weight(2) * fabs(dth2);
00098
00099 double d = d1 + d2 + d3;
00100
00101 #if 0
00102 std::cout << "theta = " << theta << ", dth1 = " << dth1
00103 << ", dth2 = " << dth2 << std::endl;
00104 std::cout << "d1:" << d1 << ", d2:" << d2 << ", d3:" << d3 << std::endl;
00105 #endif
00106
00107 if (d == 0){
00108 return from;
00109 }
00110
00111 if (ratio >= 0 && ratio*d < d1){
00112 Configuration cfg(cspace->size());
00113 cfg.value(0) = from.value(0);
00114 cfg.value(1) = from.value(1);
00115 cfg.value(2) = from.value(2) + ratio*d/d1*dth1;
00116 return cfg;
00117 }else if (ratio*d >= d1 && ratio*d < (d1+d2)){
00118 double r = (ratio*d - d1)/d2;
00119 Configuration cfg(cspace->size());
00120 cfg.value(0) = (1-r)*from.value(0) + r*to.value(0);
00121 cfg.value(1) = (1-r)*from.value(1) + r*to.value(1);
00122 cfg.value(2) = theta;
00123 return cfg;
00124 }else if (ratio*d >= (d1+d2) && ratio <= 1.0){
00125 Configuration cfg(cspace->size());
00126 cfg.value(0) = to.value(0);
00127 cfg.value(1) = to.value(1);
00128 cfg.value(2) = theta + (ratio*d-d1-d2)/d3*dth2;
00129 return cfg;
00130 }else{
00131 std::cout << "TGT::interpolate() : invalid ratio(" << ratio << ")"
00132 << std::endl;
00133 abort ();
00134 }
00135 }
00136 }
00137
00138 double TGT::distance(const Configuration& from, const Configuration& to) const
00139 {
00140 ConfigurationSpace *cspace = planner_->getConfigurationSpace();
00141 double dx = to.value(0) - from.value(0);
00142 double dy = to.value(1) - from.value(1);
00143 double theta = atan2(dy, dx);
00144
00145 dx *= cspace->weight(0);
00146 dy *= cspace->weight(1);
00147
00148 if (dx == 0 && dy == 0) {
00149 return cspace->weight(2)*fabs(theta_diff(from.value(2), to.value(2)));
00150 }
00151
00152
00153 double dth1 = fabs(theta_diff(from.value(2), theta));
00154 dth1 *= cspace->weight(2);
00155
00156 double dth2 = fabs(theta_diff(theta, to.value(2)));
00157 dth2 *= cspace->weight(2);
00158
00159
00160 return sqrt(dx*dx + dy*dy) + dth1 + dth2;
00161 }