GrxPathPlanningAlgorithmItem.java
Go to the documentation of this file.
00001 /*
00002  *  GrxPathPlanningAlgorithmItem.java
00003  *
00004  *  Copyright (C) 2007 s-cubed, Inc.
00005  *  All Rights Reserved
00006  *
00007  *  @author keisuke kobayashi (s-cubed, Inc.)
00008  */
00009  
00010 package com.generalrobotix.ui.item;
00011 
00012 import java.io.File;
00013 import java.lang.reflect.InvocationTargetException;
00014 import java.util.Collection;
00015 import java.util.List;
00016 import java.util.Vector;
00017 
00018 import org.eclipse.core.runtime.IProgressMonitor;
00019 import org.eclipse.jface.dialogs.MessageDialog;
00020 import org.eclipse.jface.dialogs.ProgressMonitorDialog;
00021 import org.eclipse.jface.operation.IRunnableWithProgress;
00022 import org.eclipse.swt.widgets.Display;
00023 
00024 import jp.go.aist.hrp.simulator.BodyInfo;
00025 import jp.go.aist.hrp.simulator.DynamicsSimulator;
00026 import jp.go.aist.hrp.simulator.PathConsumerComp;
00027 import jp.go.aist.hrp.simulator.PathPlanner;
00028 import jp.go.aist.hrp.simulator.StringSequenceHolder;
00029 import jp.go.aist.hrp.simulator.WorldStateHolder;
00030 import jp.go.aist.hrp.simulator.DynamicsSimulatorPackage.LinkDataType;
00031 import jp.go.aist.hrp.simulator.PathPlannerPackage.PointArrayHolder;
00032 import jp.go.aist.hrp.simulator.PathPlannerPackage.RoadmapHolder;
00033 import jp.go.aist.hrp.simulator.PathPlannerPackage.RoadmapNode;
00034 import jp.go.aist.rtm.RTC.Manager;
00035 
00036 import com.generalrobotix.ui.GrxBaseItem;
00037 import com.generalrobotix.ui.GrxPluginManager;
00038 import com.generalrobotix.ui.grxui.Activator;
00039 import com.generalrobotix.ui.grxui.GrxUIPerspectiveFactory;
00040 import com.generalrobotix.ui.item.GrxWorldStateItem.WorldStateEx;
00041 import com.generalrobotix.ui.util.GrxDebugUtil;
00042 import com.generalrobotix.ui.util.MessageBundle;
00043 
00044 @SuppressWarnings("serial")
00048 public class GrxPathPlanningAlgorithmItem extends GrxBaseItem {
00049 
00050         public static final String TITLE = "PPAlgorithm";
00051         public static final String FILE_EXTENSION = "ppa";
00052         
00053         // 経路計画コンポーネント
00054         private PathPlanner planner_ = null;
00055         private static PathConsumerComp ppcomp_ = null;
00056         private boolean connectChange_ = false;
00057 
00058         public GrxPathPlanningAlgorithmItem(String name, GrxPluginManager manager) {
00059                 super(name, manager);
00060 
00061                 setExclusive(true);
00062 
00063                 setProperty("rebuildRoadmap", "true");
00064                 setProperty("carpetZ", "0.01");
00065                 setProperty("tolerance", "0.0");
00066                 
00067                 _pathConsumer();
00068                 
00069                 Runnable run = new Runnable(){
00070                         public void run() {     
00071                                 if(connectChange_){
00072                                         notifyObservers("connected");
00073                                         connectChange_ = false;
00074                                 }
00075                                 Display display = Display.getCurrent();
00076                                 if ( display!=null && !display.isDisposed()){
00077                                         display.timerExec(100, this);
00078                                 }
00079                         }
00080                 };
00081                 Display display = Display.getCurrent();
00082                 if ( display!=null && !display.isDisposed()){
00083                         display.timerExec(100, run);
00084                 }
00085         }
00086 
00087         public boolean create() {
00088                 return true;
00089         }
00090         
00091         @SuppressWarnings("unchecked")
00092         public void delete(){
00093                 Collection<GrxPathPlanningAlgorithmItem> col=(Collection<GrxPathPlanningAlgorithmItem>) manager_.getItemMap(GrxPathPlanningAlgorithmItem.class).values();
00094                 if(col.size()==1 && col.contains(this))
00095                         Manager.instance().deleteComponent(ppcomp_.getInstanceName());
00096                 ppcomp_ = null;
00097                 super.delete(); 
00098         }
00099         
00100         public boolean propertyChanged(String key, String value){
00101                 if (super.propertyChanged(key, value)){
00102                         
00103                 }else{
00104                         setProperty(key, value);
00105                         return true;
00106                 }
00107                 return false;
00108         }
00109         
00110         public PathPlanner getPlanner(){
00111                 return _pathConsumer().getImpl();
00112         }
00113         
00114         private PathConsumerComp _pathConsumer(){
00115                 if (ppcomp_ == null){
00116                         execPathPlannerConsumer();
00117                 }
00118                 return ppcomp_;
00119         }
00120         
00124         private void execPathPlannerConsumer(){
00125         String [] args = {"-f", getConfigFilePath()};
00126         if( args[1].isEmpty() ){
00127             args[0] = "";
00128         }
00129         GrxDebugUtil.println("[GrxPathPlanner] RTC SERVICE CONSUMER THREAD START"); //$NON-NLS-1$
00130 
00131         // Initialize manager
00132         final Manager manager = Manager.init(args);
00133 
00134         // Set module initialization procedure
00135         // This procedure will be invoked in activateManager() function.
00136         ppcomp_ = new PathConsumerComp();
00137         manager.setModuleInitProc(ppcomp_);
00138 
00139         // Activate manager and register to naming service
00140         manager.activateManager();
00141 
00142         // run the manager in blocking mode
00143         // runManager(false) is the default.
00144         //manager.runManager();
00145 
00146         // If you want to run the manager in non-blocking mode, do like this
00147         manager.runManager(true);
00148         
00149                 ppcomp_.setConnectedCallback(this);
00150         }
00151         
00156         private String getConfigFilePath(){
00157                 String confPath=""; //$NON-NLS-1$
00158         File defualtRtcFile = new File(Activator.getDefault().getTempDir() + File.separator + "rtc.conf");
00159         if( defualtRtcFile.isFile() ){
00160             confPath = defualtRtcFile.getPath(); 
00161         }
00162         GrxDebugUtil.println("[GrxPathPlanner] default Config File path="+confPath); //$NON-NLS-1$
00163         // From Property ( if exist )
00164                 String confPathFromProperty = System.getProperty("com.generalrobotix.grxui.rtcpath"); //$NON-NLS-1$
00165                 if( confPathFromProperty != null ) {
00166                         confPath = confPathFromProperty;
00167                 GrxDebugUtil.println("[GrxPathPlanner] Config File path from Property="+confPath); //$NON-NLS-1$
00168                 }
00169 
00170                 return confPath;
00171         }
00172         
00176         public void propertyUpdate(){
00177                 planner_ = _pathConsumer().getImpl();
00178             if( planner_ == null ){
00179                         MessageDialog.openInformation(GrxUIPerspectiveFactory.getCurrentShell(),"", MessageBundle.get("GrxPathPlanningView.dialog.messasge.notConnect")); //$NON-NLS-1$ //$NON-NLS-2$
00180                 return;
00181             }
00182 
00183             StringSequenceHolder propertyNames_ = new StringSequenceHolder();
00184                 StringSequenceHolder propertyDefaults_ = new StringSequenceHolder();
00185                 planner_.getProperties( getProperty("algorithm"), propertyNames_, propertyDefaults_ );
00186                 String[] props = propertyNames_.value;
00187                 String[] defaults = propertyDefaults_.value;
00188                 
00189                 //ppa.clear(); // プロパティを全て破棄
00190                 for( int i=0; i<props.length; i++ )
00191                         setProperty( props[i], defaults[i] );
00192         }
00193         
00198         private boolean initPathPlanner(){
00199                 GrxDebugUtil.println("[GrxPathPlanningView]@initPathPlanner" ); //$NON-NLS-1$
00200             planner_ = _pathConsumer().getImpl();
00201             if( planner_ == null ){
00202                         MessageDialog.openInformation(GrxUIPerspectiveFactory.getCurrentShell(),"", MessageBundle.get("GrxPathPlanningView.dialog.message.notConnect")); //$NON-NLS-1$ //$NON-NLS-2$
00203                 return false;
00204             }
00205 
00206             //初期化
00207                 planner_.initPlanner();
00208                 
00209                 //アルゴリズムを指定
00210                 planner_.setAlgorithmName( getProperty("algorithm") );
00211 
00212             // 全ての有効なモデルを登録
00213                 GrxDebugUtil.println("[GrxPathPlanner]@initPathPlanner register character" ); //$NON-NLS-1$
00214                 List<GrxBaseItem> models = manager_.getSelectedItemList( GrxModelItem.class );
00215                 for( GrxBaseItem model: models ){
00216                         GrxModelItem m = (GrxModelItem) model;
00217                         BodyInfo binfo = m.getBodyInfo();
00218                         if (binfo != null) {
00219                                 GrxDebugUtil.println("register "+m.getName() ); //$NON-NLS-1$
00220                         planner_.registerCharacter( m.getName(), binfo );
00221                         }else{
00222                                 GrxDebugUtil.println("not register "+m.getName() ); //$NON-NLS-1$
00223                         }
00224                 }
00225 
00226                 //全てのコリジョンペアを登録する
00227                 GrxDebugUtil.println("[GrxPathPlanningView]@initPathPlanner register collision pair" ); //$NON-NLS-1$
00228                 double t = getDbl("tolerance", 0.0);
00229                 List<GrxBaseItem> collisionPair = manager_.getSelectedItemList(GrxCollisionPairItem.class);
00230                 for(GrxBaseItem i : collisionPair) {
00231                         GrxCollisionPairItem item = (GrxCollisionPairItem) i;
00232                         planner_.registerIntersectionCheckPair(
00233                                         item.getStr("objectName1", ""), item.getStr("jointName1", ""),  //$NON-NLS-1$ //$NON-NLS-2$ //$NON-NLS-3$ //$NON-NLS-4$
00234                                         item.getStr("objectName2", ""), item.getStr("jointName2", ""),  //$NON-NLS-1$ //$NON-NLS-2$ //$NON-NLS-3$ //$NON-NLS-4$
00235                                         t);
00236                 }
00237 
00238                 //移動ロボットとして使用するmodelとrobotを指定
00239                 String modelName = getProperty("model");
00240                 if( modelName == null || modelName.equals("") ){ //$NON-NLS-1$
00241                         MessageDialog.openInformation(GrxUIPerspectiveFactory.getCurrentShell(),"", MessageBundle.get("GrxPathPlanningView.dialog.message.model")); //$NON-NLS-1$ //$NON-NLS-2$
00242                         return false;
00243                 }
00244                 String mobilityName = getProperty("mobility");
00245                 if( mobilityName == null || mobilityName.equals("") ){ //$NON-NLS-1$
00246                         MessageDialog.openInformation(GrxUIPerspectiveFactory.getCurrentShell(),"", MessageBundle.get("GrxPathPlanningView.dialog.message.mobility")); //$NON-NLS-1$ //$NON-NLS-2$
00247                         return false;
00248                 }
00249                 planner_.setMobilityName(mobilityName);
00250                 GrxDebugUtil.println("[GrxPathPlanningView]@initPathPlanner set robot name" ); //$NON-NLS-1$
00251                 planner_.setRobotName(modelName);
00252                 
00253 
00254                 // Algorithm Property
00255                 String[][]p = getAlgoProperty();
00256                 if( p == null )
00257                         return false;
00258                 GrxDebugUtil.println("[GrxPathPlanningView]@initPathPlanner set property" ); //$NON-NLS-1$
00259                 planner_.setProperties( p );
00260 
00261                 GrxDebugUtil.println("[GrxPathPlanningView]@initPathPlanner initialize simulation" ); //$NON-NLS-1$
00262                 planner_.initSimulation();
00263 
00264                 for( GrxBaseItem model: models ){
00265                         GrxModelItem m = (GrxModelItem) model;
00266                         BodyInfo binfo = m.getBodyInfo();
00267                         if (binfo != null) {
00268                             double v[] = m.getTransformArray(m.rootLink());
00269                             GrxDebugUtil.println(m.getName()+":"+v[0]+" "+v[1]+" "+v[2]); //$NON-NLS-1$ //$NON-NLS-2$ //$NON-NLS-3$
00270                             planner_.setCharacterPosition(m.getName(), v);
00271                         }
00272                 }
00273 
00274                 GrxDebugUtil.println("[GrxPathPlanningView] Planner initialize Succeed"); //$NON-NLS-1$
00275                 return true;
00276         }
00277         
00278         private String[][] getAlgoProperty(){
00279                 // 値の入っているパラメータを取得
00280                 Vector<String[]> tmp = new Vector<String[]>();
00281                 for( Object s : keySet() ){
00282                         if( ! getStr( s.toString() ).equals("") ){ //$NON-NLS-1$
00283                                 String []tmp2 = { s.toString(), getStr( s.toString() ) }; 
00284                                 tmp.add( tmp2 );
00285                         }
00286                 }
00287                 String [][]params = new String[tmp.size()+1][2];
00288                 for( int i=0; i<tmp.size(); i++ )
00289                         params[i] = tmp.get(i);
00290                 
00291                 // Z position from input
00292                 Double z = getZPosition();
00293                 if( z == null ){
00294                         MessageDialog.openInformation(GrxUIPerspectiveFactory.getCurrentShell(),"", MessageBundle.get("GrxPathPlanningView.dialog.message.invalidRobot")); //$NON-NLS-1$ //$NON-NLS-2$
00295                         return null;
00296                 }
00297                         
00298                 params[ params.length-1 ][0] = "z-pos"; //$NON-NLS-1$
00299                 params[ params.length-1 ][1] = String.valueOf( getZPosition() );
00300 
00301                 return params;
00302         }
00303         
00308         Double getZPosition(){
00309                 String modelName = getProperty("model");
00310                 GrxModelItem robot = (GrxModelItem)(manager_.getItem( GrxModelItem.class, modelName ));
00311                 if (robot == null){
00312                         return 0d;
00313                 }else{
00314                         double [] tr = robot.getDblAry(getRobotBaseLink(robot)+".translation",null); //$NON-NLS-1$
00315                         return tr[2];
00316                 }
00317         }
00318 
00319         private String getRobotBaseLink(String name ) {
00320                  GrxModelItem model = (GrxModelItem) manager_.getItem( GrxModelItem.class, name );
00321                 return model.rootLink().getName();
00322         }
00323         
00324         private String getRobotBaseLink( GrxModelItem model ) {
00325                 return model.rootLink().getName();
00326         }
00327         
00332         private boolean _setStartPosition(){
00333                 // Start POSITION
00334                 double sx = 0,sy = 0,st = 0;
00335                 try{
00336                         sx = getDbl("startX", 0.0);
00337                         sy = getDbl("startY", 0.0);
00338                         st = getDbl("startTheta", 0.0)/ 360f * (2*Math.PI);
00339                         while (st < 0) st += 2*Math.PI;
00340                         while (st > 2*Math.PI) st -= 2*Math.PI;
00341                 }catch(Exception e){
00342                         MessageDialog.openInformation(GrxUIPerspectiveFactory.getCurrentShell(),"", MessageBundle.get("GrxPathPlanningView.dialog.message.invalidStart")); //$NON-NLS-1$ //$NON-NLS-2$
00343                         return false;
00344                 }
00345                 GrxDebugUtil.println("[GrxPathPlanningView]@initPathPlanner set start" ); //$NON-NLS-1$
00346                 planner_.setStartPosition( sx,sy,st );
00347                 return true;
00348         }
00349         
00350 
00355         private boolean _setGoalPosition(){
00356                 // GOAL Position
00357                 double ex = 0,ey = 0,et = 0;
00358                 try{
00359                         ex = getDbl("goalX", 0.0);
00360                         ey = getDbl("goalY", 0.0);
00361                         et = getDbl("goalTheta", 0.0) / 360f * (2*Math.PI);
00362                         while (et < 0) et += 2*Math.PI;
00363                         while (et > 2*Math.PI) et -= 2*Math.PI;
00364                 }catch(Exception e){
00365                         MessageDialog.openInformation(GrxUIPerspectiveFactory.getCurrentShell(),"", MessageBundle.get("GrxPathPlanningView.dialog.message.InvalidEnd")); //$NON-NLS-1$ //$NON-NLS-2$
00366                         return false;
00367                 }
00368                 GrxDebugUtil.println("[GrxPathPlanningView]@initPathPlanner set end" ); //$NON-NLS-1$
00369                 planner_.setGoalPosition( ex,ey,et);
00370                 return true;
00371         }
00372 
00373         public void setStartPoint(){
00374                 double sx = 0,sy = 0,st = 0;
00375 
00376                 try{
00377                         sx = getDbl("startX", 0.0);
00378                         sy = getDbl("startY", 0.0);
00379                         st = getDbl("startTheta", 0.0)/ 360f * (2*Math.PI);
00380                 }catch(Exception e){
00381                         return;
00382                 }
00383 
00384                 String modelName = getProperty("model");
00385                 GrxModelItem robot = (GrxModelItem)(manager_.getItem( GrxModelItem.class, modelName ));
00386                 if( robot == null )
00387                         return;
00388                 robot.propertyChanged(getRobotBaseLink(robot)+".translation", ""+sx+" "+sy+" "+getZPosition()); //$NON-NLS-1$ //$NON-NLS-2$ //$NON-NLS-3$ //$NON-NLS-4$
00389                 robot.propertyChanged(getRobotBaseLink(robot)+".rotation", "0.0 0.0 1.0 "+st); //$NON-NLS-1$ //$NON-NLS-2$
00390         }
00391 
00392         public void setEndPoint(){
00393                 double ex = 0,ey = 0,et = 0;
00394 
00395                 try{
00396                         ex = getDbl("goalX", 0.0);
00397                         ey = getDbl("goalY", 0.0);
00398                         et = getDbl("goalTheta", 0.0) / 360f * (2*Math.PI);
00399                 }catch(Exception e){
00400                         return;
00401                 }
00402 
00403                 String modelName = getProperty("model");
00404                 GrxModelItem robot = (GrxModelItem)(manager_.getItem( GrxModelItem.class, modelName ));
00405                 robot.propertyChanged(getRobotBaseLink(robot)+".translation", ""+ex+" "+ey+" "+getZPosition() ); //$NON-NLS-1$ //$NON-NLS-2$ //$NON-NLS-3$ //$NON-NLS-4$
00406                 robot.propertyChanged(getRobotBaseLink(robot)+".rotation", "0.0 0.0 1.0 "+et); //$NON-NLS-1$ //$NON-NLS-2$
00407         }
00408         
00409         public void startCalc(){
00410             planner_ = _pathConsumer().getImpl();
00411             if( planner_ == null ){
00412                         MessageDialog.openInformation(GrxUIPerspectiveFactory.getCurrentShell(),"", MessageBundle.get("GrxPathPlanningView.dialog.message.notConnect")); //$NON-NLS-1$ //$NON-NLS-2$
00413                 return;
00414             }
00415 
00416         if ( getProperty("rebuildRoadmap").equals("true")){
00417                 if (!initPathPlanner()){
00418                         return;
00419                 }
00420         }
00421         _setStartPosition();
00422         _setGoalPosition();
00423         
00424         //ロボットをスタート位置に移動
00425         setStartPoint();
00426         calc();
00427         }
00428         
00429         private boolean calcSucceed=false;
00430         private void calc(){
00431                 GrxDebugUtil.println("[GrxPathPlanner]@calc" ); //$NON-NLS-1$
00432                 calcSucceed = false;
00433                 IRunnableWithProgress runnableProgress = new IRunnableWithProgress() {
00434                         public void run(IProgressMonitor monitor) throws InterruptedException {
00435                                 Thread calcThread = new Thread( new Runnable(){
00436                                         public void run(){
00437                                         calcSucceed = planner_.calcPath();
00438                                         }
00439                                 });
00440                                 GrxDebugUtil.println("[GrxPathPlanner]@calc Thread Start" ); //$NON-NLS-1$
00441                                 calcThread.start();
00442                                 monitor.beginTask("Planning a path. Please Wait.", IProgressMonitor.UNKNOWN ); //$NON-NLS-1$
00443                                 while( calcThread.isAlive() ){
00444                                         Thread.sleep(200);
00445                                         GrxDebugUtil.print("." ); //$NON-NLS-1$
00446                                         if( monitor.isCanceled() ){
00447                                                 GrxDebugUtil.println("[GrxPathPlanner]@calc Cancel" ); //$NON-NLS-1$
00448                                                 planner_.stopPlanning();
00449                                                 break;
00450                                         }
00451                                 }
00452                                 monitor.done();
00453                         }
00454                 };
00455                 ProgressMonitorDialog progressMonitorDlg = new ProgressMonitorDialog( GrxUIPerspectiveFactory.getCurrentShell() );
00456                 try {
00457                         progressMonitorDlg.run(true,true, runnableProgress);
00458                 } catch (InvocationTargetException e) {
00459                         e.printStackTrace();
00460                         return;
00461                 } catch (InterruptedException e) {
00462                         e.printStackTrace();
00463                         return;
00464                 }
00465                 
00466                 if( calcSucceed == false ){
00467                         MessageDialog.openInformation( GrxUIPerspectiveFactory.getCurrentShell(), MessageBundle.get("GrxPathPlanningView.dialog.message.cancel0"), MessageBundle.get("GrxPathPlanningView.dialog.message.cancel1")); //$NON-NLS-1$ //$NON-NLS-2$
00468                 }
00469                 
00470                 displayPath();
00471         }
00472 
00476         public void optimize() {
00477                 planner_ = _pathConsumer().getImpl();
00478             if( planner_ == null ){
00479                         MessageDialog.openInformation(GrxUIPerspectiveFactory.getCurrentShell(),"", MessageBundle.get("GrxPathPlanningView.dialog.message.notConnect")); //$NON-NLS-1$ //$NON-NLS-2$
00480                 return;
00481             }
00482             
00483                 String optimizer = getProperty("optimizer");
00484                 if( optimizer == null || optimizer.equals("") ){ //$NON-NLS-1$
00485                         MessageDialog.openInformation(GrxUIPerspectiveFactory.getCurrentShell(),"", MessageBundle.get("GrxPathPlanningView.dialog.mesage.notSelect")); //$NON-NLS-1$ //$NON-NLS-2$
00486                         return;
00487                 }
00488 
00489             planner_.optimize(optimizer);
00490             displayPath();
00491         }
00492 
00493         private void displayPath() {
00494                 PointArrayHolder path = new PointArrayHolder();
00495         planner_.getPath( path );
00496                 GrxDebugUtil.println("[GrxPathPlanningView] Path length="+path.value.length ); //$NON-NLS-1$
00497 
00498         // TODO:DynamicsSimulatorのdefault simulation time 20sec オーバすると落ちる
00499         double dt = 10.0 / path.value.length, nowTime = 0;
00500         GrxWorldStateItem currentWorld_ = (GrxWorldStateItem)manager_.getItem( GrxWorldStateItem.class, null );
00501         if( currentWorld_ == null ) {
00502                 GrxDebugUtil.printErr("[GrxPathPlanningView] There is no World."); //$NON-NLS-1$
00503                 return;
00504         }
00505         currentWorld_.clearLog();
00506         currentWorld_.setDbl("logTimeStep", dt); //$NON-NLS-1$
00507                 DynamicsSimulator dynSim = getDynamicsSimulator();
00508                 if( dynSim == null ){
00509                 GrxDebugUtil.printErr("[GrxPathPlanningView] Faild to get DynamicsSimulator."); //$NON-NLS-1$
00510                 return;
00511                 }
00512 
00513         // register robot
00514                 String model = getProperty("model");
00515                 String base = getRobotBaseLink(model);
00516             WorldStateHolder stateH_ = new WorldStateHolder();
00517         for( double p[] : path.value ) {
00518                 //System.out.println("[GrxPathPlanner] x="+p[0]+" y="+p[1]+" theta="+p[2] );
00519                 double newPos[] = new double[12];
00520                 newPos[0] = p[0];            newPos[1] = p[1];            newPos[2] = getZPosition();
00521                 newPos[0 + 3] = Math.cos( p[2] ); newPos[1 + 3] = -Math.sin( p[2] ); newPos[2 + 3] = 0.0;
00522                 newPos[3 + 3] = Math.sin( p[2] ); newPos[4 + 3] =  Math.cos( p[2] ); newPos[5 + 3] = 0.0;
00523                 newPos[6 + 3] = 0.0;         newPos[7 + 3] = 0.0;         newPos[8 + 3] = 1.0;
00524 
00525                 dynSim.setCharacterLinkData( model, base, LinkDataType.ABS_TRANSFORM, newPos);
00526 
00527                 dynSim.calcWorldForwardKinematics();
00528 
00529             // 結果を得る
00530                 dynSim.getWorldState(stateH_);
00531                         WorldStateEx wsx = new WorldStateEx(stateH_.value);
00532 
00533                         wsx.time = nowTime;
00534             currentWorld_.addValue(nowTime, wsx);
00535                         nowTime += dt;
00536         }
00537         System.out.println("worldstate.getLogSize() = "+currentWorld_.getLogSize()); //$NON-NLS-1$
00538         currentWorld_.setPosition(currentWorld_.getLogSize()-1);
00539         }
00540 
00541         // 経路計画サーバへ渡すためDynamicsSimulatorを取得
00542         DynamicsSimulator getDynamicsSimulator(){
00543                 GrxSimulationItem simItem = manager_.<GrxSimulationItem>getSelectedItem(GrxSimulationItem.class, null);
00544                 if(simItem==null){
00545                         simItem = (GrxSimulationItem)manager_.createItem(GrxSimulationItem.class, null);
00546                         manager_.itemChange(simItem, GrxPluginManager.ADD_ITEM);
00547                         manager_.setSelectedItem(simItem, true);
00548                 }
00549                 if( ! simItem.initDynamicsSimulator() ){
00550                         MessageDialog.openInformation(GrxUIPerspectiveFactory.getCurrentShell(),"", MessageBundle.get("GrxPathPlanningView.dialog.message.Dynamics")); //$NON-NLS-1$ //$NON-NLS-2$
00551                         return null;
00552                 }
00553                 return simItem.getDynamicsSimulator(false);
00554         }
00555 
00556         public String[] getAlgorithms(){
00557                 if(planner_==null)
00558                         return new String[0];
00559                 StringSequenceHolder names = new StringSequenceHolder();
00560                 // planning algorithms
00561                 planner_.getAlgorithmNames( names );
00562                 return  names.value;
00563         }
00564         
00565         public String[] getMobilityNames(){
00566                 if(planner_==null)
00567                         return new String[0];
00568                 StringSequenceHolder names = new StringSequenceHolder();
00569                 planner_.getMobilityNames( names );
00570                 return names.value;
00571         }
00572         
00573         public String[] getOptimizerNames(){
00574                 if(planner_==null)
00575                         return new String[0];
00576                 StringSequenceHolder names = new StringSequenceHolder();
00577                 planner_.getOptimizerNames( names );
00578                 return names.value;
00579         }
00580         
00581         public RoadmapNode[] getRoadmap(){
00582                 if(planner_==null)
00583                         return new RoadmapNode[0];
00584                 RoadmapHolder h = new RoadmapHolder();
00585                 planner_.getRoadmap(h);
00586                 return (RoadmapNode[])h.value;
00587         }
00588         
00589         public double[][] getPath(){
00590                 if(planner_==null)
00591                         return new double[0][0];
00592                 PointArrayHolder path = new PointArrayHolder();
00593                 planner_.getPath( path );
00594                 return path.value;
00595         }
00596 
00597         public void connectedCallback(boolean b) {
00598                 if(b)
00599                         planner_ = _pathConsumer().getImpl();
00600                 else
00601                         planner_ = null;
00602                 connectChange_ = true;
00603         }
00604 }


openhrp3
Author(s): AIST, General Robotix Inc., Nakamura Lab of Dept. of Mechano Informatics at University of Tokyo
autogenerated on Sun Apr 2 2017 03:43:54