move_ankle.cpp
Go to the documentation of this file.
00001 #include <hrpUtil/OnlineViewerUtil.h>
00002 #include <hrpUtil/Eigen3d.h>
00003 #include <hrpModel/Link.h>
00004 #include <hrpModel/LinkTraverse.h>
00005 #include <hrpModel/JointPath.h>
00006 #include <hrpModel/ModelLoaderUtil.h>
00007 
00008 using namespace std;
00009 using namespace hrp;
00010 using namespace OpenHRP;
00011 
00012 enum {X, Y, Z};
00013 #define deg2rad(x) ( 3.14159265358979 / 180*(x) )
00014 
00015 int
00016 main(int argc, char* argv[])
00017 {
00018     int i;
00019     string url = "file://";
00020     // -urlでモデルのURLを指定   //
00021     for(i=0; i < argc; i++) {
00022         if( strcmp(argv[i], "-url") == 0 && i+1 < argc) url += argv[i+1];
00023     }
00024 
00025     // モデルロード  //
00026     BodyPtr body(new Body());
00027     if(!loadBodyFromModelLoader(body, url.c_str(), argc, argv)){
00028         cerr << "ModelLoader: " << url << " cannot be loaded" << endl;
00029         return 0;
00030     }
00031 
00032     body->calcForwardKinematics();
00033 
00034     // OnlineViewer設定  //
00035     OnlineViewer_var olv = getOnlineViewer(argc, argv);
00036     try {
00037         olv->load(body->modelName().c_str(), url.c_str());
00038         olv->setLogName("move_ankle");
00039     } catch (CORBA::SystemException& ex) {
00040         cerr << "Failed to connect GrxUI." << endl;
00041         return 1;
00042     }
00043 
00044         // 特異点にならないよう最初は曲げておく  //
00045     body->joint(1)->q = deg2rad(-10);
00046     body->joint(3)->q = deg2rad(20);
00047     body->joint(4)->q = deg2rad(-10);
00048     body->calcForwardKinematics();
00049 
00050     // 腰から足首までのパスを設定  //
00051     Link* waist = body->link("WAIST");
00052     Link* ankle = body->link("RLEG_ANKLE_R");
00053     JointPathPtr path = body->getJointPath(waist, ankle);
00054 
00055     // WorldStateを作成する  //
00056     WorldState world;
00057     world.characterPositions.length(1);
00058     world.collisions.length(0);
00059 
00060     // SampleRobot用CharacterPosition  //
00061     //world.collisions.length(0);
00062     CharacterPosition& robot = world.characterPositions[0];
00063     robot.characterName = CORBA::string_dup("SampleRobot");
00064 
00065     // 時間は0  //
00066     world.time=0;
00067 
00068     while (1) {
00069         // 時間を進める  //
00070         world.time+=0.01;
00071 
00072         // 少し動かす  //
00073         Vector3 p = ankle->p;
00074         Matrix33 R = ankle->R;
00075         p(2) += 0.002;
00076 
00077         // もし逆運動学計算に失敗したら終わり  //
00078         if (!path->calcInverseKinematics(p, R)) {
00079             break;
00080         }
00081 
00082         // LinkをWorldStateにコピーする。  //
00083         int n = body->numLinks();
00084         robot.linkPositions.length(n);
00085         for (int i=0; i<n; i++) {
00086             Link* link = body->link(i);
00087             setVector3(link->p, robot.linkPositions[i].p);
00088             setMatrix33ToRowMajorArray(link->R, robot.linkPositions[i].R);
00089         }
00090 
00091         // OnlineViewer アップデート  //
00092         try {       
00093             olv->update(world);
00094         } catch (CORBA::SystemException& ex) {
00095             std::cerr << "OnlineViewer could not be updated." << endl;
00096             return 1;
00097         }
00098     }
00099 
00100     return 0;
00101 }


openhrp3
Author(s): AIST, General Robotix Inc., Nakamura Lab of Dept. of Mechano Informatics at University of Tokyo
autogenerated on Thu Apr 11 2019 03:30:17