GrxWorldStateItem.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  */
00010 /*
00011  *  GrxWorldStateItem.java
00012  *
00013  *  Copyright (C) 2007 GeneralRobotix, Inc.
00014  *  All Rights Reserved
00015  *
00016  *  @author Yuichiro Kawasumi (General Robotix, Inc.)
00017  */
00018 
00019 package com.generalrobotix.ui.item;
00020 
00021 import java.io.*;
00022 import java.lang.reflect.InvocationTargetException;
00023 import java.lang.Runtime;
00024 import java.nio.ByteBuffer;
00025 import java.util.*;
00026 import java.util.zip.ZipEntry;
00027 import java.util.zip.ZipFile;
00028 import javax.vecmath.Matrix3d;
00029 
00030 import org.eclipse.core.runtime.IProgressMonitor;
00031 import org.eclipse.jface.action.Action;
00032 import org.eclipse.jface.dialogs.MessageDialog;
00033 import org.eclipse.jface.dialogs.ProgressMonitorDialog;
00034 import org.eclipse.jface.operation.IRunnableWithProgress;
00035 import org.eclipse.swt.SWT;
00036 import org.eclipse.swt.widgets.FileDialog;
00037 import org.eclipse.swt.widgets.DirectoryDialog;
00038 
00039 import jp.go.aist.hrp.simulator.*;
00040 
00041 import com.generalrobotix.ui.GrxBaseView;
00042 import com.generalrobotix.ui.GrxPluginManager;
00043 import com.generalrobotix.ui.GrxPositionObserver;
00044 import com.generalrobotix.ui.GrxTimeSeriesItem;
00045 import com.generalrobotix.ui.grxui.GrxUIPerspectiveFactory;
00046 import com.generalrobotix.ui.util.AxisAngle4d;
00047 import com.generalrobotix.ui.util.GrxDebugUtil;
00048 import com.generalrobotix.ui.util.MessageBundle;
00049 import com.generalrobotix.ui.view.graph.*;
00050 import com.generalrobotix.ui.util.GrxCopyUtil;
00051 
00052 @SuppressWarnings("serial") //$NON-NLS-1$
00053 public class GrxWorldStateItem extends GrxTimeSeriesItem {
00054         public static final String TITLE = "World State"; //$NON-NLS-1$
00055         public static final String DEFAULT_DIR = "/"; //$NON-NLS-1$
00056         public static final String FILE_EXTENSION = "log"; //$NON-NLS-1$
00057         
00058         public static final double DEFAULT_TOTAL_TIME = 20.0;
00059     private static final int MAX_RAM_BUFFER_SIZE = -1; // 無制限
00060     private static final int LOAD_LOG_MODITOR_DIM = 32; // プログレスモニター用定数
00061     private static final long HEAP_MEMORY_TOLERANCE = 4*1024*1024; //残りヒープメモリサイズの許容量
00062     private static final String OVER_HEAP_LOG_DIR_NAME = "over"; //ヒープメモリを超えたときにログを退避させるディレクトリ //$NON-NLS-1$
00063         private static String LOG_DIR;
00064         
00065         private WorldStateEx newStat_ = null;
00066         private WorldStateEx preStat_ = null;
00067         private int prePos_ = -1;
00068     
00069     public  final LogManager logger_ = new LogManager();
00070         private float   recDat_[][];
00071     private String  lastCharName_ = null;
00072         private boolean initLogFlag_ = false;
00073         private boolean useDisk_ = true;
00074         private boolean storeAllPos_ = true;
00075         
00076         private Action save_ = new Action(){
00077         public String getText(){ return MessageBundle.get("GrxWorldStateItem.menu.saveLog"); } //$NON-NLS-1$
00078                 public void run(){
00079             if(isRemoved()){
00080                 MessageDialog.openWarning(
00081                         GrxUIPerspectiveFactory.getCurrentShell(),
00082                         MessageBundle.get("GrxWorldStateItem.dialog.title.saveLog"), //$NON-NLS-1$
00083                         MessageBundle.get("GrxWorldStateItem.dialog.message.saveLog")); //$NON-NLS-1$
00084                 return;
00085             }
00086                         _saveLog();
00087                 }
00088         };
00089         private Action saveCSV_ = new Action(){
00090         public String getText(){ return MessageBundle.get("GrxWorldStateItem.menu.saveAsCSV"); } //$NON-NLS-1$
00091         public void run(){
00092             if(isRemoved()){
00093                 MessageDialog.openWarning(
00094                         GrxUIPerspectiveFactory.getCurrentShell(),
00095                         MessageBundle.get("GrxWorldStateItem.dialog.title.saveAsCSV"), //$NON-NLS-1$
00096                         MessageBundle.get("GrxWorldStateItem.dialog.message.saveAsCSV")); //$NON-NLS-1$
00097                 return;
00098             }
00099             _saveCSV();
00100                 }
00101         };
00102         private Action clear_ = new Action(){
00103         public String getText(){ return MessageBundle.get("GrxWorldStateItem.menu.clearLog"); } //$NON-NLS-1$
00104                 public void run(){
00105                         if( MessageDialog.openQuestion( null, MessageBundle.get("GrxWorldStateItem.dialog.title.clearLog"), MessageBundle.get("GrxWorldStateItem.dialog.message.clearLog") ) ) //$NON-NLS-1$ //$NON-NLS-2$
00106                                 clearLog();
00107                 }
00108         };
00109         
00110         private AxisAngle4d a4d = new AxisAngle4d();
00111         private Matrix3d m3d = new Matrix3d();
00112         private AxisAngle4d a4dg = new AxisAngle4d();
00113         private Matrix3d m3dg = new Matrix3d();
00114 
00115         private String tempDirBase_;
00116         private String tempDir_;
00117         
00118         public GrxWorldStateItem(String name, GrxPluginManager manager) {
00119         super(name, manager);
00120 
00121                 tempDirBase_ = System.getProperty("java.io.tmpdir")+File.separator+"grxui-"+System.getProperty("user.name")+File.separator; //$NON-NLS-1$ //$NON-NLS-2$ //$NON-NLS-3$
00122                 tempDir_ = tempDirBase_+getName();
00123         
00124         Action load = new Action(){
00125             public String getText(){ return MessageBundle.get("GrxWorldStateItem.menu.loadLog"); } //$NON-NLS-1$
00126                 public void run(){
00127                 FileDialog fdlg = new FileDialog(GrxUIPerspectiveFactory.getCurrentShell(), SWT.OPEN);
00128                 fdlg.setFilterExtensions(new String[]{"*.log"}); //$NON-NLS-1$
00129                 fdlg.setFilterPath(getDefaultDir().getAbsolutePath());
00130                 final String fPath = fdlg.open();                
00131                         _loadLog(new File(fPath));
00132                         setDefaultDirectory(new File(fPath).getParent());
00133                 }
00134         };
00135                 setMenuItem(save_);
00136                 setMenuItem(load);
00137                 setMenuItem(saveCSV_);
00138                 setMenuItem(clear_);
00139 
00140                 setExclusive(true);
00141                 setIcon( "world.png" ); //$NON-NLS-1$
00142 
00143                 logger_.init();
00144         }
00145         
00146 //      public static String[] getStaticMenu() {
00147 //              return new String[]{"create", "load"};
00148 //      }
00149 
00150         public boolean create() {
00151         clearLog();
00152                 setDbl("logTimeStep", 0.001); //$NON-NLS-1$
00153                 return true;
00154         }
00155 
00156         public boolean load(File f) {
00157         _loadLog(f);
00158         return true;
00159     }
00160     
00161         public void setLogMenus(boolean bAble){
00162         save_.setEnabled(bAble);
00163         saveCSV_.setEnabled(bAble);
00164         clear_.setEnabled(bAble);
00165     }
00166     
00167         public void restoreProperties() {
00168                 super.restoreProperties();
00169                 useDisk_ = isTrue("useDisk", true); //$NON-NLS-1$
00170                 storeAllPos_ = isTrue("storeAllPosition", storeAllPos_); //$NON-NLS-1$
00171         int size = getInt("bufferSize", MAX_RAM_BUFFER_SIZE); //$NON-NLS-1$
00172                 if ( useDisk_ ) {
00173                         super.setMaximumLogSize(MAX_RAM_BUFFER_SIZE);
00174                 } else {
00175                         GrxDebugUtil.println("GrxWorldStateItem: useDisk = false"); //$NON-NLS-1$
00176                         super.setMaximumLogSize(size);
00177                 }
00178         }
00179         
00180         public void rename(String newName) {
00181                 File oldDir = new File(LOG_DIR+File.separator+getName());
00182                 super.rename(newName);
00183                 File newDir = new File(LOG_DIR+File.separator+getName());
00184                 if (newDir.isDirectory()) 
00185                         newDir.delete();
00186                 
00187                 if (oldDir.isDirectory())
00188                         oldDir.renameTo(newDir);
00189                 else 
00190                         newDir.mkdir();
00191                 
00192                 logger_.setTempDir(newDir.getPath());
00193         }
00194         
00195         public void clearLog() {
00196                 super.clearLog();
00197         initLogFlag_ = false;
00198                 logger_.init();
00199                 newStat_ = null;
00200                 preStat_ = null;
00201                 prePos_ = -1;
00202                 lastCharName_ = null;
00203         setLogMenus(false);
00204         remove("url");
00205         if(useDisk_)
00206                 logger_.closeReads();
00207                 syncExec(new Runnable(){
00208                 public void run(){
00209                         notifyObservers("ClearLog"); //$NON-NLS-1$
00210                 }
00211         });
00212         }
00213         
00214         @SuppressWarnings("unchecked") //$NON-NLS-1$
00215         public void registerCharacter(String cname, BodyInfo binfo) {
00216                 ArrayList<String> logList = new ArrayList<String>();
00217                 logList.add("time"); //$NON-NLS-1$
00218                 logList.add("float"); //$NON-NLS-1$
00219                 
00220                 LinkInfo[] li = binfo.links();
00221                 ArrayList<Short> jointList = new ArrayList<Short>();
00222                 ArrayList<SensorInfoLocal> sensList  = new ArrayList<SensorInfoLocal>();
00223                 for (int i=0; i<li.length; i++)  {
00224                         jointList.add(li[i].jointId);
00225                         
00226                         SensorInfo[] si = li[i].sensors;
00227                         for (int j=0; j<si.length; j++) 
00228                                 sensList.add(new SensorInfoLocal(si[j]));
00229                 }
00230                 
00231                 Collections.sort(sensList);
00232                 
00233                 int len = storeAllPos_ ? li.length : 1;
00234                 for (int i=0; i< len; i++) {
00235                         String jname = li[i].name;
00236                         logList.add(jname+".translation"); //$NON-NLS-1$
00237                         logList.add("float[3]"); //$NON-NLS-1$
00238                         logList.add(jname+".rotation"); //$NON-NLS-1$
00239                         logList.add("float[4]"); //$NON-NLS-1$
00240                 }
00241                 
00242                 for (short i=0; i<jointList.size(); i++) { // short <- its depend on the idl def.
00243                         int idx = jointList.indexOf(i);
00244                         if (idx >= 0) {
00245                                 String jname = li[idx].name;
00246                                 logList.add(jname+".angle"); //$NON-NLS-1$
00247                                 logList.add("float"); //$NON-NLS-1$
00248                                 logList.add(jname+".jointTorque"); //$NON-NLS-1$
00249                                 logList.add("float"); //$NON-NLS-1$
00250                         }
00251                 }
00252                 
00253                 for (int i=0; i<sensList.size(); i++) {
00254                         String sname = sensList.get(i).name;
00255                         // ##### [Changed] NewModelLoader.IDL
00256                         //switch(sensList.get(i).type) {
00257                         //case SensorType._FORCE_SENSOR:
00258                         //    logList.add(sname+".force");
00259                         //    logList.add("float[3]");
00260                         //    logList.add(sname+".torque");
00261                         //    logList.add("float[3]");
00262                         //    break;
00263                         //case SensorType._RATE_GYRO:
00264                         //    logList.add(sname+".angularVelocity");
00265                         //    logList.add("float[3]");
00266                         //    break;
00267                         //case SensorType._ACCELERATION_SENSOR:
00268                         //    logList.add(sname+".acceleration");
00269                         //    logList.add("float[3]");
00270                         //    break;
00271                         //default:
00272                         //    break;
00273                         //}
00274                         if(sensList.get(i).type.equals("Force")){ //$NON-NLS-1$
00275                                 logList.add(sname+".force"); //$NON-NLS-1$
00276                                 logList.add("float[3]"); //$NON-NLS-1$
00277                                 logList.add(sname+".torque"); //$NON-NLS-1$
00278                                 logList.add("float[3]"); //$NON-NLS-1$
00279                         }else if(sensList.get(i).type.equals("RateGyro")){ //$NON-NLS-1$
00280                             logList.add(sname+".angularVelocity"); //$NON-NLS-1$
00281                             logList.add("float[3]"); //$NON-NLS-1$
00282                         }else if(sensList.get(i).type.equals("Acceleration")){ //$NON-NLS-1$
00283                             logList.add(sname+".acceleration"); //$NON-NLS-1$
00284                             logList.add("float[3]"); //$NON-NLS-1$
00285                         }else if(sensList.get(i).type.equals("Range")){ //$NON-NLS-1$
00286                                 logList.add(sname+".range"); //$NON-NLS-1$
00287                                 SensorInfo info = sensList.get(i).info;
00288                                 int half = (int)(info.specValues[0]/2/info.specValues[1]);// = scanAngle/scanStep
00289                                 logList.add("float["+(half*2+1)+"]"); //$NON-NLS-1$ //$NON-NLS-2$
00290                         }else{
00291                         }
00292                 }
00293                 
00294                 logList.add("command"); //$NON-NLS-1$
00295                 logList.add("float["+jointList.size()+"]"); //$NON-NLS-1$ //$NON-NLS-2$
00296                 logList.add("servoState");
00297                 logList.add("float["+jointList.size()+"]");
00298                 logList.add("powerState");
00299                 logList.add("float[2]");
00300                 try {
00301                         logger_.addLogObject(cname, logList.toArray(new String[0]));
00302                 } catch (LogFileFormatException e) {
00303                         e.printStackTrace();
00304                 }
00305         }
00306 
00307         public void addValue(Double t, Object obj) {
00308                 if ( useDisk_ ) {
00309             _addValueToLog(t, obj);
00310         }else {
00311             if(overPos_ > 0){
00312                 // メモリ+ファイル記録のログのファイル記録
00313                 _addValueToLog(t, obj);
00314                 ++overPos_;
00315             } else {
00316                 if( Runtime.getRuntime().freeMemory() < HEAP_MEMORY_TOLERANCE){
00317                     //メモリからファイルへのログ保存に切替
00318                     File f = new File(tempDir_);
00319                     File pf = f.getParentFile();
00320                     if (!pf.isDirectory()){
00321                         pf.mkdir();
00322                     }
00323                     tempDir_ = tempDirBase_ + getName() + File.separator + OVER_HEAP_LOG_DIR_NAME;
00324                     changePos_ = super.getLogSize();
00325                     _addValueToLog(t, obj);
00326                     ++overPos_;
00327                 } else {
00328                     super.addValue(t, obj);
00329                 }
00330             }
00331         }
00332     }
00333 
00334     private void _addValueToLog(Double t, Object obj){
00335         if (obj instanceof WorldStateEx) {
00336             newStat_ = (WorldStateEx) obj;
00337             _initLog();
00338             _toLogFile(logger_);
00339             super.addValue(newStat_.time, null);
00340         }
00341     }
00342 
00343     private void _addValueToLogFromSuperLog(Double t, Object obj, LogManager temp){
00344         if (obj instanceof WorldStateEx) {
00345             newStat_ = (WorldStateEx) obj;
00346             if(temp == logger_){
00347                 _initLog();
00348             }
00349             _toLogFile(temp);
00350         }
00351     }
00352 
00353     private void _toLogFile(LogManager temp){
00354         temp.setTime(new Time((float)newStat_.time));
00355         
00356         try {
00357             Collision[] cols = newStat_.collisions;
00358             if (cols != null && cols.length > 0) {
00359                 List<CollisionPoint> cdList = new ArrayList<CollisionPoint>();
00360                 for (int i=0; i < cols.length; i++) {
00361                     if( cols[i].points == null){
00362                         continue;
00363                     }
00364                     for (int j=0; j<cols[i].points.length; j++) 
00365                         cdList.add(cols[i].points[j]);
00366                 }
00367                 CollisionPoint[] cd = cdList.toArray(new CollisionPoint[0]);
00368                 temp.putCollisionPointData(cd);
00369             }
00370         } catch (Exception e) {
00371             GrxDebugUtil.printErr("",e); //$NON-NLS-1$
00372         }
00373         
00374         for (int i=0; i < newStat_.charList.size(); i++) {
00375             int k = 0;
00376             recDat_[i][k++] = (float) newStat_.time;
00377             CharacterStateEx cpos = newStat_.charList.get(i);
00378             int len = storeAllPos_ ? cpos.position.length : 1;
00379             for (int j=0; j<len; j++) {
00380                 for (int m=0; m<3; m++)
00381                     recDat_[i][k++] = (float)cpos.position[j].p[m];
00382                 m3d.set(cpos.position[j].R);
00383                 //m3d.transpose();
00384                 a4d.setMatrix(m3d);
00385                 recDat_[i][k++] = (float) a4d.x;
00386                 recDat_[i][k++] = (float) a4d.y;
00387                 recDat_[i][k++] = (float) a4d.z;
00388                 recDat_[i][k++] = (float) a4d.angle;
00389             }
00390             
00391             String cname = cpos.characterName;
00392             SensorState sdata = cpos.sensorState;
00393             if (sdata != null) {
00394                 for (int j=0; j<sdata.q.length; j++) {
00395                     recDat_[i][k++] = (float) sdata.q[j];
00396                     recDat_[i][k++] = (float) sdata.u[j];
00397                 }
00398                 for (int j=0; j<sdata.force.length; j++) {
00399                     for (int m=0; m<sdata.force[j].length; m++) 
00400                         recDat_[i][k++] = (float)sdata.force[j][m];
00401                 }
00402                 for (int j=0; j<sdata.rateGyro.length; j++) {
00403                     for (int m=0; m<sdata.rateGyro[j].length; m++) 
00404                         recDat_[i][k++] = (float)sdata.rateGyro[j][m];
00405                 }
00406                 for (int j=0; j<sdata.accel.length; j++) {
00407                     for (int m=0; m<sdata.accel[j].length; m++) 
00408                         recDat_[i][k++] = (float)sdata.accel[j][m];
00409                 }
00410                 if (sdata.range != null){
00411                         for (int j=0; j<sdata.range.length; j++) {
00412                                 for (int m=0; m<sdata.range[j].length; m++) 
00413                                         recDat_[i][k++] = (float)sdata.range[j][m];
00414                         }
00415                 }
00416             }
00417             if (cpos.targetState != null){
00418                 for (int j=0; j<cpos.targetState.length; j++){
00419                         recDat_[i][k++]  = (float)cpos.targetState[j];
00420                 }
00421             }
00422             if (cpos.servoState != null){
00423                 for (int j=0; j<cpos.servoState.length; j++){
00424                         ByteBuffer buffer = ByteBuffer.allocate(10);
00425                         buffer.putInt(cpos.servoState[j]);
00426                         float f = buffer.getFloat(0);
00427                         recDat_[i][k++] = f;
00428                         //recDat_[i][k++] = (float)cpos.servoState[j];
00429                 }
00430             }
00431             if (cpos.powerState != null){
00432                 for (int j=0; j<cpos.powerState.length; j++){
00433                         recDat_[i][k++] = (float)cpos.powerState[j];
00434                 }
00435             }
00436             try {
00437                 temp.put(cname, recDat_[i]);
00438             } catch (Exception e) {
00439                 e.printStackTrace();
00440             }
00441         }
00442     }
00443     
00444         public void init(){
00445                 double val = getDbl("logTimeStep",0.001); //$NON-NLS-1$
00446                 setDbl("logTimeStep",val); //$NON-NLS-1$
00447         }
00448         
00449         private void _initLog() {
00450                 if ( initLogFlag_ ){
00451                         return;
00452                 }
00453         initLogFlag_ = true;
00454         setLogMenus(true);
00455                 SimulationTime stime = new SimulationTime();
00456                 stime.setCurrentTime(newStat_.time);
00457                 stime.setStartTime(newStat_.time);
00458                 
00459                 double val = getDbl("logTimeStep",0.001); //$NON-NLS-1$
00460                 stime.setTimeStep(val);
00461                         
00462                 GrxSimulationItem simItem = manager_.<GrxSimulationItem>getSelectedItem(GrxSimulationItem.class, null);
00463                 String method = "";
00464                 if(simItem!=null){
00465                         val = simItem.getDbl("totalTime", DEFAULT_TOTAL_TIME); //$NON-NLS-1$
00466                         method = simItem.getStr("method");
00467                 }else
00468                         val = DEFAULT_TOTAL_TIME;
00469                 stime.setTotalTime(val);
00470                 
00471                 logger_.setTempDir(tempDir_);
00472                 
00473                 logger_.initCollisionLog(stime);
00474                 try {
00475                         logger_.openAsWrite(stime, method);
00476                         logger_.openAsRead();
00477                         logger_.openCollisionLogAsWrite();
00478                         logger_.openCollisionLogAsRead();
00479                 } catch (FileOpenFailException e) {
00480             e.printStackTrace();
00481                 } catch (IOException e1) {
00482                         e1.printStackTrace();
00483                 }
00484                 recDat_ = new float[logger_.getLogObjectNum()][];
00485                 for (int i=0; i<recDat_.length; i++)
00486                         recDat_[i] = new float[logger_.getDataLength(newStat_.charList.get(i).characterName)];
00487                 preStat_ = newStat_;
00488         }
00489 
00490         public WorldStateEx getValue() {
00491         WorldStateEx ret = null;
00492         
00493         int pos = getPosition();
00494         if(useDisk_){
00495             if (pos >= 0){
00496                 if (pos == getLogSize()-1 && newStat_ != null){
00497                     ret = newStat_;
00498                 }
00499                 if (pos != prePos_ && preStat_ != null)
00500                     ret = getValue(pos);
00501             }
00502         } else {
00503             if( pos > changePos_ && changePos_ >= 0){
00504                 ret = _getValueFromLog( pos - changePos_ );
00505             } else {
00506                 ret = (WorldStateEx)super.getValue(pos);
00507             }
00508         }
00509                 return ret;
00510         }
00511 
00512         public WorldStateEx getValue(int pos) {
00513         WorldStateEx ret = null;
00514                 if (pos >= 0){
00515                 if ( useDisk_ ){
00516                 ret = _getValueFromLog( pos );
00517             } else {
00518                 if( pos > changePos_ && changePos_ >= 0 ){
00519                     ret = _getValueFromLog(pos - changePos_);
00520                 } else {
00521                     ret = (WorldStateEx)super.getValue(pos);
00522                 }
00523             }
00524         }
00525                 return ret;
00526         }
00527         
00528     private WorldStateEx _getValueFromLog(int pos){
00529         if(pos == prePos_)
00530             return preStat_;
00531 
00532         try {        
00533             preStat_.collisions = new Collision[]{new Collision()};
00534             preStat_.collisions[0].points = logger_.getCollisionPointData(pos);
00535             
00536             for (int i=0; i<preStat_.charList.size(); i++) {
00537                 int k=0;
00538                 CharacterStateEx cpos = preStat_.charList.get(i);
00539                 float[] f = logger_.get(cpos.characterName, (long)pos);
00540                 preStat_.time = (double)f[k++];
00541                 for (int j=0; j<cpos.position.length; j++) {
00542                     LinkPosition lpos = cpos.position[j];
00543                     if (storeAllPos_ || j == 0) { 
00544                         for (int m=0; m<3; m++)
00545                             lpos.p[m] = (double)f[k++];
00546                         a4dg.set((double)f[k++], (double)f[k++], (double)f[k++], (double)f[k++]);
00547                         m3dg.set(a4dg);
00548                         lpos.R[0] = m3dg.m00;
00549                         lpos.R[1] = m3dg.m01;
00550                         lpos.R[2] = m3dg.m02;
00551                         lpos.R[3] = m3dg.m10;
00552                         lpos.R[4] = m3dg.m11;
00553                         lpos.R[5] = m3dg.m12;
00554                         lpos.R[6] = m3dg.m20;
00555                         lpos.R[7] = m3dg.m21;
00556                         lpos.R[8] = m3dg.m22;
00557                     } else {
00558                         lpos.p = null;  // to calculate kinema in model
00559                         lpos.R = null;  // to calculate kinema in model
00560                     }
00561                 }
00562                 
00563                 SensorState sdata = cpos.sensorState;
00564                 if (sdata != null) {
00565                     for (int j=0; j<sdata.q.length; j++) {
00566                         sdata.q[j] = (double)f[k++]; 
00567                         sdata.u[j] = (double)f[k++];
00568                     }
00569                     for (int j=0; j<sdata.force.length; j++) {
00570                         for (int m=0; m<sdata.force[j].length; m++) 
00571                             sdata.force[j][m] = (double)f[k++];
00572                     }
00573                     for (int j=0; j<sdata.rateGyro.length; j++) {
00574                         for (int m=0; m<sdata.rateGyro[j].length; m++) 
00575                             sdata.rateGyro[j][m] = (double)f[k++];
00576                     }
00577                     for (int j=0; j<sdata.accel.length; j++) {
00578                         for (int m=0; m<sdata.accel[j].length; m++) 
00579                             sdata.accel[j][m] = (double)f[k++];
00580                     }
00581                     if (sdata.range != null){
00582                         for (int j=0; j<sdata.range.length; j++) {
00583                                 for (int m=0; m<sdata.range[j].length; m++) 
00584                                         sdata.range[j][m] = (double)f[k++];
00585                         }
00586                     }
00587                 }
00588                 if (cpos.targetState != null){
00589                         for (int j=0; j<cpos.targetState.length; j++){
00590                                 cpos.targetState[j]     = (double)f[k++];
00591                         }
00592                 }
00593                 if (cpos.servoState != null){
00594                         for (int j=0; j<cpos.servoState.length; j++){
00595                                 ByteBuffer buffer = ByteBuffer.allocate(10);
00596                                 buffer.putFloat(f[k++]);
00597                                 int ss = buffer.getInt(0);
00598                                 cpos.servoState[j] = ss;
00599                                 //cpos.servoState[j] = (int)f[k++];
00600                         }
00601                 }
00602                 if (cpos.powerState != null){
00603                         for (int j=0; j<cpos.powerState.length; j++){
00604                                 cpos.powerState[j] = (double)f[k++];
00605                         }
00606                 }
00607             }
00608         } catch (IOException e) {
00609             e.printStackTrace();
00610         }
00611         prePos_ = pos;
00612         return preStat_;
00613     }
00614     
00615         private void _loadLog(final File logFile) {
00616         try {
00617                 IRunnableWithProgress op = new IRunnableWithProgress() {
00618                                 public void run(IProgressMonitor monitor) throws InvocationTargetException, InterruptedException {
00619                     int size = 0;
00620                     try{
00621                         ZipFile local = new ZipFile(logFile);
00622                         size = local.size();
00623                         local.close();
00624                     } catch ( IOException ex ){
00625                         ex.printStackTrace();
00626                         return;
00627                     } catch ( Exception ex){
00628                         ex.printStackTrace();
00629                         return;
00630                     }
00631                     
00632                                         monitor.beginTask("Loading log as a file:"+logFile.getName(), size + LOAD_LOG_MODITOR_DIM + 2); //$NON-NLS-1$
00633                                         _loadLog(logFile,monitor);
00634                                         monitor.done();
00635                                         notifyObservers("LoadLog");
00636                                 }
00637                 };
00638                 new ProgressMonitorDialog(GrxUIPerspectiveFactory.getCurrentShell()).run(false, true, op);
00639             setLogMenus(true);
00640         } catch (InvocationTargetException e) {
00641                 e.printStackTrace();
00642         } catch (InterruptedException e) {
00643                 clearLog();
00644         }
00645         }
00646     
00647         private void _loadLog(File logFile, IProgressMonitor monitor ) throws InterruptedException{
00648         String fname = logFile.getAbsolutePath();
00649                 try {
00650                         if (logFile == null) 
00651                                 logFile = new File("log"+File.separator+getName()+".log"); //$NON-NLS-1$ //$NON-NLS-2$
00652 
00653                         if (!logFile.isFile())
00654                                 return;
00655 
00656             clearLog();
00657             tempDir_ = tempDirBase_ + getName();
00658                         logger_.setTempDir(tempDir_);
00659                         logger_.load(fname, ""); //$NON-NLS-1$
00660             
00661                         monitor.worked(1);
00662                         final SimulationTime sTime = new SimulationTime();
00663                         logger_.getSimulationTime(sTime);
00664                         syncExec(new Runnable(){
00665                         public void run(){
00666                                 GrxSimulationItem simItem = manager_.<GrxSimulationItem>getSelectedItem(GrxSimulationItem.class, null);
00667                                 if(simItem!=null)
00668                                         simItem.setDbl("totalTime", sTime.getTotalTime());
00669                                 setDbl("logTimeStep", sTime.getTimeStep()); //$NON-NLS-1$
00670                         }
00671                 });
00672             
00673                         logger_.openAsRead();
00674                         logger_.openCollisionLogAsRead();
00675                         
00676             preStat_ = new WorldStateEx();
00677             Enumeration<? extends ZipEntry> e = new ZipFile(fname).entries();
00678             while (e.hasMoreElements()) {
00679                 monitor.worked(1);
00680                 if (monitor.isCanceled())
00681                     throw new InterruptedException();
00682                 String entry = ((ZipEntry)e.nextElement()).getName();
00683                 if (entry.indexOf(LogManager.COLLISION_LOG_NAME) >= 0 ||
00684                     entry.indexOf(LogManager.COLLISION_LOG_DAT_NAME) >= 0) {
00685                     continue;
00686                 }
00687                 
00688                 lastCharName_ = new File(entry).getName().split(".tmp")[0]; //$NON-NLS-1$
00689                 String[] format = logger_.getDataFormat(lastCharName_);
00690                 List<LinkPosition> lposList = new ArrayList<LinkPosition>();
00691                 int jointCount = 0;
00692                 int forceCount = 0;
00693                 int gyroCount = 0;
00694                 int accelCount = 0;
00695                 int rangeCount = 0;
00696                 
00697                 for (int i=0; i<format.length; i++) {
00698                         String[] str = format[i].split("[.]"); //$NON-NLS-1$
00699                         if (str.length <= 1) {
00700                                 continue;
00701                         } else if (str[1].equals("translation")) { //$NON-NLS-1$
00702                                 LinkPosition lpos = new LinkPosition();
00703                                 lpos.p = new double[3];
00704                                 lpos.R = new double[9];
00705                                 lposList.add(lpos);
00706                         } else if (str[1].equals("angle")) { //$NON-NLS-1$
00707                                 if (!storeAllPos_) 
00708                                         lposList.add(new LinkPosition());
00709                                 jointCount++;
00710                         } else if (str[1].equals("force")) { //$NON-NLS-1$
00711                                 forceCount++;
00712                         } else if (str[1].equals("angularVelocity")) { //$NON-NLS-1$
00713                                 gyroCount++;
00714                         } else if (str[1].equals("acceleration")) { //$NON-NLS-1$
00715                                 accelCount++;
00716                         } else if (str[1].equals("range")){ //$NON-NLS-1$
00717                                 rangeCount++;
00718                         }
00719                 }
00720                 
00721                                 CharacterStateEx cpos = new CharacterStateEx();
00722                                 cpos.characterName = lastCharName_;
00723                                 cpos.position = lposList.toArray(new LinkPosition[0]);
00724                 if (jointCount+forceCount+accelCount+gyroCount+rangeCount > 1) {
00725                         SensorState     sdata = new SensorState();
00726                                 sdata.q = new double[jointCount];
00727                                 sdata.u = new double[jointCount];
00728                                 sdata.force = new double[forceCount][6];
00729                                         sdata.rateGyro  = new double[gyroCount][3];
00730                                         sdata.accel = new double[accelCount][3];
00731                                         sdata.range = new double[rangeCount][]; // TODO
00732                                 cpos.sensorState = sdata;
00733                 }
00734                 preStat_.charList.add(cpos);
00735                 preStat_.charMap.put(lastCharName_, cpos);
00736             }
00737             
00738             int datLen = logger_.getRecordNum(lastCharName_);
00739             
00740             // プログレス用変数の初期化
00741             int workdim[] = new int[LOAD_LOG_MODITOR_DIM];
00742             int workdimCounter = 0;
00743             int localLength = workdim.length;
00744             
00745             for( int i = 0; i < localLength; ++i){
00746                 workdim[i] = datLen * (i + 1) / localLength; 
00747             }
00748             
00749             monitor.worked(1);
00750 
00751             if( !useDisk_ ){
00752                 recDat_ = new float[logger_.getLogObjectNum()][];
00753                 for (int i=0; i<recDat_.length; i++)
00754                     recDat_[i] = new float[logger_.getDataLength(preStat_.charList.get(i).characterName)];
00755             }
00756             
00757             for (int i=0; i < datLen; i++){
00758                 if ( useDisk_ ) {
00759                     try{
00760                         super.addValue( null , null);
00761                     } catch (Exception ex){
00762                         ex.printStackTrace();
00763                         break;
00764                     }
00765                 } else {
00766                     //メモリーに展開する場合
00767                     WorldStateEx worldState = _getValueFromLog(i);
00768                     
00769                     if( Runtime.getRuntime().freeMemory() < HEAP_MEMORY_TOLERANCE){
00770                         //ヒープメモリが足りない場合の処理
00771                         _createOverLog( worldState.time,
00772                                         i  == 0 ? worldState.time : getTime(0),
00773                                         i, datLen - i);
00774                         break;
00775                     }
00776                     
00777                     try{
00778                         super.addValue( worldState.time , worldState.clone());
00779                     } catch (Exception ex){
00780                         ex.printStackTrace();
00781                         break;
00782                     }
00783                 }
00784                 
00785                 //プログレスチェックと処理
00786                 if ( workdim[workdimCounter] < i){
00787                     workdimCounter ++;
00788                     monitor.worked(1);
00789                 }
00790             }
00791             
00792             syncExec(new Runnable(){
00793                 public void run(){
00794                         setPosition(0);
00795                 }
00796             });
00797             monitor.worked(1);
00798         } catch (FileOpenFailException e) {
00799             e.printStackTrace();
00800             MessageDialog.openError(
00801                     GrxUIPerspectiveFactory.getCurrentShell(),
00802                     MessageBundle.get("GrxWorldStateItem.dialog.title.errorLoadLogMessageDlg"),
00803                     MessageBundle.get("GrxWorldStateItem.dialog.message.errorFileOpenFail") + 
00804                     System.getProperty("line.separator") + fname);
00805         } catch (LogFileFormatException e) {
00806             e.printStackTrace();
00807             MessageDialog.openError(
00808                     GrxUIPerspectiveFactory.getCurrentShell(),
00809                     MessageBundle.get("GrxWorldStateItem.dialog.title.errorLoadLogMessageDlg"),
00810                     MessageBundle.get("GrxWorldStateItem.dialog.message.errorLogFileFormat") + 
00811                     System.getProperty("line.separator") + fname);
00812                 } catch (IOException e) {
00813             e.printStackTrace();
00814             MessageDialog.openError(
00815                     GrxUIPerspectiveFactory.getCurrentShell(),
00816                     MessageBundle.get("GrxWorldStateItem.dialog.title.errorLoadLogMessageDlg"),
00817                     MessageBundle.get("GrxWorldStateItem.dialog.message.errorIOException") + 
00818                     System.getProperty("line.separator") + fname);
00819         }
00820         }
00821 
00822         private void _saveLog() {
00823         FileDialog fdlg = new FileDialog(GrxUIPerspectiveFactory.getCurrentShell(), SWT.SAVE);
00824         fdlg.setFileName(GrxWorldStateItem.this.getName()+".log"); //$NON-NLS-1$
00825         fdlg.setFilterExtensions(new String[]{"*.log"}); //$NON-NLS-1$
00826         fdlg.setFilterPath(getDefaultDir().getAbsolutePath());
00827         final String fPath = fdlg.open();
00828         if (fPath != null) {
00829                         Thread t = new Thread() {
00830                                 public void run() {
00831                                         try {
00832                         if( useDisk_ ){
00833                             // 従来の処理
00834                             logger_.closeAsWrite();
00835                             logger_.closeCollisionLogAsWrite();
00836                             logger_.save(fPath, getName()+".prj"); //$NON-NLS-1$
00837                         } else {
00838                             // オンメモリデータをファイルへ
00839                             LogManager temp = _restoreLogFileFromSuperLog();
00840                             if(temp != null){
00841                                 temp.save(fPath, getName()+".prj"); //$NON-NLS-1$
00842                                 if(temp != logger_){
00843                                     temp.closeReads();
00844                                 }
00845                             }
00846                         }
00847                                         } catch (IOException ex){
00848                         ex.printStackTrace();
00849                     } catch (Exception ex) {
00850                                                 ex.printStackTrace();
00851                                         }
00852                                 }
00853                         };
00854                         t.start();
00855                         setDefaultDirectory(new File(fPath).getParent());
00856                         setURL(fPath);
00857         }
00858         }
00859     
00860     private void _createOverLog(double currentTime, double startTime, int changePos, int overPos ){
00861         changePos_ = changePos;
00862         overPos_ = overPos;
00863 
00864         // サブディレクトリOVER_HEAP_LOG_DIR_NAMEにuseDiskがfalseの時と同様のログを生成
00865         LogManager temp = new LogManager(logger_);
00866         if( temp == null){
00867             return;
00868         }
00869         String overDir = tempDirBase_ + getName() + File.separator + OVER_HEAP_LOG_DIR_NAME;
00870         temp.setTempDir( overDir );
00871         SimulationTime stime = new SimulationTime();
00872         logger_.getSimulationTime(stime);
00873         stime.setStartTime(currentTime);
00874         temp.initCollisionLog(stime);
00875         try {
00876             temp.openAsWrite(stime, logger_.getIntegrationMethodStr());
00877             temp.openCollisionLogAsWrite();
00878             temp.separateLogs(changePos);
00879             temp.closeAsWrite();
00880             temp.closeCollisionLogAsWrite();
00881         } catch (IOException ex) {
00882             ex.printStackTrace();
00883         }
00884         
00885         // loggerをOVER_HEAP_LOG_DIR_NAMEディレクトリ以下のファイルを参照するように変更
00886         logger_.closeReads();
00887         logger_.setTempDir(overDir);
00888         try {
00889             logger_.openAsRead();
00890             logger_.openCollisionLogAsRead();
00891         } catch (FileOpenFailException e) {
00892             e.printStackTrace();
00893         } catch (IOException ex) {
00894             ex.printStackTrace();
00895         }
00896         
00897         //overPos部分の時間を登録
00898         for(int i = 0; i < overPos; ++i){
00899             try{
00900                 super.addValue( (double)logger_.get(lastCharName_, i)[0], null);
00901             } catch (Exception ex){
00902                 ex.printStackTrace();
00903                 break;
00904             }
00905         }
00906     }
00907     
00908     private LogManager _restoreLogFileFromSuperLog() {
00909         LogManager ret = null;
00910         if( changePos_ < 0 ){
00911             // 全てのログがメモリにある場合
00912             for(int index = 0; index < getLogSize(); ++index){
00913                 TValue localTValue = getObject(index);
00914                 _addValueToLogFromSuperLog(localTValue.getTime(), localTValue.getValue(), logger_);
00915             }
00916             try{
00917                 logger_.closeAsWrite();
00918                 logger_.closeCollisionLogAsWrite();
00919                 ret = logger_;
00920             } catch (IOException ex){
00921                 ex.printStackTrace();
00922             } catch (Exception ex) {
00923                 ex.printStackTrace();
00924             }
00925         } else {
00926             //全てのログがメモリにない場合
00927             // 書き込みファイルストリームを全て閉じる
00928             try{
00929                 logger_.closeAsWrite();
00930                 logger_.closeCollisionLogAsWrite();
00931             } catch (IOException ex){
00932                 ex.printStackTrace();
00933             } catch (Exception ex) {
00934                 ex.printStackTrace();
00935             }
00936             
00937             // サブディレクトリOVER_HEAP_LOG_DIR_NAMEの親ディレクトリにuseDiskがtrueの時と同様のログを生成
00938             LogManager temp = new LogManager(logger_);
00939             if( temp == null){
00940                 return ret;
00941             }
00942             temp.setTempDir( tempDirBase_ + getName() );
00943             SimulationTime stime = new SimulationTime();
00944             stime.setCurrentTime(preStat_.time);
00945             stime.setStartTime( super.getTime(0) );
00946             
00947             double localTime = getDbl("logTimeStep",0.001); //$NON-NLS-1$
00948             stime.setTimeStep(localTime);
00949                 
00950             GrxSimulationItem simItem = manager_.<GrxSimulationItem>getSelectedItem(GrxSimulationItem.class, null);
00951             String method = "";
00952                 if(simItem!=null){
00953                         localTime = simItem.getDbl("totalTime", DEFAULT_TOTAL_TIME); //$NON-NLS-1$
00954                         method = simItem.getStr("method");
00955                 }else
00956                         localTime = DEFAULT_TOTAL_TIME;
00957             stime.setTotalTime(localTime);
00958             temp.initCollisionLog(stime);
00959             try {
00960                 temp.openAsWrite(stime, method);
00961                 temp.openAsRead();
00962                 temp.openCollisionLogAsWrite();
00963                 temp.openCollisionLogAsRead();
00964             } catch (FileOpenFailException e) {
00965                 e.printStackTrace();
00966             } catch (IOException e1) {
00967                 e1.printStackTrace();
00968             }
00969 
00970             //メモリ上のログをファイルへ展開
00971             for(int index = 0; index < getLogSize(); ++index){
00972                 TValue localTValue = getObject(index);
00973                 Object val = localTValue.getValue();
00974                 if( val != null){
00975                     _addValueToLogFromSuperLog(localTValue.getTime(), val, temp);
00976                     continue;
00977                 }
00978                 break;
00979             }
00980             
00981             try{
00982                 // 残りのサブディレクトリOVER_HEAP_LOG_DIR_NAMEのログを結合
00983                 temp.jointLogs();
00984                 temp.setTime( new Time( stime.getCurrentTime() ) );
00985                 temp.closeAsWrite();
00986                 temp.closeCollisionLogAsWrite();
00987                 ret = temp;
00988             } catch (IOException ex){
00989                 ex.printStackTrace();
00990             } catch (Exception ex) {
00991                 ex.printStackTrace();
00992             }
00993         }
00994         return ret;
00995     }
00996         
00997         private void _saveCSV() {
00998         DirectoryDialog ddlg = new DirectoryDialog(GrxUIPerspectiveFactory.getCurrentShell());
00999         try {
01000                         ddlg.setFilterPath(getDefaultDir().getCanonicalPath());
01001                 } catch (IOException e1) {
01002                         // TODO 自動生成された catch ブロック
01003                         e1.printStackTrace();
01004                 }
01005         final String dir = ddlg.open();
01006         if (dir != null){
01007             Thread t = new Thread() {
01008                         public void run() {
01009                     LogManager temp = null;
01010                     if( !useDisk_ ){                            
01011                         // オンメモリデータをファイルへ
01012                         temp = _restoreLogFileFromSuperLog();
01013                     }
01014                                 for (int i=0; i<preStat_.charList.size(); i++) {
01015                                         String name = preStat_.charList.get(i).characterName;
01016                         String fname = dir+File.separator+name+".csv"; //$NON-NLS-1$
01017                                         try {
01018                             if( useDisk_ ){                            
01019                                 logger_.saveCSV(fname, name);
01020                             }else{
01021                                 // オンメモリデータをファイルへ
01022                                 if(temp != null){
01023                                     temp.saveCSV(fname, name);
01024                                 }
01025                             }
01026                                         } catch (FileOpenFailException e) {
01027                                                 e.printStackTrace();
01028                                         }
01029                                 }
01030                     if(temp != null && temp != logger_){
01031                         temp.closeReads();
01032                     }
01033                         }
01034                 };
01035                 t.start();
01036                 setDefaultDirectory(dir);
01037         }
01038         }
01039         
01040         public Double getTime(int pos) {
01041                 if (pos < 0)
01042                         return null;
01043                 Double t = super.getTime(pos);
01044                 if (t == null && lastCharName_ != null) {
01045             try {
01046                 float[] f = logger_.get(lastCharName_, pos);
01047                 t = (double)f[0];
01048                 setTimeAt(pos, t);
01049             } catch (IOException e) {
01050                 e.printStackTrace();
01051             }
01052                 }
01053             return t;
01054         }
01055         
01056         public void extendTime(double time) {
01057                 SimulationTime stime = new SimulationTime();
01058                 stime.setCurrentTime(preStat_.time);
01059                 stime.setStartTime(preStat_.time);
01060         
01061                 double val = getDbl("logTimeStep",0.001); //$NON-NLS-1$
01062                 stime.setTimeStep(val);
01063                 setDbl("logTimeStep",val); //$NON-NLS-1$
01064                 
01065             stime.setTotalTime(time);
01066             
01067                 //logger_.initCollisionLog(stime);
01068             logger_.extendTime(stime);
01069         }
01070                 
01071         public void stopSimulation(){
01072                 if (useDisk_)
01073                         logger_.closeWrites();
01074         }
01075     
01076     public boolean isUseDsik(){ return useDisk_; }
01077 
01082     public void setPosition(Integer pos) {
01083         if (super.setPosition(pos))
01084             notifyPosition(pos);
01085     }
01086 
01087     // viewで指定された以外に通知  //
01088     public void setPosition(Integer pos, GrxBaseView view) {
01089         if (super.setPosition(pos)){
01090             ListIterator<GrxPositionObserver> it = pos_obs_.listIterator();
01091             while (it.hasNext()) {
01092                 GrxPositionObserver pos_ob = it.next();
01093                 if(pos_ob != view)
01094                     pos_ob.updatePosition(this ,pos);
01095             }
01096         }
01097     }
01098 
01099     private ArrayList<GrxPositionObserver> pos_obs_ = new ArrayList<GrxPositionObserver>();
01100     
01101     public void addPosObserver(GrxPositionObserver v){
01102         pos_obs_.add(v);
01103     }
01104     
01105     public void deletePosObserver(GrxPositionObserver v){
01106         pos_obs_.remove(v);
01107     }
01108 
01109     private void notifyPosition(Integer pos){
01110         ListIterator<GrxPositionObserver> it = pos_obs_.listIterator();
01111         while (it.hasNext()) {
01112             GrxPositionObserver pos_ob = it.next();
01113             pos_ob.updatePosition(this, pos);
01114         }
01115     }
01116 
01117         public static class WorldStateEx {
01118                 public double time;
01119                 public Collision[] collisions;
01120                 private List<CharacterStateEx> charList = new ArrayList<CharacterStateEx>();
01121                 private Map<String, CharacterStateEx> charMap = new HashMap<String, CharacterStateEx>();
01122                 
01123                 public WorldStateEx() {}
01124                 
01125                 public WorldStateEx(WorldState wstate) {
01126                         setWorldState(wstate);
01127                 }
01128                 
01129                 public CharacterStateEx get(int idx) {
01130                         return charList.get(idx);
01131                 }
01132                 
01133                 public CharacterStateEx get(String charName) {
01134                         return charMap.get(charName);
01135                 }
01136                 
01137                 private CharacterStateEx _get(String charName) {
01138                         CharacterStateEx c = get(charName);
01139                         if (c == null) {
01140                                 c = new CharacterStateEx();
01141                                 c.characterName = charName;
01142                                 charMap.put(charName, c);
01143                                 charList.add(c);
01144                         }
01145                         return c;
01146                 }
01147                 
01148                 public int size() {
01149                         return charList.size();
01150                 }
01151                 
01152                 public String[] characters() {
01153                         String[] chars = new String[charList.size()];
01154                         for (int i=0; i<charList.size(); i++) 
01155                                 chars[i] = charList.get(i).characterName;
01156                         return chars;
01157                 }
01158                 
01159                 public void setWorldState(WorldState wstate) {
01160                         time = wstate.time;
01161                         collisions = wstate.collisions;
01162                         for (int i=0; i<wstate.characterPositions.length; i++) {
01163                                 _get(wstate.characterPositions[i].characterName).position = 
01164                                         wstate.characterPositions[i].linkPositions;
01165                         }
01166                 }
01167                 
01168                 public void setSensorState(String charName, SensorState state) {
01169                         _get(charName).sensorState = state;
01170                 }
01171                 
01172                 public void setTargetState(String charName, double[] targets) {
01173                         _get(charName).targetState = targets;
01174                 }
01175                 
01176                 public void setServoState(String charName, int[] servoStat) {
01177                         _get(charName).servoState = servoStat;
01178                 }
01179                 
01180                 public void setPowerState(String charName, double voltage, double current){
01181                         _get(charName).powerState = new double[]{voltage, current};
01182                 }
01183         
01184         protected Object clone() throws CloneNotSupportedException{
01185             WorldStateEx ret = new WorldStateEx();
01186             ret.time = time;
01187             if(collisions != null){
01188                 ret.collisions = new Collision[]{new Collision()};
01189                 ret.collisions[0].points = collisions[0].points;
01190             }
01191             for(CharacterStateEx    i:charList){
01192                 ret.charList.add((CharacterStateEx)i.clone()); 
01193             }
01194             for (String i:charMap.keySet()){
01195                 ret.charMap.put(new String(i), (CharacterStateEx)charMap.get(i).clone());
01196             }
01197             return ret;
01198         }
01199         }
01200         
01201         public static class CharacterStateEx {
01202                 public String    characterName;
01203                 public LinkPosition[] position;
01204                 public SensorState sensorState;
01205                 public double[]    targetState;
01206                 public int[]        servoState;
01207                 public double[]     powerState;
01208         
01209         protected Object clone() throws CloneNotSupportedException{
01210             CharacterStateEx ret = new CharacterStateEx();
01211             ret.characterName = new String( characterName );
01212             if(position != null){
01213                 ret.position = new LinkPosition[ position.length ];
01214                 for(int i = 0; i < position.length; ++i){
01215                     ret.position[i] = new LinkPosition();
01216                     if(position[i].p != null){
01217                         ret.position[i].p = new double[position[i].p.length];
01218                         GrxCopyUtil.copyDim(position[i].p, ret.position[i].p, position[i].p.length);
01219                     }
01220                     if(position[i].R != null){
01221                         ret.position[i].R = new double[position[i].R.length];
01222                         GrxCopyUtil.copyDim(position[i].R, ret.position[i].R, position[i].R.length);
01223                     }
01224                 }
01225             }
01226             if(sensorState != null){
01227                 ret.sensorState = new SensorState();
01228                 if( sensorState.accel != null){
01229                     ret.sensorState.accel = GrxCopyUtil.copyDoubleWDim(sensorState.accel);
01230                 }
01231                 if(sensorState.force != null){
01232                     ret.sensorState.force = GrxCopyUtil.copyDoubleWDim(sensorState.force);
01233                 }
01234                 if(sensorState.range != null){
01235                     ret.sensorState.range = GrxCopyUtil.copyDoubleWDim(sensorState.range);
01236                 }
01237                 if(sensorState.rateGyro != null){
01238                     ret.sensorState.rateGyro = GrxCopyUtil.copyDoubleWDim(sensorState.rateGyro);
01239                 }
01240                 if(sensorState.dq != null){
01241                     ret.sensorState.dq = new double[sensorState.dq.length];
01242                     GrxCopyUtil.copyDim(sensorState.dq, ret.sensorState.dq, sensorState.dq.length);
01243                 }
01244                 if(sensorState.q != null){
01245                     ret.sensorState.q = new double[sensorState.q.length];
01246                     GrxCopyUtil.copyDim(sensorState.q, ret.sensorState.q, sensorState.q.length);
01247                 }
01248                 if(sensorState.u != null){
01249                     ret.sensorState.u = new double[sensorState.u.length];
01250                     GrxCopyUtil.copyDim(sensorState.u, ret.sensorState.u, sensorState.u.length);
01251                 }
01252             }
01253             if(targetState != null){
01254                 ret.targetState = new double[targetState.length];
01255                 GrxCopyUtil.copyDim(targetState, ret.targetState, targetState.length);
01256             }
01257             if(servoState != null){
01258                 ret.servoState = new int[servoState.length];
01259                 GrxCopyUtil.copyDim(servoState, ret.servoState, servoState.length);
01260             }
01261             if(powerState != null){
01262                 ret.powerState = new double[powerState.length];
01263                 GrxCopyUtil.copyDim(powerState, ret.powerState, powerState.length);
01264             }
01265             return ret;
01266         }
01267         }
01268         
01269         private static class SensorInfoLocal implements Comparable {
01270                 String name;
01271                 String type; //#####[Changed] int -> string u5・Xl"・I
01272                 int id;
01273                 SensorInfo info;
01274                 public SensorInfoLocal(SensorInfo _info) {
01275                         info = _info;
01276                         name = info.name;
01277                         // ##### [Changed] NewModelLoader.IDL
01278                         //if( info.type.equals("Force") )
01279                         //{
01280                             //type = Integer.valueOf(SensorType.FORCE_SENSOR).intValue(); 
01281                         //}
01282                         //else if( info.type.equals("RateGyro") )
01283                         //{
01284                 //type = SensorType.RATE_GYRO; 
01285                         //}
01286                         //else if( info.type.equals("Acceleration") )
01287                         //{
01288                 //type = SensorType.ACCELERATION_SENSOR; 
01289                         //}
01290                         // type = info.type.value();
01291                         type = info.type;
01292                         // ###### [Changed]
01293 
01294 
01295                         id = info.id;
01296                 }
01297 
01298                 public int compareTo(Object o) {
01299                         if (o instanceof SensorInfoLocal) {
01300                                 SensorInfoLocal s = (SensorInfoLocal) o;
01301                                 int _this = getOrder(type);
01302                                 int _s  = getOrder(s.type);
01303                                 if (_this < _s) 
01304                                     return -1;
01305                                 else if (_this == _s){
01306                                         if (id < s.id)
01307                                         return -1;
01308                                 }else
01309                                         return 1;
01310                         }
01311                         return 1;
01312                 }
01313                 
01314                 private int getOrder(String type) {
01315                         if (type.equals("Force"))  //$NON-NLS-1$
01316                                 return 0;
01317                         else if (type.equals("RateGyro"))  //$NON-NLS-1$
01318                                 return 1;
01319                         else if (type.equals("Acceleration"))  //$NON-NLS-1$
01320                                 return 2;
01321                         else if (type.equals("Vision"))  //$NON-NLS-1$
01322                                 return 3;
01323                         else if (type.equals("Range")) //$NON-NLS-1$
01324                                 return 4;
01325                         else
01326                                 return -1;
01327 
01328 
01329                 }
01330 
01331         }
01332 
01333         public void delete(){
01334                 if(useDisk_)
01335                         logger_.closeReads();
01336                 super.delete();        
01337         }
01338 
01339 } 


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