clap.cpp
Go to the documentation of this file.
1 // -*- mode: c++; indent-tabs-mode: t; tab-width: 4; c-basic-offset: 8; -*-
2 
3 #include <stdio.h>
4 #include <string>
5 #include <hrpUtil/OnlineViewerUtil.h>
6 #include <hrpModel/ModelLoaderUtil.h>
7 #include <hrpCorba/DynamicsSimulator.hh>
8 
9 using namespace std;
10 using namespace hrp;
11 using namespace OpenHRP;
12 
13 enum {X, Y, Z};
14 #define deg2rad(x) ( 3.14159265358979 / 180*(x) )
15 #define DOF 29 // 初期姿勢配列の長さ
16 
17 // サーバ群の取得
18 template <typename X, typename X_ptr>
19 X_ptr checkCorbaServer(std::string n, CosNaming::NamingContext_var &cxt)
20 {
21  CosNaming::Name ncName;
22  ncName.length(1);
23  ncName[0].id = CORBA::string_dup(n.c_str());
24  ncName[0].kind = CORBA::string_dup("");
25  X_ptr srv = NULL;
26  try
27  {
28  srv = X::_narrow(cxt->resolve(ncName));
29  }
30  catch(const CosNaming::NamingContext::NotFound &exc)
31  {
32  std::cerr << n << " not found: ";
33  switch(exc.why)
34  {
35  case CosNaming::NamingContext::missing_node:
36  std::cerr << "Missing Node" << std::endl;
37  break;
38  case CosNaming::NamingContext::not_context:
39  std::cerr << "Not Context" << std::endl;
40  break;
41  case CosNaming::NamingContext::not_object:
42  std::cerr << "Not Object" << std::endl;
43  break;
44  }
45  return (X_ptr)NULL;
46  }
47  catch(CosNaming::NamingContext::CannotProceed &exc)
48  {
49  std::cerr << "Resolve " << n << " CannotProceed" << std::endl;
50  }
51  catch(CosNaming::NamingContext::AlreadyBound &exc)
52  {
53  std::cerr << "Resolve " << n << " InvalidName" << std::endl;
54  }
55  return srv;
56 }
57 
58 
59 int main(int argc, char* argv[])
60 {
61  int i;
62  string url = "file://";
63  double ddp = 0.001;
64  double timeK = 10.0;
65 
66  // -urlでモデルのURLを指定
67  for(i=0; i < argc - 1; i++)
68  {
69  if( strcmp(argv[i], "-url") == 0)
70  {
71  url += argv[++i];
72  } else if( strcmp(argv[i], "-ddp") == 0)
73  {
74  ddp = strtod( argv[++i], NULL);
75  } else if( strcmp(argv[i], "-timeK") == 0)
76  {
77  timeK = strtod( argv[++i], NULL);
78  }
79  }
80 
81  string robot_url = url+"sample.wrl";
82  string floor_url = url+"floor.wrl";
83 
84  const char *ROBOT_URL = robot_url.c_str();
85  const char *FLOOR_URL = floor_url.c_str();
86 
88 
89  // CORBA初期化
90  CORBA::ORB_var orb;
91  orb = CORBA::ORB_init(argc, argv);
92 
93  // ROOT POA
94  CORBA::Object_var poaObj = orb -> resolve_initial_references("RootPOA");
95  PortableServer::POA_var rootPOA = PortableServer::POA::_narrow(poaObj);
96 
97  // POAマネージャへの参照を取得
98  PortableServer::POAManager_var manager = rootPOA -> the_POAManager();
99 
100  // NamingServiceの参照取得
101  CosNaming::NamingContext_var cxT;
102  {
103  CORBA::Object_var nS = orb->resolve_initial_references("NameService");
104  cxT = CosNaming::NamingContext::_narrow(nS);
105  }
106 
108 
109  // DynamicsSimulatorの取得
110  DynamicsSimulatorFactory_var dynamicsSimulatorFactory;
111  dynamicsSimulatorFactory =
112  checkCorbaServer <DynamicsSimulatorFactory,
113  DynamicsSimulatorFactory_var> ("DynamicsSimulatorFactory", cxT);
114 
115  if (CORBA::is_nil(dynamicsSimulatorFactory))
116  {
117  std::cerr << "DynamicsSimulatorFactory not found" << std::endl;
118  }
119 
120  DynamicsSimulator_var dynamicsSimulator
121  = dynamicsSimulatorFactory->create();
122 
123  // モデルの読み込み・登録
124  BodyInfo_var robotInfo = loadBodyInfo(ROBOT_URL, argc, argv);
125  dynamicsSimulator->registerCharacter("robot", robotInfo);
126 
127  // 床の読み込み・登録
128  BodyInfo_var floorInfo = loadBodyInfo(FLOOR_URL, argc, argv);
129  dynamicsSimulator->registerCharacter("floor", floorInfo);
130 
131 
133 
134  // DynamicsSimulatorの初期化
135  dynamicsSimulator->init(0.002,
136  DynamicsSimulator::RUNGE_KUTTA,
137  DynamicsSimulator::ENABLE_SENSOR);
138 
139  // 重力ベクトルの設定 DblSequence3 gVector; gVector.length(3); gVector[0] = gVector[1] = 0; gVector[2] = 9.8; dynamicsSimulator->setGVector(gVector); // 関節駆動モードの設定 dynamicsSimulator->setCharacterAllJointModes( "robot", DynamicsSimulator::TORQUE_MODE); // 初期姿勢 double init_pos[] = {0.00E+00, -3.60E-02, 0, 7.85E-02, -4.25E-02, 0.00E+00, 1.75E-01, -3.49E-03, 0, -1.57E+00, 0.00E+00, 0.00E+00, 0.00E+00, 0.00E+00, -3.60E-02, 0, 7.85E-02, -4.25E-02, 0.00E+00, 1.75E-01, 3.49E-03, 0, -1.57E+00, 0.00E+00, 0.00E+00, 0.00E+00, 0, 0, 0}; // 初期姿勢を関節角にセット DblSequence q; q.length(DOF); for (int i=0; i<DOF; i++) { q[i] = init_pos[i]; } dynamicsSimulator->setCharacterAllLinkData( "robot", DynamicsSimulator::JOINT_VALUE, q); // 順運動学計算 dynamicsSimulator->calcWorldForwardKinematics(); // 干渉チェックペアの設定−両手 DblSequence6 dc, sc; dc.length(0); sc.length(0); dynamicsSimulator->registerCollisionCheckPair( "robot", "RARM_WRIST_R", "robot", "LARM_WRIST_R", 0.5, 0.5, dc, sc, 0.0, 0.0); dynamicsSimulator->initSimulation(); ///////////////////////////////////////////////////////////////////////// // OnlineViewerの取得 OnlineViewer_var onlineViewer = getOnlineViewer(argc, argv); // OnlineViewerに対するモデルロード try { onlineViewer->load("robot", ROBOT_URL); onlineViewer->load("floor", FLOOR_URL); onlineViewer->setLogName("clap"); onlineViewer->clearLog(); } catch (CORBA::SystemException& ex) { std::cerr << "Failed to connect GrxUI." << endl; return 1; } ///////////////////////////////////////////////////////////////////////// // 逆運動学計算の準備 double RARM_p[] = {0.197403, -0.210919, 0.93732}; double RARM_R[] = {0.174891, -0.000607636, -0.984588, 0.00348999, 0.999994, 2.77917e-06, 0.984582, -0.00343669, 0.174892}; double LARM_p[] = {0.197403, 0.210919, 0.93732}; double LARM_R[] = {0.174891, 0.000607636, -0.984588, -0.00348999, 0.999994, -2.77917e-06, 0.984582, 0.00343669, 0.174892}; double dp = 0.0; ///////////////////////////////////////////////////////////////////////// WorldState_var state; while (1) { // 逆運動学計算 LinkPosition link; link.p[0] = RARM_p[0]; link.p[1] = RARM_p[1] + dp; link.p[2] = RARM_p[2]; for (int i=0; i<9; i++) link.R[i] = RARM_R[i]; dynamicsSimulator->calcCharacterInverseKinematics(CORBA::string_dup("robot"), CORBA::string_dup("CHEST"), CORBA::string_dup("RARM_WRIST_R"), link); link.p[0] = LARM_p[0]; link.p[1] = LARM_p[1] - dp; link.p[2] = LARM_p[2]; for (int i=0; i<9; i++) link.R[i] = LARM_R[i]; dynamicsSimulator->calcCharacterInverseKinematics(CORBA::string_dup("robot"), CORBA::string_dup("CHEST"), CORBA::string_dup("LARM_WRIST_R"), link); dynamicsSimulator->calcWorldForwardKinematics(); dp += ddp; // OnlineViewer更新 try { dynamicsSimulator->getWorldState(state); state->time = dp*timeK; onlineViewer->update(state); } catch (CORBA::SystemException& ex) { std::cerr << "OnlineViewer could not be updated." << endl; std::cerr << "ex._rep_id() = " << ex._rep_id() << endl; return 1; } if(state->time >= 200.0){ std::cout << state->time << endl; } // 干渉チェック dynamicsSimulator->checkCollision(true); if (state->collisions.length() > 0) { if (state->collisions[0].points.length() > 0) { break; } } } return 0; }
140  DblSequence3 gVector;
141  gVector.length(3);
142  gVector[0] = gVector[1] = 0;
143  gVector[2] = 9.8;
144  dynamicsSimulator->setGVector(gVector);
145 
146  // 関節駆動モードの設定 dynamicsSimulator->setCharacterAllJointModes( "robot", DynamicsSimulator::TORQUE_MODE); // 初期姿勢 double init_pos[] = {0.00E+00, -3.60E-02, 0, 7.85E-02, -4.25E-02, 0.00E+00, 1.75E-01, -3.49E-03, 0, -1.57E+00, 0.00E+00, 0.00E+00, 0.00E+00, 0.00E+00, -3.60E-02, 0, 7.85E-02, -4.25E-02, 0.00E+00, 1.75E-01, 3.49E-03, 0, -1.57E+00, 0.00E+00, 0.00E+00, 0.00E+00, 0, 0, 0}; // 初期姿勢を関節角にセット DblSequence q; q.length(DOF); for (int i=0; i<DOF; i++) { q[i] = init_pos[i]; } dynamicsSimulator->setCharacterAllLinkData( "robot", DynamicsSimulator::JOINT_VALUE, q); // 順運動学計算 dynamicsSimulator->calcWorldForwardKinematics(); // 干渉チェックペアの設定−両手 DblSequence6 dc, sc; dc.length(0); sc.length(0); dynamicsSimulator->registerCollisionCheckPair( "robot", "RARM_WRIST_R", "robot", "LARM_WRIST_R", 0.5, 0.5, dc, sc, 0.0, 0.0); dynamicsSimulator->initSimulation(); ///////////////////////////////////////////////////////////////////////// // OnlineViewerの取得 OnlineViewer_var onlineViewer = getOnlineViewer(argc, argv); // OnlineViewerに対するモデルロード try { onlineViewer->load("robot", ROBOT_URL); onlineViewer->load("floor", FLOOR_URL); onlineViewer->setLogName("clap"); onlineViewer->clearLog(); } catch (CORBA::SystemException& ex) { std::cerr << "Failed to connect GrxUI." << endl; return 1; } ///////////////////////////////////////////////////////////////////////// // 逆運動学計算の準備 double RARM_p[] = {0.197403, -0.210919, 0.93732}; double RARM_R[] = {0.174891, -0.000607636, -0.984588, 0.00348999, 0.999994, 2.77917e-06, 0.984582, -0.00343669, 0.174892}; double LARM_p[] = {0.197403, 0.210919, 0.93732}; double LARM_R[] = {0.174891, 0.000607636, -0.984588, -0.00348999, 0.999994, -2.77917e-06, 0.984582, 0.00343669, 0.174892}; double dp = 0.0; ///////////////////////////////////////////////////////////////////////// WorldState_var state; while (1) { // 逆運動学計算 LinkPosition link; link.p[0] = RARM_p[0]; link.p[1] = RARM_p[1] + dp; link.p[2] = RARM_p[2]; for (int i=0; i<9; i++) link.R[i] = RARM_R[i]; dynamicsSimulator->calcCharacterInverseKinematics(CORBA::string_dup("robot"), CORBA::string_dup("CHEST"), CORBA::string_dup("RARM_WRIST_R"), link); link.p[0] = LARM_p[0]; link.p[1] = LARM_p[1] - dp; link.p[2] = LARM_p[2]; for (int i=0; i<9; i++) link.R[i] = LARM_R[i]; dynamicsSimulator->calcCharacterInverseKinematics(CORBA::string_dup("robot"), CORBA::string_dup("CHEST"), CORBA::string_dup("LARM_WRIST_R"), link); dynamicsSimulator->calcWorldForwardKinematics(); dp += ddp; // OnlineViewer更新 try { dynamicsSimulator->getWorldState(state); state->time = dp*timeK; onlineViewer->update(state); } catch (CORBA::SystemException& ex) { std::cerr << "OnlineViewer could not be updated." << endl; std::cerr << "ex._rep_id() = " << ex._rep_id() << endl; return 1; } if(state->time >= 200.0){ std::cout << state->time << endl; } // 干渉チェック dynamicsSimulator->checkCollision(true); if (state->collisions.length() > 0) { if (state->collisions[0].points.length() > 0) { break; } } } return 0; }
147  dynamicsSimulator->setCharacterAllJointModes(
148  "robot", DynamicsSimulator::TORQUE_MODE);
149 
150  // 初期姿勢
151  double init_pos[] = {0.00E+00, -3.60E-02, 0, 7.85E-02, -4.25E-02, 0.00E+00,
152  1.75E-01, -3.49E-03, 0, -1.57E+00, 0.00E+00, 0.00E+00,
153  0.00E+00, 0.00E+00, -3.60E-02, 0, 7.85E-02, -4.25E-02,
154  0.00E+00, 1.75E-01, 3.49E-03, 0, -1.57E+00, 0.00E+00,
155  0.00E+00, 0.00E+00, 0, 0, 0};
156 
157  // 初期姿勢を関節角にセット
158  DblSequence q;
159  q.length(DOF);
160  for (int i=0; i<DOF; i++)
161  {
162  q[i] = init_pos[i];
163  }
164  dynamicsSimulator->setCharacterAllLinkData(
165  "robot", DynamicsSimulator::JOINT_VALUE, q);
166 
167  // 順運動学計算
168  dynamicsSimulator->calcWorldForwardKinematics();
169 
170  // 干渉チェックペアの設定−両手
171  DblSequence6 dc, sc;
172  dc.length(0);
173  sc.length(0);
174 
175  dynamicsSimulator->registerCollisionCheckPair(
176  "robot",
177  "RARM_WRIST_R",
178  "robot",
179  "LARM_WRIST_R",
180  0.5,
181  0.5,
182  dc,
183  sc,
184  0.0,
185  0.0);
186 
187  dynamicsSimulator->initSimulation();
188 
190 
191  // OnlineViewerの取得
192  OnlineViewer_var onlineViewer = getOnlineViewer(argc, argv);
193  // OnlineViewerに対するモデルロード
194  try
195  {
196  onlineViewer->load("robot", ROBOT_URL);
197  onlineViewer->load("floor", FLOOR_URL);
198  onlineViewer->setLogName("clap");
199  onlineViewer->clearLog();
200  }
201  catch (CORBA::SystemException& ex)
202  {
203  std::cerr << "Failed to connect GrxUI." << endl;
204  return 1;
205  }
207 
208  // 逆運動学計算の準備
209  double RARM_p[] = {0.197403, -0.210919, 0.93732};
210  double RARM_R[] = {0.174891, -0.000607636, -0.984588,
211  0.00348999, 0.999994, 2.77917e-06,
212  0.984582, -0.00343669, 0.174892};
213 
214  double LARM_p[] = {0.197403, 0.210919, 0.93732};
215  double LARM_R[] = {0.174891, 0.000607636, -0.984588,
216  -0.00348999, 0.999994, -2.77917e-06,
217  0.984582, 0.00343669, 0.174892};
218  double dp = 0.0;
219 
221 
222  WorldState_var state;
223  while (1)
224  {
225  // 逆運動学計算
226  LinkPosition link;
227  link.p[0] = RARM_p[0];
228  link.p[1] = RARM_p[1] + dp;
229  link.p[2] = RARM_p[2];
230  for (int i=0; i<9; i++)
231  link.R[i] = RARM_R[i];
232  dynamicsSimulator->calcCharacterInverseKinematics(CORBA::string_dup("robot"),
233  CORBA::string_dup("CHEST"),
234  CORBA::string_dup("RARM_WRIST_R"),
235  link);
236 
237  link.p[0] = LARM_p[0];
238  link.p[1] = LARM_p[1] - dp;
239  link.p[2] = LARM_p[2];
240  for (int i=0; i<9; i++)
241  link.R[i] = LARM_R[i];
242  dynamicsSimulator->calcCharacterInverseKinematics(CORBA::string_dup("robot"),
243  CORBA::string_dup("CHEST"),
244  CORBA::string_dup("LARM_WRIST_R"),
245  link);
246 
247  dynamicsSimulator->calcWorldForwardKinematics();
248  dp += ddp;
249  // OnlineViewer更新
250  try
251  {
252  dynamicsSimulator->getWorldState(state);
253  state->time = dp*timeK;
254  onlineViewer->update(state);
255  }
256  catch (CORBA::SystemException& ex)
257  {
258  std::cerr << "OnlineViewer could not be updated." << endl;
259  std::cerr << "ex._rep_id() = " << ex._rep_id() << endl;
260  return 1;
261  }
262 
263  if(state->time >= 200.0){
264  std::cout << state->time << endl;
265  }
266 
267  // 干渉チェック
268  dynamicsSimulator->checkCollision(true);
269 
270  if (state->collisions.length() > 0)
271  {
272  if (state->collisions[0].points.length() > 0)
273  {
274  break;
275  }
276  }
277 
278  }
279 
280  return 0;
281 
282 }
hrpPrep.NotFound
NotFound
Definition: hrpPrep.py:129
DOF
#define DOF
Definition: clap.cpp:15
hrp::loadBodyInfo
HRPMODEL_API OpenHRP::BodyInfo_var loadBodyInfo(const char *url, int &argc, char *argv[])
Definition: hrplib/hrpModel/ModelLoaderUtil.cpp:632
state
state
Y
Definition: clap.cpp:13
X
Definition: clap.cpp:13
main
int main(int argc, char *argv[])
Definition: clap.cpp:59
manager
manager
i
png_uint_32 i
Definition: png.h:2735
Z
Definition: clap.cpp:13
hrp::getOnlineViewer
HRP_UTIL_EXPORT OpenHRP::OnlineViewer_var getOnlineViewer(int argc, char **argv)
Definition: hrpUtil/OnlineViewerUtil.cpp:61
checkCorbaServer
X_ptr checkCorbaServer(std::string n, CosNaming::NamingContext_var &cxt)
Definition: clap.cpp:19

openhrp3
Author(s): AIST, General Robotix Inc., Nakamura Lab of Dept. of Mechano Informatics at University of Tokyo
autogenerated on Sat Apr 13 2019 02:14:20