PathPlannerSVC_impl.cpp
Go to the documentation of this file.
1 // -*-C++-*-
8 #include "PathPlannerSVC_impl.h"
9 
10 #include <hrpPlanner/Roadmap.h>
11 #include <hrpPlanner/RoadmapNode.h>
12 #include <hrpPlanner/TimeUtil.h>
13 
14 /*
15  * Implementational code for IDL interface OpenHRP::PathPlanner
16  */
18 {
22  cspace->unboundedRotation(2, true);
23  cspace->bounds(0,-2,2);
24  cspace->bounds(1,-2,2);
25 }
26 
27 
29 {
30  delete path_;
31 }
32 
34 {
36 }
37 
38 /*
39  * Methods corresponding to IDL attributes and operations
40  */
41 void OpenHRP_PathPlannerSVC_impl::getRoadmap(OpenHRP::PathPlanner::Roadmap_out graph)
42 {
43  std::cout << "getRoadmap()" << std::endl;
44 
46 
47  graph = new OpenHRP::PathPlanner::Roadmap;
48  std::cout << "the number of nodes = " << roadmap->nNodes() << std::endl;
49  graph->length(roadmap->nNodes());
50 
51  for (unsigned int i=0; i<roadmap->nNodes(); i++) {
52  PathEngine::RoadmapNodePtr node = roadmap->node(i);
53  const PathEngine::Configuration& pos = node->position();
54  graph[i].cfg[0] = pos.value(0);
55  graph[i].cfg[1] = pos.value(1);
56  graph[i].cfg[2] = pos.value(2);
57 
58  graph[i].neighbors.length(node->nChildren());
59  for (unsigned int j=0; j<node->nChildren(); j++) {
60  graph[i].neighbors[j] = roadmap->indexOfNode(node->child(j));
61  }
62  }
63 }
64 
66 {
68  rm->clear();
69 }
70 
71 void OpenHRP_PathPlannerSVC_impl::getMobilityNames(OpenHRP::StringSequence_out mobilities)
72 {
73  mobilities = new OpenHRP::StringSequence;
74  std::vector<std::string> mobilityNames;
75  path_->getMobilityNames(mobilityNames);
76  mobilities->length(mobilityNames.size());
77 
78  for (unsigned int i=0; i<mobilityNames.size(); i++) {
79  mobilities[i] = CORBA::string_dup(mobilityNames[i].c_str());
80  }
81 }
82 
83 void OpenHRP_PathPlannerSVC_impl::getOptimizerNames(OpenHRP::StringSequence_out optimizers)
84 {
85  optimizers = new OpenHRP::StringSequence;
86  std::vector<std::string> optimizerNames;
87  path_->getOptimizerNames(optimizerNames);
88  optimizers->length(optimizerNames.size());
89 
90  for (unsigned int i=0; i<optimizerNames.size(); i++) {
91  optimizers[i] = CORBA::string_dup(optimizerNames[i].c_str());
92  }
93 }
94 
96 {
97  path_->setRobotName(model);
98 }
99 
101 {
102  path_->setAlgorithmName(algorithm);
103 }
104 
106 {
107  return path_->setMobilityName(mobility);
108 }
109 
110 void OpenHRP_PathPlannerSVC_impl::getAlgorithmNames(OpenHRP::StringSequence_out algos)
111 {
112  algos = new OpenHRP::StringSequence;
113  std::vector<std::string> algoNames;
114  path_->getAlgorithmNames(algoNames);
115  algos->length(algoNames.size());
116 
117  for (unsigned int i=0; i<algoNames.size(); i++) {
118  algos[i] = CORBA::string_dup(algoNames[i].c_str());
119  }
120 }
121 
122 bool OpenHRP_PathPlannerSVC_impl::getProperties(const char* alg, OpenHRP::StringSequence_out props, OpenHRP::StringSequence_out defaults)
123 {
124  props = new OpenHRP::StringSequence;
125  defaults = new OpenHRP::StringSequence;
126 
127 
128  std::vector<std::string> propNames;
129  std::vector<std::string> defaultValues;
130 
131  propNames.push_back("weight-x");
132  defaultValues.push_back("1.0");
133  propNames.push_back("weight-y");
134  defaultValues.push_back("1.0");
135  propNames.push_back("weight-theta");
136  defaultValues.push_back("1.0");
137 
138  propNames.push_back("min-x");
139  defaultValues.push_back("-2");
140  propNames.push_back("min-y");
141  defaultValues.push_back("-2");
142  propNames.push_back("max-x");
143  defaultValues.push_back("2");
144  propNames.push_back("max-y");
145  defaultValues.push_back("2");
146 
147  if (!path_->getProperties(alg, propNames, defaultValues)) return false;
148 
149  props->length(propNames.size());
150  defaults->length(propNames.size());
151 
152  for (unsigned int i=0; i<propNames.size(); i++) {
153  props[i] = CORBA::string_dup(propNames[i].c_str());
154  defaults[i] = CORBA::string_dup(defaultValues[i].c_str());
155  }
156  return true;
157 }
158 
160 {
161  std::cout << "initPlanner()" << std::endl;
163  std::cout << "fin. " << std::endl;
164 }
165 
166 void OpenHRP_PathPlannerSVC_impl::setStartPosition(CORBA::Double x, CORBA::Double y, CORBA::Double theta)
167 {
168  std::cout << "setStartPosition(" << x << ", " << y << ", " << theta << ")" << std::endl;
170  pos.value(0) = x;
171  pos.value(1) = y;
172  pos.value(2) = theta;
174  std::cout << "fin. " << std::endl;
175 }
176 
177 void OpenHRP_PathPlannerSVC_impl::setGoalPosition(CORBA::Double x, CORBA::Double y, CORBA::Double theta)
178 {
179  std::cout << "setGoalPosition(" << x << ", " << y << ", " << theta << ")" << std::endl;
181  pos.value(0) = x;
182  pos.value(1) = y;
183  pos.value(2) = theta;
185  std::cout << "fin. " << std::endl;
186 }
187 
188 void OpenHRP_PathPlannerSVC_impl::setProperties(const OpenHRP::PathPlanner::Property& properites)
189 {
190  std::cout << "setProperties()" << std::endl;
191  std::map<std::string, std::string> prop;
192  for (unsigned int i=0; i<properites.length(); i++) {
193  std::string name(properites[i][0]);
194  std::string value(properites[i][1]);
197  //std::cout << name << ": " << value << std::endl;
198  if (name == "min-x"){
199  cspace->lbound(0) = atof(value.c_str());
200  }else if (name == "max-x"){
201  cspace->ubound(0) = atof(value.c_str());
202  }else if (name == "min-y"){
203  cspace->lbound(1) = atof(value.c_str());
204  }else if (name == "max-y"){
205  cspace->ubound(1) = atof(value.c_str());
206  }else if (name == "weight-x"){
207  cspace->weight(0) = atof(value.c_str());
208  }else if (name == "weight-y"){
209  cspace->weight(1) = atof(value.c_str());
210  }else if (name == "weight-theta"){
211  cspace->weight(2) = atof(value.c_str());
212  }else{
213  prop.insert(std::map<std::string, std::string>::value_type(name, value));
214  }
215  }
216  path_->setProperties(prop);
217 
218  std::cout << "fin. " << std::endl;
219 }
220 
222 {
223  std::cout << "OpenHRP_PathPlannerSVC_impl::calcPath()" << std::endl;
224  tick_t t1 = get_tick();
225  bool status = path_->calcPath();
226  std::cout << "OpenHRP_PathPlannerSVC_impl::fin." << std::endl;
227  std::cout << "total computation time = " << tick2sec(get_tick()-t1) << "[s]"
228  << std::endl;
229  std::cout << "computation time for collision check = "
230  << path_->timeCollisionCheck() << "[s]" << std::endl;
231  std::cout << "computation time for forward kinematics = "
232  << path_->timeForwardKinematics() << "[s]" << std::endl;
233  std::cout << "collision check function was called "
234  << path_->countCollisionCheck() << " times" << std::endl;
235  return status;
236 }
237 
238 
239 void OpenHRP_PathPlannerSVC_impl::getPath(OpenHRP::PathPlanner::PointArray_out path)
240 {
241  std::cerr << "OpenHRP_PathPlannerSVC_impl::getPath()" << std::endl;
242  const std::vector<PathEngine::Configuration>& p = path_->getPath();
243 
244  path = new OpenHRP::PathPlanner::PointArray;
245  path->length(p.size());
246  std::cout << "length of path = " << p.size() << std::endl;
247  for (unsigned int i=0; i<p.size(); i++) {
248  //std::cerr << i << " : " << p[i] << std::endl;
249  path[i].length(3);
250  path[i][0] = p[i].value(0);
251  path[i][1] = p[i].value(1);
252  path[i][2] = p[i].value(2);
253  }
254  std::cerr << "OpenHRP_PathPlannerSVC_impl::fin. length of path = "
255  << p.size() << std::endl;
256 }
257 
258 void OpenHRP_PathPlannerSVC_impl::registerIntersectionCheckPair(const char* char1, const char* name1, const char* char2, const char* name2, CORBA::Double tolerance)
259 {
261  name1,
262  char2,
263  name2,
264  tolerance);
265 }
266 
267 void OpenHRP_PathPlannerSVC_impl::registerCharacter(const char* name, OpenHRP::BodyInfo_ptr cInfo)
268 {
269  path_->registerCharacter(name, cInfo);
270 }
271 
273 {
274  path_->registerCharacterByURL(name, url);
275 }
276 
278  const OpenHRP::DblSequence& pos)
279 {
280  path_->setCharacterPosition(character, pos);
281 }
282 
283 
285 {
286  std::cout << "initSimulation()" << std::endl;
288  std::cout << "fin. " << std::endl;
289 }
290 
291 // End of example implementational code
292 
293 
294 CORBA::Boolean OpenHRP_PathPlannerSVC_impl::optimize(const char *optimizer)
295 {
296  return path_->optimize(optimizer);
297 }
void registerCharacterByURL(const char *name, const char *url)
void setRobotName(const char *model)
void initPlanner(const std::string &nameServer)
初期化。NameServer, ModelLoaderとの接続を行う。
unsigned int size()
get the number of degrees of freedom
unsigned long long tick_t
Definition: TimeUtil.h:9
void setCharacterPosition(const char *character, const OpenHRP::DblSequence &pos)
bool setMobilityName(const std::string &mobility)
使用する移動能力を設定する
CORBA::Boolean optimize(const char *optimizer)
* x
Definition: IceUtils.h:98
ConfigurationSpace * getConfigurationSpace()
コンフィギュレーション空間設定を取得する
void getAlgorithmNames(OpenHRP::StringSequence_out algos)
void setStartPosition(CORBA::Double x, CORBA::Double y, CORBA::Double theta)
png_infop png_charpp name
Definition: png.h:2382
png_voidp int value
Definition: png.h:2113
void setGoalConfiguration(const Configuration &pos)
ゴール位置を設定する
void getPath(OpenHRP::PathPlanner::PointArray_out path)
bool setMobilityName(const char *mobility)
void setProperties(const OpenHRP::PathPlanner::Property &properites)
png_uint_32 i
Definition: png.h:2735
bool optimize(const std::string &optimizer)
指定した方法で経路を最適化する
void setProperties(const std::map< std::string, std::string > &properties)
アルゴリズムに対して各種情報を設定する
void getOptimizerNames(std::vector< std::string > &optimizers)
経路最適化アルゴリズム名の一覧を取得する
void unboundedRotation(unsigned int i_rank, bool i_flag)
specify i th degree of freedom is unbounded rotaion or not. default is false
void getRoadmap(OpenHRP::PathPlanner::Roadmap_out graph)
double timeCollisionCheck() const
干渉チェックに使用した時間[s]を取得する
bool getProperties(const std::string &algorithm, std::vector< std::string > &names, std::vector< std::string > &values)
経路計画アルゴリズムのプロパティ名一覧を取得する
void setAlgorithmName(const char *algorithm)
boost::shared_ptr< RoadmapNode > RoadmapNodePtr
Definition: RoadmapNode.h:11
void initSimulation()
動力学シミュレータを初期化する
hrp::BodyPtr registerCharacterByURL(const char *name, const char *url)
キャラクタを動力学シミュレータに登録する。
void setRobotName(const std::string &model)
移動動作の設計対象にするモデルを設定する
unsigned int countCollisionCheck() const
干渉チェックを呼び出した回数を取得する
void setCharacterPosition(const char *character, const OpenHRP::DblSequence &pos)
位置を設定する
double & weight(unsigned int i_rank)
get weight for i_rank th element
void getMobilityNames(std::vector< std::string > &mobilitys)
移動能力名の一覧を取得する
void setGoalPosition(CORBA::Double x, CORBA::Double y, CORBA::Double theta)
RoadmapPtr getRoadmap()
ロードマップを取得する
void bounds(unsigned int i_rank, double min, double max)
set bounds for i_rank th element
PathEngine::PathPlanner * path_
prop
tick_t get_tick()
Definition: TimeUtil.cpp:6
#define tick2sec(t)
Definition: TimeUtil.h:18
hrp::BodyPtr registerCharacter(const char *name, OpenHRP::BodyInfo_ptr cInfo)
キャラクタを動力学シミュレータに登録する。
void getOptimizerNames(OpenHRP::StringSequence_out optimizers)
void stopPlanning()
計算を中止する
const double value(unsigned int i_rank) const
double & lbound(unsigned int i_rank)
get lower bound of i_rank th element
bool calcPath()
経路計画を行う
void getAlgorithmNames(std::vector< std::string > &algorithms)
経路計画アルゴリズム名の一覧を取得する
void setStartConfiguration(const Configuration &pos)
スタート位置を設定する
Service implementation header of PathPlanner.idl.
bool getProperties(const char *alg, OpenHRP::StringSequence_out props, OpenHRP::StringSequence_out defaults)
std::vector< Configuration > getPath()
計画された経路の補間されたものを取得する
double & ubound(unsigned int i_rank)
get upper bound for i_rank th element
void registerIntersectionCheckPair(const char *char1, const char *name1, const char *char2, const char *name2, CORBA::Double tolerance)
衝突検出ペアを設定する
void registerIntersectionCheckPair(const char *char1, const char *name1, const char *char2, const char *name2, CORBA::Double tolerance)
boost::shared_ptr< Roadmap > RoadmapPtr
Definition: Roadmap.h:13
void registerCharacter(const char *name, OpenHRP::BodyInfo_ptr cInfo)
* y
Definition: IceUtils.h:97
void getMobilityNames(OpenHRP::StringSequence_out mobilities)
void setAlgorithmName(const std::string &algorithm)
使用する経路計画アルゴリズム名を指定する


openhrp3
Author(s): AIST, General Robotix Inc., Nakamura Lab of Dept. of Mechano Informatics at University of Tokyo
autogenerated on Thu Sep 8 2022 02:24:04