00001
00002
00003 #include <stdio.h>
00004 #include <string>
00005 #include <hrpUtil/OnlineViewerUtil.h>
00006 #include <hrpModel/ModelLoaderUtil.h>
00007 #include <hrpCorba/DynamicsSimulator.hh>
00008
00009 using namespace std;
00010 using namespace hrp;
00011 using namespace OpenHRP;
00012
00013 enum {X, Y, Z};
00014 #define deg2rad(x) ( 3.14159265358979 / 180*(x) )
00015 #define DOF 29 // 初期姿勢配列の長さ
00016
00017
00018 template <typename X, typename X_ptr>
00019 X_ptr checkCorbaServer(std::string n, CosNaming::NamingContext_var &cxt)
00020 {
00021 CosNaming::Name ncName;
00022 ncName.length(1);
00023 ncName[0].id = CORBA::string_dup(n.c_str());
00024 ncName[0].kind = CORBA::string_dup("");
00025 X_ptr srv = NULL;
00026 try
00027 {
00028 srv = X::_narrow(cxt->resolve(ncName));
00029 }
00030 catch(const CosNaming::NamingContext::NotFound &exc)
00031 {
00032 std::cerr << n << " not found: ";
00033 switch(exc.why)
00034 {
00035 case CosNaming::NamingContext::missing_node:
00036 std::cerr << "Missing Node" << std::endl;
00037 break;
00038 case CosNaming::NamingContext::not_context:
00039 std::cerr << "Not Context" << std::endl;
00040 break;
00041 case CosNaming::NamingContext::not_object:
00042 std::cerr << "Not Object" << std::endl;
00043 break;
00044 }
00045 return (X_ptr)NULL;
00046 }
00047 catch(CosNaming::NamingContext::CannotProceed &exc)
00048 {
00049 std::cerr << "Resolve " << n << " CannotProceed" << std::endl;
00050 }
00051 catch(CosNaming::NamingContext::AlreadyBound &exc)
00052 {
00053 std::cerr << "Resolve " << n << " InvalidName" << std::endl;
00054 }
00055 return srv;
00056 }
00057
00058
00059 int main(int argc, char* argv[])
00060 {
00061 int i;
00062 string url = "file://";
00063 double ddp = 0.001;
00064 double timeK = 10.0;
00065
00066
00067 for(i=0; i < argc - 1; i++)
00068 {
00069 if( strcmp(argv[i], "-url") == 0)
00070 {
00071 url += argv[++i];
00072 } else if( strcmp(argv[i], "-ddp") == 0)
00073 {
00074 ddp = strtod( argv[++i], NULL);
00075 } else if( strcmp(argv[i], "-timeK") == 0)
00076 {
00077 timeK = strtod( argv[++i], NULL);
00078 }
00079 }
00080
00081 string robot_url = url+"sample.wrl";
00082 string floor_url = url+"floor.wrl";
00083
00084 const char *ROBOT_URL = robot_url.c_str();
00085 const char *FLOOR_URL = floor_url.c_str();
00086
00088
00089
00090 CORBA::ORB_var orb;
00091 orb = CORBA::ORB_init(argc, argv);
00092
00093
00094 CORBA::Object_var poaObj = orb -> resolve_initial_references("RootPOA");
00095 PortableServer::POA_var rootPOA = PortableServer::POA::_narrow(poaObj);
00096
00097
00098 PortableServer::POAManager_var manager = rootPOA -> the_POAManager();
00099
00100
00101 CosNaming::NamingContext_var cxT;
00102 {
00103 CORBA::Object_var nS = orb->resolve_initial_references("NameService");
00104 cxT = CosNaming::NamingContext::_narrow(nS);
00105 }
00106
00108
00109
00110 DynamicsSimulatorFactory_var dynamicsSimulatorFactory;
00111 dynamicsSimulatorFactory =
00112 checkCorbaServer <DynamicsSimulatorFactory,
00113 DynamicsSimulatorFactory_var> ("DynamicsSimulatorFactory", cxT);
00114
00115 if (CORBA::is_nil(dynamicsSimulatorFactory))
00116 {
00117 std::cerr << "DynamicsSimulatorFactory not found" << std::endl;
00118 }
00119
00120 DynamicsSimulator_var dynamicsSimulator
00121 = dynamicsSimulatorFactory->create();
00122
00123
00124 BodyInfo_var robotInfo = loadBodyInfo(ROBOT_URL, argc, argv);
00125 dynamicsSimulator->registerCharacter("robot", robotInfo);
00126
00127
00128 BodyInfo_var floorInfo = loadBodyInfo(FLOOR_URL, argc, argv);
00129 dynamicsSimulator->registerCharacter("floor", floorInfo);
00130
00131
00133
00134
00135 dynamicsSimulator->init(0.002,
00136 DynamicsSimulator::RUNGE_KUTTA,
00137 DynamicsSimulator::ENABLE_SENSOR);
00138
00139
00140 DblSequence3 gVector;
00141 gVector.length(3);
00142 gVector[0] = gVector[1] = 0;
00143 gVector[2] = 9.8;
00144 dynamicsSimulator->setGVector(gVector);
00145
00146
00147 dynamicsSimulator->setCharacterAllJointModes(
00148 "robot", DynamicsSimulator::TORQUE_MODE);
00149
00150
00151 double init_pos[] = {0.00E+00, -3.60E-02, 0, 7.85E-02, -4.25E-02, 0.00E+00,
00152 1.75E-01, -3.49E-03, 0, -1.57E+00, 0.00E+00, 0.00E+00,
00153 0.00E+00, 0.00E+00, -3.60E-02, 0, 7.85E-02, -4.25E-02,
00154 0.00E+00, 1.75E-01, 3.49E-03, 0, -1.57E+00, 0.00E+00,
00155 0.00E+00, 0.00E+00, 0, 0, 0};
00156
00157
00158 DblSequence q;
00159 q.length(DOF);
00160 for (int i=0; i<DOF; i++)
00161 {
00162 q[i] = init_pos[i];
00163 }
00164 dynamicsSimulator->setCharacterAllLinkData(
00165 "robot", DynamicsSimulator::JOINT_VALUE, q);
00166
00167
00168 dynamicsSimulator->calcWorldForwardKinematics();
00169
00170
00171 DblSequence6 dc, sc;
00172 dc.length(0);
00173 sc.length(0);
00174
00175 dynamicsSimulator->registerCollisionCheckPair(
00176 "robot",
00177 "RARM_WRIST_R",
00178 "robot",
00179 "LARM_WRIST_R",
00180 0.5,
00181 0.5,
00182 dc,
00183 sc,
00184 0.0,
00185 0.0);
00186
00187 dynamicsSimulator->initSimulation();
00188
00190
00191
00192 OnlineViewer_var onlineViewer = getOnlineViewer(argc, argv);
00193
00194 try
00195 {
00196 onlineViewer->load("robot", ROBOT_URL);
00197 onlineViewer->load("floor", FLOOR_URL);
00198 onlineViewer->setLogName("clap");
00199 onlineViewer->clearLog();
00200 }
00201 catch (CORBA::SystemException& ex)
00202 {
00203 std::cerr << "Failed to connect GrxUI." << endl;
00204 return 1;
00205 }
00207
00208
00209 double RARM_p[] = {0.197403, -0.210919, 0.93732};
00210 double RARM_R[] = {0.174891, -0.000607636, -0.984588,
00211 0.00348999, 0.999994, 2.77917e-06,
00212 0.984582, -0.00343669, 0.174892};
00213
00214 double LARM_p[] = {0.197403, 0.210919, 0.93732};
00215 double LARM_R[] = {0.174891, 0.000607636, -0.984588,
00216 -0.00348999, 0.999994, -2.77917e-06,
00217 0.984582, 0.00343669, 0.174892};
00218 double dp = 0.0;
00219
00221
00222 WorldState_var state;
00223 while (1)
00224 {
00225
00226 LinkPosition link;
00227 link.p[0] = RARM_p[0];
00228 link.p[1] = RARM_p[1] + dp;
00229 link.p[2] = RARM_p[2];
00230 for (int i=0; i<9; i++)
00231 link.R[i] = RARM_R[i];
00232 dynamicsSimulator->calcCharacterInverseKinematics(CORBA::string_dup("robot"),
00233 CORBA::string_dup("CHEST"),
00234 CORBA::string_dup("RARM_WRIST_R"),
00235 link);
00236
00237 link.p[0] = LARM_p[0];
00238 link.p[1] = LARM_p[1] - dp;
00239 link.p[2] = LARM_p[2];
00240 for (int i=0; i<9; i++)
00241 link.R[i] = LARM_R[i];
00242 dynamicsSimulator->calcCharacterInverseKinematics(CORBA::string_dup("robot"),
00243 CORBA::string_dup("CHEST"),
00244 CORBA::string_dup("LARM_WRIST_R"),
00245 link);
00246
00247 dynamicsSimulator->calcWorldForwardKinematics();
00248 dp += ddp;
00249
00250 try
00251 {
00252 dynamicsSimulator->getWorldState(state);
00253 state->time = dp*timeK;
00254 onlineViewer->update(state);
00255 }
00256 catch (CORBA::SystemException& ex)
00257 {
00258 std::cerr << "OnlineViewer could not be updated." << endl;
00259 std::cerr << "ex._rep_id() = " << ex._rep_id() << endl;
00260 return 1;
00261 }
00262
00263 if(state->time >= 200.0){
00264 std::cout << state->time << endl;
00265 }
00266
00267
00268 dynamicsSimulator->checkCollision(true);
00269
00270 if (state->collisions.length() > 0)
00271 {
00272 if (state->collisions[0].points.length() > 0)
00273 {
00274 break;
00275 }
00276 }
00277
00278 }
00279
00280 return 0;
00281
00282 }