TGT.cpp
Go to the documentation of this file.
00001 // -*- mode: c++; indent-tabs-mode: nil; c-basic-offset: 4; tab-width: 4; -*-
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     // from - changedFrom
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     // changedFrom - changedTo
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     // changedTo - to
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     //std::cout << "d = " << sqrt(dx*dx + dy*dy) << " +  " << dth1 << " + " <<  dth2 << std::endl;
00160     return sqrt(dx*dx + dy*dy) + dth1 + dth2;
00161 }


openhrp3
Author(s): AIST, General Robotix Inc., Nakamura Lab of Dept. of Mechano Informatics at University of Tokyo
autogenerated on Sun Apr 2 2017 03:43:57