/*
* Methods corresponding to IDL attributes and operations
*/
void OpenHRP_PathPlannerSVC_impl::getRoadmap(OpenHRP::PathPlanner::Roadmap_out graph)
{
std::cout << "getRoadmap()" << std::endl;
PathEngine::Roadmap *roadmap = path_->getRoadmap();
graph = new OpenHRP::PathPlanner::Roadmap;
std::cout << "the number of nodes = " << roadmap->nNodes() << std::endl;
graph->length(roadmap->nNodes());
for (unsigned int i=0; i<roadmap->nNodes(); i++) {
PathEngine::RoadmapNode *node = roadmap->node(i);
const PathEngine::Configuration& pos = node->position();
graph[i].cfg[0] = pos.value(0);
graph[i].cfg[1] = pos.value(1);
graph[i].cfg[2] = pos.value(2);
graph[i].neighbors.length(node->nChildren());
for (unsigned int j=0; j<node->nChildren(); j++) {
graph[i].neighbors[j] = roadmap->indexOfNode(node->child(j));
}
}
}
int main(int argc, char *argv[])
{
srand((unsigned)time(NULL));
const char *robotURL = NULL;
std::vector<std::string> obstacleURL;
std::vector<Vector3> obstacleP;
std::vector<Vector3> obstacleRpy;
for(int i = 1 ; i < argc; i++) {
if (strcmp(argv[i], "-robot") == 0) {
robotURL = argv[++i];
} else if (strcmp(argv[i], "-obstacle") == 0) {
obstacleURL.push_back(argv[++i]);
Vector3 p, rpy;
for (int j=0; j<3; j++) p[j] = atof(argv[++i]);
obstacleP.push_back(p);
for (int j=0; j<3; j++) rpy[j] = atof(argv[++i]);
obstacleRpy.push_back(rpy);
}
}
if (robotURL == NULL) {
std::cerr << "please specify URL of VRML model by -robot option"
<< std::endl;
return 1;
}
BodyPtr robot = new Body();
loadBodyFromModelLoader(robot, robotURL, argc, argv, true);
problem prob;
prob.addRobot("robot", robotURL, robot);
std::vector<BodyPtr> obstacles;
for (unsigned int i=0; i<obstacleURL.size(); i++) {
char buf[20];
sprintf(buf, "obstacle%02d", i);
obstacles.push_back(prob.addObstacle(buf, obstacleURL[i]));
}
// This must be called after all bodies are added
prob.initOLV(argc, argv);
PathEngine::Configuration::size(4+6+6);
PathEngine::Configuration::bounds(0, 0.2, 0.8); // body z
PathEngine::Configuration::bounds(1, -0.5, 0.5); // body roll
PathEngine::Configuration::bounds(2, -0.0, 0.5); // body pitch
PathEngine::Configuration::bounds(3, -0.5, 0.5); // body yaw
int arm;
PathEngine::Configuration goalCfg;
std::ifstream ifs("goal.txt");
ifs >> arm;
for (unsigned int i=0; i<PathEngine::Configuration::size(); i++) {
ifs >> goalCfg[i];
}
#if 1
PathEngine::Configuration::weight(0) = 0.1; // z
PathEngine::Configuration::weight(1) = 1; // roll
PathEngine::Configuration::weight(2) = 1; // pitch
PathEngine::Configuration::weight(3) = 1; // yaw
PathEngine::Configuration::weight(4) = 0.8;
PathEngine::Configuration::weight(5) = 0.6;
PathEngine::Configuration::weight(6) = 0.4;
PathEngine::Configuration::weight(7) = 0.3;
PathEngine::Configuration::weight(8) = 0.2;
PathEngine::Configuration::weight(9) = 0.1;
PathEngine::Configuration::weight(10) = 0.8;
PathEngine::Configuration::weight(11) = 0.6;
PathEngine::Configuration::weight(12) = 0.4;
PathEngine::Configuration::weight(13) = 0.3;
PathEngine::Configuration::weight(14) = 0.2;
PathEngine::Configuration::weight(15) = 0.1;
#endif
JointPathPtr armPath[2];
Link *chest = robot->link("CHEST_JOINT1");
Link *wrist[2] = {robot->link("RARM_JOINT5"),
robot->link("LARM_JOINT5")
};
for (int k=0; k<2; k++) {
armPath[k] = robot->getJointPath(chest, wrist[k]);
for (int i=0; i<armPath[k]->numJoints(); i++) {
Link *j = armPath[k]->joint(i);
PathEngine::Configuration::bounds(4+k*6+i, j->llimit, j->ulimit);
}
}
PathEngine::PathPlanner *planner = prob.planner();
planner->setMobilityName("OmniWheel");
planner->setAlgorithmName("RRT");
PathEngine::RRT *rrt = (PathEngine::RRT *)planner->getAlgorithm();
rrt->extendFromGoal(true);
planner->getAlgorithm()->setProperty("eps", "0.1");
planner->getAlgorithm()->setProperty("max-trials", "50000");
planner->getAlgorithm()->setProperty("interpolation-distance", "0.05");
prob.initCollisionCheckPairs();
prob.initPlanner();
for (unsigned int i=0; i<obstacles.size(); i++) {
Link *root = obstacles[i]->rootLink();
root->p = obstacleP[i];
root->R = rotFromRpy(obstacleRpy[i]);
root->coldetModel->setPosition(root->R, root->p);
obstacles[i]->calcForwardKinematics();
}
// set halfconf
dvector halfconf;
halfconf.setZero(robot->numJoints());
#define ToRad(x) ((x)*M_PI/180)
halfconf[2] = halfconf[ 8] = ToRad(-40);
halfconf[3] = halfconf[ 9] = ToRad( 78);
halfconf[4] = halfconf[10] = ToRad(-38);
double leg_link_len1=0, leg_link_len2=0;
halfconf[16] = halfconf[23] = ToRad(20);
halfconf[17] = ToRad(-10);
halfconf[24] = -halfconf[17];
halfconf[19] = halfconf[26] = ToRad(-30);
if (robot->modelName() == "HRP2") {
halfconf[20] = ToRad(80);
halfconf[27] = -halfconf[20];
halfconf[22] = halfconf[29] = -1.0;
}
leg_link_len1 = leg_link_len2 = 0.3;
double waistHeight = leg_link_len1*cos(halfconf[2])
+ leg_link_len2*cos(halfconf[4])
+ 0.105;
robot->rootLink()->p(2) = waistHeight;
for (int i=0; i<robot->numJoints(); i++) {
robot->joint(i)->q = halfconf[i];
}
robot->calcForwardKinematics();
myCfgSetter setter(robot);
PathEngine::Configuration startCfg;
startCfg[0] = robot->rootLink()->p[2];
startCfg[1] = startCfg[2] = startCfg[3] = 0;
for (int j=0; j<2; j++) {
for (int i=0; i<armPath[j]->numJoints(); i++) {
startCfg[4+j*6+i] = armPath[j]->joint(i)->q;
}
}
planner->setStartConfiguration(startCfg);
planner->setGoalConfiguration(goalCfg);
planner->setApplyConfigFunc(boost::bind(&myCfgSetter::set, &setter,
_1, _2));
planner->setConfiguration(startCfg);
prob.updateOLV();
planner->setConfiguration(goalCfg);
prob.updateOLV();
#if 0
int earm;
ifs >> earm;
while (!ifs.eof()) {
PathEngine::Configuration cfg;
for (unsigned int i=0; i<PathEngine::Configuration::size(); i++) {
ifs >> cfg[i];
}
if (arm == earm) {
rrt->addExtraGoal(cfg);
std::cout << "added an extra goal" << std::endl;
planner->setConfiguration(cfg);
prob.updateOLV();
}
ifs >> earm;
}
#endif
CustomCD cd(robot, "hrp2.shape", "hrp2.pairs",
obstacles[0], "plant.pc");
prob.planner()->setCollisionDetector(&cd);
struct timeval tv1, tv2;
gettimeofday(&tv1, NULL);
// plan
bool ret = planner->calcPath();
if (ret) {
for (int i=0; i<5; i++) {
planner->optimize("Shortcut");
planner->optimize("RandomShortcut");
}
}
gettimeofday(&tv2, NULL);
if (ret) {
const std::vector<PathEngine::Configuration>& postures
= planner->getWayPoints();
std::ofstream ofs("path.txt");
ofs << arm << std::endl;
for (unsigned int i=0; i<postures.size(); i++) {
planner->setConfiguration(postures[i]);
for (int j=0; j<3; j++) {
ofs << robot->rootLink()->p[j] << " ";
}
for (int j=0; j<3; j++) {
for (int k=0; k<3; k++) {
ofs << robot->rootLink()->R(j,k) << " ";
}
}
for (int j=0; j<robot->numJoints(); j++) {
ofs << robot->joint(j)->q << " ";
}
ofs << std::endl;
prob.updateOLV();
}
} else {
PathEngine::Roadmap *roadmap = planner->getRoadmap();
for (unsigned int i=0; i<roadmap->nNodes(); i++) {
PathEngine::RoadmapNode *node = roadmap->node(i);
planner->setConfiguration(node->position());
prob.updateOLV();
}
}
double time = (tv2.tv_sec - tv1.tv_sec) + ((double)(tv2.tv_usec - tv1.tv_usec))/1e6;
std::cout << "total time = " << time << "[s]" << std::endl;
double cdtime = prob.planner()->timeCollisionCheck();
std::cout << "time spent in collision detection:"
<< cdtime << "[s](" << cdtime/time*100 << "[%]), "
<< cdtime*1e3/prob.planner()->countCollisionCheck() << "[ms/call]" << std::endl;
std::cout << "profile:";
setter.profile();
return 0;
}
