00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
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")
00053 public class GrxWorldStateItem extends GrxTimeSeriesItem {
00054 public static final String TITLE = "World State";
00055 public static final String DEFAULT_DIR = "/";
00056 public static final String FILE_EXTENSION = "log";
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";
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"); }
00078 public void run(){
00079 if(isRemoved()){
00080 MessageDialog.openWarning(
00081 GrxUIPerspectiveFactory.getCurrentShell(),
00082 MessageBundle.get("GrxWorldStateItem.dialog.title.saveLog"),
00083 MessageBundle.get("GrxWorldStateItem.dialog.message.saveLog"));
00084 return;
00085 }
00086 _saveLog();
00087 }
00088 };
00089 private Action saveCSV_ = new Action(){
00090 public String getText(){ return MessageBundle.get("GrxWorldStateItem.menu.saveAsCSV"); }
00091 public void run(){
00092 if(isRemoved()){
00093 MessageDialog.openWarning(
00094 GrxUIPerspectiveFactory.getCurrentShell(),
00095 MessageBundle.get("GrxWorldStateItem.dialog.title.saveAsCSV"),
00096 MessageBundle.get("GrxWorldStateItem.dialog.message.saveAsCSV"));
00097 return;
00098 }
00099 _saveCSV();
00100 }
00101 };
00102 private Action clear_ = new Action(){
00103 public String getText(){ return MessageBundle.get("GrxWorldStateItem.menu.clearLog"); }
00104 public void run(){
00105 if( MessageDialog.openQuestion( null, MessageBundle.get("GrxWorldStateItem.dialog.title.clearLog"), MessageBundle.get("GrxWorldStateItem.dialog.message.clearLog") ) )
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;
00122 tempDir_ = tempDirBase_+getName();
00123
00124 Action load = new Action(){
00125 public String getText(){ return MessageBundle.get("GrxWorldStateItem.menu.loadLog"); }
00126 public void run(){
00127 FileDialog fdlg = new FileDialog(GrxUIPerspectiveFactory.getCurrentShell(), SWT.OPEN);
00128 fdlg.setFilterExtensions(new String[]{"*.log"});
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" );
00142
00143 logger_.init();
00144 }
00145
00146
00147
00148
00149
00150 public boolean create() {
00151 clearLog();
00152 setDbl("logTimeStep", 0.001);
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);
00170 storeAllPos_ = isTrue("storeAllPosition", storeAllPos_);
00171 int size = getInt("bufferSize", MAX_RAM_BUFFER_SIZE);
00172 if ( useDisk_ ) {
00173 super.setMaximumLogSize(MAX_RAM_BUFFER_SIZE);
00174 } else {
00175 GrxDebugUtil.println("GrxWorldStateItem: useDisk = false");
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");
00210 }
00211 });
00212 }
00213
00214 @SuppressWarnings("unchecked")
00215 public void registerCharacter(String cname, BodyInfo binfo) {
00216 ArrayList<String> logList = new ArrayList<String>();
00217 logList.add("time");
00218 logList.add("float");
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");
00237 logList.add("float[3]");
00238 logList.add(jname+".rotation");
00239 logList.add("float[4]");
00240 }
00241
00242 for (short i=0; i<jointList.size(); i++) {
00243 int idx = jointList.indexOf(i);
00244 if (idx >= 0) {
00245 String jname = li[idx].name;
00246 logList.add(jname+".angle");
00247 logList.add("float");
00248 logList.add(jname+".jointTorque");
00249 logList.add("float");
00250 }
00251 }
00252
00253 for (int i=0; i<sensList.size(); i++) {
00254 String sname = sensList.get(i).name;
00255
00256
00257
00258
00259
00260
00261
00262
00263
00264
00265
00266
00267
00268
00269
00270
00271
00272
00273
00274 if(sensList.get(i).type.equals("Force")){
00275 logList.add(sname+".force");
00276 logList.add("float[3]");
00277 logList.add(sname+".torque");
00278 logList.add("float[3]");
00279 }else if(sensList.get(i).type.equals("RateGyro")){
00280 logList.add(sname+".angularVelocity");
00281 logList.add("float[3]");
00282 }else if(sensList.get(i).type.equals("Acceleration")){
00283 logList.add(sname+".acceleration");
00284 logList.add("float[3]");
00285 }else if(sensList.get(i).type.equals("Range")){
00286 logList.add(sname+".range");
00287 SensorInfo info = sensList.get(i).info;
00288 int half = (int)(info.specValues[0]/2/info.specValues[1]);
00289 logList.add("float["+(half*2+1)+"]");
00290 }else{
00291 }
00292 }
00293
00294 logList.add("command");
00295 logList.add("float["+jointList.size()+"]");
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);
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
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
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);
00446 setDbl("logTimeStep",val);
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);
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);
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;
00559 lpos.R = null;
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
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);
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");
00652
00653 if (!logFile.isFile())
00654 return;
00655
00656 clearLog();
00657 tempDir_ = tempDirBase_ + getName();
00658 logger_.setTempDir(tempDir_);
00659 logger_.load(fname, "");
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());
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];
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("[.]");
00699 if (str.length <= 1) {
00700 continue;
00701 } else if (str[1].equals("translation")) {
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")) {
00707 if (!storeAllPos_)
00708 lposList.add(new LinkPosition());
00709 jointCount++;
00710 } else if (str[1].equals("force")) {
00711 forceCount++;
00712 } else if (str[1].equals("angularVelocity")) {
00713 gyroCount++;
00714 } else if (str[1].equals("acceleration")) {
00715 accelCount++;
00716 } else if (str[1].equals("range")){
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][];
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");
00825 fdlg.setFilterExtensions(new String[]{"*.log"});
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");
00837 } else {
00838
00839 LogManager temp = _restoreLogFileFromSuperLog();
00840 if(temp != null){
00841 temp.save(fPath, getName()+".prj");
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
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
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
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
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);
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);
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
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
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";
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);
01062 stime.setTimeStep(val);
01063 setDbl("logTimeStep",val);
01064
01065 stime.setTotalTime(time);
01066
01067
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
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;
01272 int id;
01273 SensorInfo info;
01274 public SensorInfoLocal(SensorInfo _info) {
01275 info = _info;
01276 name = info.name;
01277
01278
01279
01280
01281
01282
01283
01284
01285
01286
01287
01288
01289
01290
01291 type = info.type;
01292
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"))
01316 return 0;
01317 else if (type.equals("RateGyro"))
01318 return 1;
01319 else if (type.equals("Acceleration"))
01320 return 2;
01321 else if (type.equals("Vision"))
01322 return 3;
01323 else if (type.equals("Range"))
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 }