BehaviorManager.java
Go to the documentation of this file.
00001 /*
00002  * Copyright (c) 2008, AIST, the University of Tokyo and General Robotix Inc.
00003  * All rights reserved. This program is made available under the terms of the
00004  * Eclipse Public License v1.0 which accompanies this distribution, and is
00005  * available at http://www.eclipse.org/legal/epl-v10.html
00006  * Contributors:
00007  * General Robotix Inc.
00008  * National Institute of Advanced Industrial Science and Technology (AIST) 
00009  */
00017 package com.generalrobotix.ui.view.tdview;
00018 
00019 import java.util.Iterator;
00020 import java.util.List;
00021 import java.util.Map;
00022 import java.util.Vector;
00023 
00024 import javax.media.j3d.BranchGroup;
00025 import javax.media.j3d.Transform3D;
00026 import javax.media.j3d.TransformGroup;
00027 
00028 import org.eclipse.jface.dialogs.MessageDialog;
00029 import org.eclipse.swt.widgets.Display;
00030 
00031 import jp.go.aist.hrp.simulator.BodyInfo;
00032 import jp.go.aist.hrp.simulator.DynamicsSimulator;
00033 import jp.go.aist.hrp.simulator.DynamicsSimulatorFactory;
00034 import jp.go.aist.hrp.simulator.DynamicsSimulatorFactoryHelper;
00035 import jp.go.aist.hrp.simulator.DynamicsSimulatorPackage.IntegrateMethod;
00036 import jp.go.aist.hrp.simulator.DynamicsSimulatorPackage.LinkDataType;
00037 import jp.go.aist.hrp.simulator.DynamicsSimulatorPackage.SensorOption;
00038 import jp.go.aist.hrp.simulator.*;
00039 
00040 import com.generalrobotix.ui.GrxBaseItem;
00041 import com.generalrobotix.ui.GrxPluginManager;
00042 import com.generalrobotix.ui.item.GrxCollisionPairItem;
00043 import com.generalrobotix.ui.item.GrxLinkItem;
00044 import com.generalrobotix.ui.item.GrxModelItem;
00045 import com.generalrobotix.ui.util.Grx3DViewClickListener;
00046 import com.generalrobotix.ui.util.GrxCorbaUtil;
00047 import com.generalrobotix.ui.util.GrxDebugUtil;
00048 import com.generalrobotix.ui.util.MessageBundle;
00049 import com.generalrobotix.ui.view.Grx3DView;
00050 import com.sun.j3d.utils.picking.PickCanvas;
00051 import com.sun.j3d.utils.picking.PickTool;
00052 
00058 public class BehaviorManager implements WorldReplaceListener {
00059     //--------------------------------------------------------------------
00060     // 定数
00061     // オペレーションモード
00062     public static final int OPERATION_MODE_NONE         = 0;
00063     public static final int OBJECT_ROTATION_MODE        = 1;
00064     public static final int OBJECT_TRANSLATION_MODE     = 2;
00065     public static final int JOINT_ROTATION_MODE         = 3;
00066     public static final int FITTING_FROM_MODE           = 4;
00067     public static final int FITTING_TO_MODE             = 5;
00068     public static final int INV_KINEMA_FROM_MODE        = 6;
00069     public static final int INV_KINEMA_TRANSLATION_MODE = 7;
00070     public static final int INV_KINEMA_ROTATION_MODE    = 8;
00071 
00072     //  ビューモード
00073     public static final int WALK_VIEW_MODE     = 1;
00074     public static final int ROOM_VIEW_MODE     = 2;
00075     public static final int PARALLEL_VIEW_MODE = 3;
00076 
00077     // インスタンス変数
00078     DynamicsSimulator currentDynamics_;
00079     private IseBehavior behavior_;
00080     private IseBehaviorHandler handler_;
00081     private int viewMode_;
00082     private int operationMode_;
00083     private ThreeDDrawable drawable_;
00084     private BehaviorInfo info_;
00085     private BehaviorHandler indicator_;
00086     private GrxPluginManager manager_;
00087     private Grx3DView viewer_;
00088     private InvKinemaResolver resolver_;
00089     private boolean itemChangeFlag_ = false;
00090     private boolean messageSkip_ = false;
00091     
00092     private List<GrxModelItem> currentModels_ = null;
00093     private List<GrxCollisionPairItem> currentCollisionPairs_ = null;
00094     
00095     //--------------------------------------------------------------------
00096     // コンストラクタ
00097     public BehaviorManager(GrxPluginManager manager) {
00098         manager_ = manager;
00099         resolver_ = new InvKinemaResolver(manager_);
00100         //Simulator.getInstance().addWorldReplaceListener(this);
00101     }
00102 
00103     // 公開メソッド
00104     public void setThreeDViewer(Grx3DView viewer) {
00105         viewer_ = viewer;
00106         PickCanvas pickCanvas = new PickCanvas(
00107             viewer.getCanvas3D(),
00108             viewer.getBranchGroupRoot()
00109         );
00110         pickCanvas.setMode(PickTool.GEOMETRY_INTERSECT_INFO);
00111 
00112         handler_ = new IseBehaviorHandler(this);
00113 
00114         behavior_ = new IseBehavior(handler_);
00115  
00116         BranchGroup bg = new BranchGroup();
00117         bg.addChild(behavior_);
00118 
00119         BranchGroup bgRoot = viewer_.getBranchGroupRoot();
00120         bgRoot.addChild(bg);
00121         
00122         //drawable_ = viewer.getDrawable();
00123         drawable_ = viewer;
00124 
00125         ViewInfo viewInfo = drawable_.getViewInfo();
00126         switch (viewInfo.getViewMode()) {
00127         case ViewInfo.VIEW_MODE_ROOM:
00128             setViewMode(ROOM_VIEW_MODE);
00129             break;
00130         case ViewInfo.VIEW_MODE_WALK:
00131             setViewMode(WALK_VIEW_MODE);
00132             break;
00133         case ViewInfo.VIEW_MODE_PARALLEL:
00134             setViewMode(PARALLEL_VIEW_MODE);
00135             break;
00136         }
00137         //TransformGroup tgView = drawable_.getTransformGroupRoot();
00138         info_ = new BehaviorInfo(manager_, pickCanvas, drawable_);
00139 
00140         behavior_.setBehaviorInfo(info_);
00141     }
00142 
00143     public void setViewIndicator(BehaviorHandler handler) {
00144         indicator_ = handler;
00145     }
00146 
00147     public void setViewMode(int mode) {
00148         viewMode_ = mode;
00149         handler_.setViewMode(viewMode_);
00150     }
00151 
00152     public void setOperationMode(int mode) {
00153         operationMode_ = mode;
00154         handler_.setOperationMode(operationMode_);
00155     }
00156 
00157     public void setViewHandlerMode(String str) {
00158         handler_.setViewHandlerMode(str);
00159     }
00160 
00161     public boolean fit() {
00162         return handler_.fit(info_);
00163     }
00164 
00165     public void setPickTarget(TransformGroup tg) {
00166         BehaviorInfo info = behavior_.getBehaviorInfo();
00167         handler_.setPickTarget(tg, info);
00168     }
00169 
00170     //--------------------------------------------------------------------
00171     // ViewChangeListenerの実装
00172     public void viewChanged(ViewChangeEvent evt) {
00173         Transform3D transform = new Transform3D();
00174         evt.getTransformGroup().getTransform(transform);
00175 
00176         // データの通知は ViewNode に任せる
00177         drawable_.setTransform(transform);
00178     }
00179 
00180     //--------------------------------------------------------------------
00181     // WorldReplaceListenerの実装
00182     public void replaceWorld(List<GrxBaseItem> list) {
00183         //handler_ = new IseBehaviorHandler();
00184         //behavior_ = new IseBehavior(handler_);
00185  
00186         //BranchGroup bg = new BranchGroup();
00187         //bg.addChild(behavior_);
00188 
00189         //BranchGroup bgRoot = viewer_.getBranchGroupRoot();
00190         //bgRoot.addChild(bg);
00191 
00192         //InvKinemaResolver resolver = new InvKinemaResolver(world_);
00193         //ProjectManager project = ProjectManager.getInstance();
00194         //resolver.setDynamicsSimulator(project.getDynamicsSimulator());
00195         //handler_.setInvKinemaResolver(resolver);
00196         
00197         if (handler_ != null) 
00198                 handler_.setViewIndicator(indicator_);
00199     }
00200 
00206         public DynamicsSimulator getDynamicsSimulator(boolean update) {
00207                 //currentDynamics_ = dynamicsMap_.get(currentWorld_);
00208                 if (update && currentDynamics_ != null) {
00209             try {
00210                           currentDynamics_.destroy();
00211             } catch (Exception e) {
00212                                 GrxDebugUtil.printErr("getDynamicsSimulator: destroy failed."); //$NON-NLS-1$
00213             }
00214                         currentDynamics_ = null;
00215                 }
00216                 
00217                 if (currentDynamics_ == null) {
00218                         try {
00219                                 org.omg.CORBA.Object obj = //process_.get(DynamicsSimulatorID_).getReference();
00220                                 GrxCorbaUtil.getReference("DynamicsSimulatorFactory"); //$NON-NLS-1$
00221                                 DynamicsSimulatorFactory ifactory = DynamicsSimulatorFactoryHelper
00222                                                 .narrow(obj);
00223                                 currentDynamics_ = ifactory.create();
00224                                 currentDynamics_._non_existent();
00225                                 //dynamicsMap_.put(currentWorld_, currentDynamics_);
00226 
00227                         } catch (Exception e) {
00228                                 GrxDebugUtil.printErr("getDynamicsSimulator: create failed."); //$NON-NLS-1$
00229                                 currentDynamics_ = null;
00230                         }
00231                 }
00232                 return currentDynamics_;
00233         }
00234         
00235         public int getOperationMode() {
00236                 return operationMode_;
00237         }
00238 
00246         public boolean initDynamicsSimulator() {        
00247                 boolean ModelModified = false;
00248                 Iterator<GrxModelItem> itr = currentModels_.iterator();
00249                 while(itr.hasNext()){
00250                         GrxModelItem model = itr.next();
00251                         if(model.isModified()){
00252                                 ModelModified = true;
00253                                 if(!messageSkip_){
00254                                         final String name = model.getName();
00255                                         Display display = Display.getDefault();
00256                                         display.syncExec(new Runnable(){
00257                                                 public void run(){
00258                                                         MessageDialog.openInformation(null, "", MessageBundle.get("BehaviorManager.dialog.message.reloadModel")+name+") "); //$NON-NLS-1$ //$NON-NLS-2$ //$NON-NLS-3$
00259                                                 }
00260                                         });
00261                                 }
00262                         }
00263                         if(ModelModified) return false;
00264                 }
00265                 if(!itemChangeFlag_) return true;
00266                 if(getDynamicsSimulator(true) == null) return false;
00267 
00268                 try {
00269                         // register characters
00270                         for (int i=0; i<currentModels_.size(); i++) {
00271                                 final GrxModelItem model = currentModels_.get(i);
00272                                 BodyInfo bodyInfo = model.getBodyInfo();
00273                                 if(bodyInfo==null)      return false;
00274                                 currentDynamics_.registerCharacter(model.getName(), bodyInfo);
00275                         }
00276 
00277                         IntegrateMethod m = IntegrateMethod.EULER;
00278                         currentDynamics_.init(0.005, m, SensorOption.ENABLE_SENSOR);
00279                         currentDynamics_.setGVector(new double[] { 0.0, 0.0, 9.8});
00280                         
00281                         // set position/orientation and joint angles
00282                         for (int i=0; i<currentModels_.size(); i++) {
00283                                 GrxModelItem model = currentModels_.get(i);
00284                                 if (model.links_ == null)
00285                                         continue;
00286                                 
00287                                 GrxLinkItem base = model.rootLink();
00288                                 currentDynamics_.setCharacterLinkData(
00289                                         model.getName(), base.getName(), LinkDataType.ABS_TRANSFORM, 
00290                                         model.getTransformArray(base));
00291                                 
00292                                 currentDynamics_.setCharacterAllLinkData(
00293                                         model.getName(), LinkDataType.JOINT_VALUE, 
00294                                         model.getJointValues());
00295                         }
00296                         
00297             // set collision check pairs 
00298                         Map<String, GrxModelItem> modelmap = (Map<String, GrxModelItem>)manager_.getItemMap(GrxModelItem.class);
00299                         for (int i=0; i<currentCollisionPairs_.size(); i++) {
00300                                 GrxCollisionPairItem item = (GrxCollisionPairItem) currentCollisionPairs_.get(i);
00301                                 GrxModelItem m1 = modelmap.get(item.getStr("objectName1", "")); //$NON-NLS-1$ //$NON-NLS-2$
00302                                 GrxModelItem m2 = modelmap.get(item.getStr("objectName2", "")); //$NON-NLS-1$ //$NON-NLS-2$
00303                                 if (m1 == null || m2 == null) continue;
00304                                 Vector<GrxLinkItem> links1, links2;
00305                                 String lname1 = item.getStr("jointName1",""); //$NON-NLS-1$ //$NON-NLS-2$
00306                                 if (lname1.equals("")){ //$NON-NLS-1$
00307                                         links1 = m1.links_;
00308                                 }else{
00309                                         links1 = new Vector<GrxLinkItem>();
00310                                         GrxLinkItem l = m1.getLink(lname1);
00311                                         if (l != null) links1.add(l);
00312                                 }
00313                                 String lname2 = item.getStr("jointName2",""); //$NON-NLS-1$ //$NON-NLS-2$
00314                                 if (lname2.equals("")){ //$NON-NLS-1$
00315                                         links2 = m2.links_;
00316                                 }else{
00317                                         links2 = new Vector<GrxLinkItem>();
00318                                         GrxLinkItem l = m2.getLink(lname2);
00319                                         if (l != null) links2.add(l);
00320                                 }
00321                                 for (int j=0; j<links1.size(); j++){
00322                                         for (int k=0; k<links2.size(); k++){
00323                                                 currentDynamics_.registerIntersectionCheckPair(
00324                                                                 m1.getName(), links1.get(j).getName(),
00325                                                                 m2.getName(), links2.get(k).getName(),
00326                                                                 links1.get(j).getDbl("tolerance",0.0)+links2.get(k).getDbl("tolerance",0.0)); //$NON-NLS-1$ //$NON-NLS-2$
00327                                         }
00328                                 }
00329                         }
00330             //state_.value = null;
00331                 } catch (Exception e) {
00332                         GrxDebugUtil.printErr("initDynamicsSimulator:", e); //$NON-NLS-1$
00333                         return false;
00334                 }
00335                 resolver_.setDynamicsSimulator(currentDynamics_);
00336                 handler_.setInvKinemaResolver(resolver_);
00337                 itemChangeFlag_ = false;
00338                 return true;
00339         }
00340 
00341         public void addClickListener( Grx3DViewClickListener listener ){
00342                 behavior_.addClickListener( listener );
00343         }
00344         public void removeClickListener( Grx3DViewClickListener listener ){
00345                 behavior_.removeClickListener( listener );
00346         }
00347         
00348         private boolean setCharacterData(){
00349                 if(currentCollisionPairs_.isEmpty() || currentModels_.isEmpty()) return false;
00350                 if(!initDynamicsSimulator())    return false;
00351                 if (currentDynamics_ == null) return false;
00352                 for (int i=0; i<currentModels_.size(); i++)  {
00353                         GrxModelItem model = currentModels_.get(i);
00354                         String name = model.getName();
00355                         GrxLinkItem base = model.rootLink();
00356                         double[] data = model.getTransformArray(base);
00357                         currentDynamics_.setCharacterLinkData(name, base.getName(), LinkDataType.ABS_TRANSFORM, data);
00358                         data = model.getJointValues();
00359                         currentDynamics_.setCharacterAllLinkData(name, LinkDataType.JOINT_VALUE, data);
00360                 }
00361                 return true;
00362         }
00363         
00371         public Collision[] getCollision() {
00372                 if(!setCharacterData())
00373                         return null;
00374                 if (currentDynamics_.checkCollision(true)){
00375                         WorldStateHolder wsH = new WorldStateHolder();
00376                         currentDynamics_.getWorldState(wsH);
00377                         return wsH.value.collisions;
00378                 }else{
00379                         return null;
00380                 }
00381         }
00382 
00390         public Distance[] getDistance() {
00391                 if(!setCharacterData())
00392                         return null;
00393                 return currentDynamics_.checkDistance();
00394         }
00395         
00403         public LinkPair[] getIntersection() {
00404                 if(!setCharacterData())
00405                         return null;
00406                 return currentDynamics_.checkIntersection(true);
00407         }
00408 
00409         public void setItem(List<GrxModelItem> models, List<GrxCollisionPairItem> cols){
00410                 currentModels_ = models;
00411                 currentCollisionPairs_ = cols;
00412                 itemChangeFlag_ = true;
00413         }
00414         
00415         public void destroyDynamicsSimulator(){
00416                 try {
00417                         currentDynamics_.destroy();
00418                 } catch (Exception e) {
00419                         GrxDebugUtil.printErr("getDynamicsSimulator: destroy failed."); //$NON-NLS-1$
00420                 }
00421                 currentDynamics_ = null;
00422         }
00423         
00424         public void setMessageSkip(boolean flg){
00425                 messageSkip_ = flg;
00426         }
00427         
00428         public void setItemChange(){
00429                 itemChangeFlag_ = true;
00430         }
00431 }


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:52