Go to the documentation of this file.00001
00008 #include "PathPlannerSVC_impl.h"
00009
00010 #include <hrpPlanner/Roadmap.h>
00011 #include <hrpPlanner/RoadmapNode.h>
00012 #include <hrpPlanner/TimeUtil.h>
00013
00014
00015
00016
00017 OpenHRP_PathPlannerSVC_impl::OpenHRP_PathPlannerSVC_impl()
00018 {
00019 path_ = new PathEngine::PathPlanner(3);
00020 PathEngine::ConfigurationSpace *cspace
00021 = path_->getConfigurationSpace();
00022 cspace->unboundedRotation(2, true);
00023 cspace->bounds(0,-2,2);
00024 cspace->bounds(1,-2,2);
00025 }
00026
00027
00028 OpenHRP_PathPlannerSVC_impl::~OpenHRP_PathPlannerSVC_impl()
00029 {
00030 delete path_;
00031 }
00032
00033 void OpenHRP_PathPlannerSVC_impl::stopPlanning()
00034 {
00035 path_->stopPlanning();
00036 }
00037
00038
00039
00040
00041 void OpenHRP_PathPlannerSVC_impl::getRoadmap(OpenHRP::PathPlanner::Roadmap_out graph)
00042 {
00043 std::cout << "getRoadmap()" << std::endl;
00044
00045 PathEngine::RoadmapPtr roadmap = path_->getRoadmap();
00046
00047 graph = new OpenHRP::PathPlanner::Roadmap;
00048 std::cout << "the number of nodes = " << roadmap->nNodes() << std::endl;
00049 graph->length(roadmap->nNodes());
00050
00051 for (unsigned int i=0; i<roadmap->nNodes(); i++) {
00052 PathEngine::RoadmapNodePtr node = roadmap->node(i);
00053 const PathEngine::Configuration& pos = node->position();
00054 graph[i].cfg[0] = pos.value(0);
00055 graph[i].cfg[1] = pos.value(1);
00056 graph[i].cfg[2] = pos.value(2);
00057
00058 graph[i].neighbors.length(node->nChildren());
00059 for (unsigned int j=0; j<node->nChildren(); j++) {
00060 graph[i].neighbors[j] = roadmap->indexOfNode(node->child(j));
00061 }
00062 }
00063 }
00064
00065 void OpenHRP_PathPlannerSVC_impl::clearRoadmap()
00066 {
00067 PathEngine::RoadmapPtr rm = path_->getRoadmap();
00068 rm->clear();
00069 }
00070
00071 void OpenHRP_PathPlannerSVC_impl::getMobilityNames(OpenHRP::StringSequence_out mobilities)
00072 {
00073 mobilities = new OpenHRP::StringSequence;
00074 std::vector<std::string> mobilityNames;
00075 path_->getMobilityNames(mobilityNames);
00076 mobilities->length(mobilityNames.size());
00077
00078 for (unsigned int i=0; i<mobilityNames.size(); i++) {
00079 mobilities[i] = CORBA::string_dup(mobilityNames[i].c_str());
00080 }
00081 }
00082
00083 void OpenHRP_PathPlannerSVC_impl::getOptimizerNames(OpenHRP::StringSequence_out optimizers)
00084 {
00085 optimizers = new OpenHRP::StringSequence;
00086 std::vector<std::string> optimizerNames;
00087 path_->getOptimizerNames(optimizerNames);
00088 optimizers->length(optimizerNames.size());
00089
00090 for (unsigned int i=0; i<optimizerNames.size(); i++) {
00091 optimizers[i] = CORBA::string_dup(optimizerNames[i].c_str());
00092 }
00093 }
00094
00095 void OpenHRP_PathPlannerSVC_impl::setRobotName(const char* model)
00096 {
00097 path_->setRobotName(model);
00098 }
00099
00100 void OpenHRP_PathPlannerSVC_impl::setAlgorithmName(const char* algorithm)
00101 {
00102 path_->setAlgorithmName(algorithm);
00103 }
00104
00105 bool OpenHRP_PathPlannerSVC_impl::setMobilityName(const char* mobility)
00106 {
00107 return path_->setMobilityName(mobility);
00108 }
00109
00110 void OpenHRP_PathPlannerSVC_impl::getAlgorithmNames(OpenHRP::StringSequence_out algos)
00111 {
00112 algos = new OpenHRP::StringSequence;
00113 std::vector<std::string> algoNames;
00114 path_->getAlgorithmNames(algoNames);
00115 algos->length(algoNames.size());
00116
00117 for (unsigned int i=0; i<algoNames.size(); i++) {
00118 algos[i] = CORBA::string_dup(algoNames[i].c_str());
00119 }
00120 }
00121
00122 bool OpenHRP_PathPlannerSVC_impl::getProperties(const char* alg, OpenHRP::StringSequence_out props, OpenHRP::StringSequence_out defaults)
00123 {
00124 props = new OpenHRP::StringSequence;
00125 defaults = new OpenHRP::StringSequence;
00126
00127
00128 std::vector<std::string> propNames;
00129 std::vector<std::string> defaultValues;
00130
00131 propNames.push_back("weight-x");
00132 defaultValues.push_back("1.0");
00133 propNames.push_back("weight-y");
00134 defaultValues.push_back("1.0");
00135 propNames.push_back("weight-theta");
00136 defaultValues.push_back("1.0");
00137
00138 propNames.push_back("min-x");
00139 defaultValues.push_back("-2");
00140 propNames.push_back("min-y");
00141 defaultValues.push_back("-2");
00142 propNames.push_back("max-x");
00143 defaultValues.push_back("2");
00144 propNames.push_back("max-y");
00145 defaultValues.push_back("2");
00146
00147 if (!path_->getProperties(alg, propNames, defaultValues)) return false;
00148
00149 props->length(propNames.size());
00150 defaults->length(propNames.size());
00151
00152 for (unsigned int i=0; i<propNames.size(); i++) {
00153 props[i] = CORBA::string_dup(propNames[i].c_str());
00154 defaults[i] = CORBA::string_dup(defaultValues[i].c_str());
00155 }
00156 return true;
00157 }
00158
00159 void OpenHRP_PathPlannerSVC_impl::initPlanner()
00160 {
00161 std::cout << "initPlanner()" << std::endl;
00162 path_->initPlanner(nameServer_);
00163 std::cout << "fin. " << std::endl;
00164 }
00165
00166 void OpenHRP_PathPlannerSVC_impl::setStartPosition(CORBA::Double x, CORBA::Double y, CORBA::Double theta)
00167 {
00168 std::cout << "setStartPosition(" << x << ", " << y << ", " << theta << ")" << std::endl;
00169 PathEngine::Configuration pos(path_->getConfigurationSpace()->size());
00170 pos.value(0) = x;
00171 pos.value(1) = y;
00172 pos.value(2) = theta;
00173 path_->setStartConfiguration(pos);
00174 std::cout << "fin. " << std::endl;
00175 }
00176
00177 void OpenHRP_PathPlannerSVC_impl::setGoalPosition(CORBA::Double x, CORBA::Double y, CORBA::Double theta)
00178 {
00179 std::cout << "setGoalPosition(" << x << ", " << y << ", " << theta << ")" << std::endl;
00180 PathEngine::Configuration pos(path_->getConfigurationSpace()->size());
00181 pos.value(0) = x;
00182 pos.value(1) = y;
00183 pos.value(2) = theta;
00184 path_->setGoalConfiguration(pos);
00185 std::cout << "fin. " << std::endl;
00186 }
00187
00188 void OpenHRP_PathPlannerSVC_impl::setProperties(const OpenHRP::PathPlanner::Property& properites)
00189 {
00190 std::cout << "setProperties()" << std::endl;
00191 std::map<std::string, std::string> prop;
00192 for (unsigned int i=0; i<properites.length(); i++) {
00193 std::string name(properites[i][0]);
00194 std::string value(properites[i][1]);
00195 PathEngine::ConfigurationSpace *cspace =
00196 path_->getConfigurationSpace();
00197
00198 if (name == "min-x"){
00199 cspace->lbound(0) = atof(value.c_str());
00200 }else if (name == "max-x"){
00201 cspace->ubound(0) = atof(value.c_str());
00202 }else if (name == "min-y"){
00203 cspace->lbound(1) = atof(value.c_str());
00204 }else if (name == "max-y"){
00205 cspace->ubound(1) = atof(value.c_str());
00206 }else if (name == "weight-x"){
00207 cspace->weight(0) = atof(value.c_str());
00208 }else if (name == "weight-y"){
00209 cspace->weight(1) = atof(value.c_str());
00210 }else if (name == "weight-theta"){
00211 cspace->weight(2) = atof(value.c_str());
00212 }else{
00213 prop.insert(std::map<std::string, std::string>::value_type(name, value));
00214 }
00215 }
00216 path_->setProperties(prop);
00217
00218 std::cout << "fin. " << std::endl;
00219 }
00220
00221 CORBA::Boolean OpenHRP_PathPlannerSVC_impl::calcPath()
00222 {
00223 std::cout << "OpenHRP_PathPlannerSVC_impl::calcPath()" << std::endl;
00224 tick_t t1 = get_tick();
00225 bool status = path_->calcPath();
00226 std::cout << "OpenHRP_PathPlannerSVC_impl::fin." << std::endl;
00227 std::cout << "total computation time = " << tick2sec(get_tick()-t1) << "[s]"
00228 << std::endl;
00229 std::cout << "computation time for collision check = "
00230 << path_->timeCollisionCheck() << "[s]" << std::endl;
00231 std::cout << "computation time for forward kinematics = "
00232 << path_->timeForwardKinematics() << "[s]" << std::endl;
00233 std::cout << "collision check function was called "
00234 << path_->countCollisionCheck() << " times" << std::endl;
00235 return status;
00236 }
00237
00238
00239 void OpenHRP_PathPlannerSVC_impl::getPath(OpenHRP::PathPlanner::PointArray_out path)
00240 {
00241 std::cerr << "OpenHRP_PathPlannerSVC_impl::getPath()" << std::endl;
00242 const std::vector<PathEngine::Configuration>& p = path_->getPath();
00243
00244 path = new OpenHRP::PathPlanner::PointArray;
00245 path->length(p.size());
00246 std::cout << "length of path = " << p.size() << std::endl;
00247 for (unsigned int i=0; i<p.size(); i++) {
00248
00249 path[i].length(3);
00250 path[i][0] = p[i].value(0);
00251 path[i][1] = p[i].value(1);
00252 path[i][2] = p[i].value(2);
00253 }
00254 std::cerr << "OpenHRP_PathPlannerSVC_impl::fin. length of path = "
00255 << p.size() << std::endl;
00256 }
00257
00258 void OpenHRP_PathPlannerSVC_impl::registerIntersectionCheckPair(const char* char1, const char* name1, const char* char2, const char* name2, CORBA::Double tolerance)
00259 {
00260 path_->registerIntersectionCheckPair(char1,
00261 name1,
00262 char2,
00263 name2,
00264 tolerance);
00265 }
00266
00267 void OpenHRP_PathPlannerSVC_impl::registerCharacter(const char* name, OpenHRP::BodyInfo_ptr cInfo)
00268 {
00269 path_->registerCharacter(name, cInfo);
00270 }
00271
00272 void OpenHRP_PathPlannerSVC_impl::registerCharacterByURL(const char* name, const char* url)
00273 {
00274 path_->registerCharacterByURL(name, url);
00275 }
00276
00277 void OpenHRP_PathPlannerSVC_impl::setCharacterPosition(const char* character,
00278 const OpenHRP::DblSequence& pos)
00279 {
00280 path_->setCharacterPosition(character, pos);
00281 }
00282
00283
00284 void OpenHRP_PathPlannerSVC_impl::initSimulation()
00285 {
00286 std::cout << "initSimulation()" << std::endl;
00287 path_->initSimulation();
00288 std::cout << "fin. " << std::endl;
00289 }
00290
00291
00292
00293
00294 CORBA::Boolean OpenHRP_PathPlannerSVC_impl::optimize(const char *optimizer)
00295 {
00296 return path_->optimize(optimizer);
00297 }