19 package com.generalrobotix.ui.item;
22 import java.lang.reflect.InvocationTargetException;
23 import java.lang.Runtime;
24 import java.nio.ByteBuffer;
26 import java.util.zip.ZipEntry;
27 import java.util.zip.ZipFile;
28 import javax.vecmath.Matrix3d;
30 import org.eclipse.core.runtime.IProgressMonitor;
31 import org.eclipse.jface.action.Action;
32 import org.eclipse.jface.dialogs.MessageDialog;
33 import org.eclipse.jface.dialogs.ProgressMonitorDialog;
34 import org.eclipse.jface.operation.IRunnableWithProgress;
35 import org.eclipse.swt.SWT;
36 import org.eclipse.swt.widgets.FileDialog;
37 import org.eclipse.swt.widgets.DirectoryDialog;
52 @SuppressWarnings(
"serial")
54 public static final String TITLE =
"World State";
55 public static final String DEFAULT_DIR =
"/";
56 public static final String FILE_EXTENSION =
"log";
58 public static final double DEFAULT_TOTAL_TIME = 20.0;
59 private static final int MAX_RAM_BUFFER_SIZE = -1;
60 private static final int LOAD_LOG_MODITOR_DIM = 32;
61 private static final long HEAP_MEMORY_TOLERANCE = 4*1024*1024;
62 private static final String OVER_HEAP_LOG_DIR_NAME =
"over";
67 private int prePos_ = -1;
70 private float recDat_[][];
71 private String lastCharName_ =
null;
72 private boolean initLogFlag_ =
false;
73 private boolean useDisk_ =
true;
74 private boolean storeAllPos_ =
true;
76 private Action save_ =
new Action(){
77 public String getText(){
return MessageBundle.
get(
"GrxWorldStateItem.menu.saveLog"); }
80 MessageDialog.openWarning(
89 private Action saveCSV_ =
new Action(){
90 public String getText(){
return MessageBundle.
get(
"GrxWorldStateItem.menu.saveAsCSV"); }
93 MessageDialog.openWarning(
102 private Action clear_ =
new Action(){
103 public String getText(){
return MessageBundle.
get(
"GrxWorldStateItem.menu.clearLog"); }
105 if( MessageDialog.openQuestion(
null,
MessageBundle.
get(
"GrxWorldStateItem.dialog.title.clearLog"),
MessageBundle.
get(
"GrxWorldStateItem.dialog.message.clearLog") ) )
111 private Matrix3d m3d =
new Matrix3d();
113 private Matrix3d m3dg =
new Matrix3d();
119 super(
name, manager);
121 tempDirBase_ = System.getProperty(
"java.io.tmpdir")+File.separator+
"grxui-"+System.getProperty(
"user.name")+File.separator;
122 tempDir_ = tempDirBase_+getName();
124 Action load =
new Action(){
125 public String getText(){
return MessageBundle.
get(
"GrxWorldStateItem.menu.loadLog"); }
128 fdlg.setFilterExtensions(
new String[]{
"*.log"});
129 fdlg.setFilterPath(getDefaultDir().getAbsolutePath());
130 final String fPath = fdlg.open();
131 _loadLog(
new File(fPath));
132 setDefaultDirectory(
new File(fPath).getParent());
137 setMenuItem(saveCSV_);
141 setIcon(
"world.png" );
152 setDbl(
"logTimeStep", 0.001);
162 save_.setEnabled(bAble);
163 saveCSV_.setEnabled(bAble);
164 clear_.setEnabled(bAble);
168 super.restoreProperties();
169 useDisk_ = isTrue(
"useDisk",
true);
170 storeAllPos_ = isTrue(
"storeAllPosition", storeAllPos_);
171 int size = getInt(
"bufferSize", MAX_RAM_BUFFER_SIZE);
173 super.setMaximumLogSize(MAX_RAM_BUFFER_SIZE);
176 super.setMaximumLogSize(
size);
181 File oldDir =
new File(LOG_DIR+File.separator+getName());
182 super.rename(newName);
183 File newDir =
new File(LOG_DIR+File.separator+getName());
184 if (newDir.isDirectory())
187 if (oldDir.isDirectory())
188 oldDir.renameTo(newDir);
197 initLogFlag_ =
false;
202 lastCharName_ =
null;
209 notifyObservers(
"ClearLog");
214 @SuppressWarnings(
"unchecked")
215 public
void registerCharacter(String cname, BodyInfo binfo) {
216 ArrayList<String> logList =
new ArrayList<String>();
218 logList.add(
"float");
220 LinkInfo[] li = binfo.links();
221 ArrayList<Short> jointList =
new ArrayList<Short>();
222 ArrayList<SensorInfoLocal> sensList =
new ArrayList<SensorInfoLocal>();
223 for (
int i=0;
i<li.length;
i++) {
224 jointList.add(li[
i].jointId);
226 SensorInfo[] si = li[
i].sensors;
227 for (
int j=0; j<si.length; j++)
231 Collections.sort(sensList);
233 int len = storeAllPos_ ? li.length : 1;
234 for (
int i=0;
i< len;
i++) {
235 String jname = li[
i].name;
236 logList.add(jname+
".translation");
237 logList.add(
"float[3]");
238 logList.add(jname+
".rotation");
239 logList.add(
"float[4]");
242 for (
short i=0;
i<jointList.size();
i++) {
243 int idx = jointList.indexOf(
i);
245 String jname = li[idx].name;
246 logList.add(jname+
".angle");
247 logList.add(
"float");
248 logList.add(jname+
".jointTorque");
249 logList.add(
"float");
253 for (
int i=0;
i<sensList.size();
i++) {
254 String sname = sensList.get(
i).name;
274 if(sensList.get(
i).type.equals(
"Force")){
275 logList.add(sname+
".force");
276 logList.add(
"float[3]");
277 logList.add(sname+
".torque");
278 logList.add(
"float[3]");
279 }
else if(sensList.get(
i).type.equals(
"RateGyro")){
280 logList.add(sname+
".angularVelocity");
281 logList.add(
"float[3]");
282 }
else if(sensList.get(
i).type.equals(
"Acceleration")){
283 logList.add(sname+
".acceleration");
284 logList.add(
"float[3]");
285 }
else if(sensList.get(
i).type.equals(
"Range")){
286 logList.add(sname+
".range");
287 SensorInfo
info = sensList.get(
i).info;
288 int half = (
int)(
info.specValues[0]/2/
info.specValues[1]);
289 logList.add(
"float["+(half*2+1)+
"]");
294 logList.add(
"command");
295 logList.add(
"float["+jointList.size()+
"]");
296 logList.add(
"servoState");
297 logList.add(
"float["+jointList.size()+
"]");
298 logList.add(
"powerState");
299 logList.add(
"float[2]");
301 logger_.
addLogObject(cname, logList.toArray(
new String[0]));
309 _addValueToLog(t,
obj);
313 _addValueToLog(t,
obj);
316 if( Runtime.getRuntime().freeMemory() < HEAP_MEMORY_TOLERANCE){
318 File
f =
new File(tempDir_);
319 File pf =
f.getParentFile();
320 if (!pf.isDirectory()){
323 tempDir_ = tempDirBase_ + getName() + File.separator + OVER_HEAP_LOG_DIR_NAME;
324 changePos_ = super.getLogSize();
325 _addValueToLog(t,
obj);
328 super.addValue(t,
obj);
339 super.addValue(newStat_.
time,
null);
358 if (cols !=
null && cols.length > 0) {
359 List<CollisionPoint> cdList =
new ArrayList<CollisionPoint>();
360 for (
int i=0;
i < cols.length;
i++) {
361 if( cols[
i].points ==
null){
364 for (
int j=0; j<cols[
i].points.length; j++)
365 cdList.add(cols[
i].points[j]);
367 CollisionPoint[] cd = cdList.toArray(
new CollisionPoint[0]);
370 }
catch (Exception e) {
376 recDat_[
i][k++] = (float) newStat_.
time;
378 int len = storeAllPos_ ? cpos.position.length : 1;
379 for (
int j=0; j<len; j++) {
380 for (
int m=0; m<3; m++)
381 recDat_[
i][k++] = (
float)cpos.position[j].p[m];
382 m3d.set(cpos.position[j].R);
385 recDat_[
i][k++] = (float) a4d.x;
386 recDat_[
i][k++] = (
float) a4d.y;
387 recDat_[
i][k++] = (float) a4d.z;
388 recDat_[
i][k++] = (
float) a4d.angle;
391 String cname = cpos.characterName;
392 SensorState sdata = cpos.sensorState;
394 for (
int j=0; j<sdata.q.length; j++) {
395 recDat_[
i][k++] = (float) sdata.q[j];
396 recDat_[
i][k++] = (
float) sdata.u[j];
398 for (
int j=0; j<sdata.force.length; j++) {
399 for (
int m=0; m<sdata.force[j].length; m++)
400 recDat_[
i][k++] = (
float)sdata.force[j][m];
402 for (
int j=0; j<sdata.rateGyro.length; j++) {
403 for (
int m=0; m<sdata.rateGyro[j].length; m++)
404 recDat_[
i][k++] = (
float)sdata.rateGyro[j][m];
406 for (
int j=0; j<sdata.accel.length; j++) {
407 for (
int m=0; m<sdata.accel[j].length; m++)
408 recDat_[
i][k++] = (
float)sdata.accel[j][m];
410 if (sdata.range !=
null){
411 for (
int j=0; j<sdata.range.length; j++) {
412 for (
int m=0; m<sdata.range[j].length; m++)
413 recDat_[
i][k++] = (
float)sdata.range[j][m];
417 if (cpos.targetState !=
null){
418 for (
int j=0; j<cpos.targetState.length; j++){
419 recDat_[
i][k++] = (float)cpos.targetState[j];
422 if (cpos.servoState !=
null){
423 for (
int j=0; j<cpos.servoState.length; j++){
424 ByteBuffer
buffer = ByteBuffer.allocate(10);
425 buffer.putInt(cpos.servoState[j]);
431 if (cpos.powerState !=
null){
432 for (
int j=0; j<cpos.powerState.length; j++){
433 recDat_[
i][k++] = (float)cpos.powerState[j];
437 temp.
put(cname, recDat_[
i]);
438 }
catch (Exception e) {
445 double val = getDbl(
"logTimeStep",0.001);
446 setDbl(
"logTimeStep",
val);
459 double val = getDbl(
"logTimeStep",0.001);
465 val = simItem.
getDbl(
"totalTime", DEFAULT_TOTAL_TIME);
468 val = DEFAULT_TOTAL_TIME;
481 }
catch (IOException e1) {
482 e1.printStackTrace();
485 for (
int i=0;
i<recDat_.length;
i++)
493 int pos = getPosition();
496 if (pos == getLogSize()-1 && newStat_ !=
null){
499 if (pos != prePos_ && preStat_ !=
null)
503 if( pos > changePos_ && changePos_ >= 0){
504 ret = _getValueFromLog( pos - changePos_ );
516 ret = _getValueFromLog( pos );
518 if( pos > changePos_ && changePos_ >= 0 ){
519 ret = _getValueFromLog(pos - changePos_);
533 preStat_.
collisions =
new Collision[]{
new Collision()};
540 preStat_.
time = (double)
f[k++];
541 for (
int j=0; j<cpos.
position.length; j++) {
542 LinkPosition lpos = cpos.
position[j];
543 if (storeAllPos_ || j == 0) {
544 for (
int m=0; m<3; m++)
545 lpos.p[m] = (
double)
f[k++];
546 a4dg.set((
double)
f[k++], (double)
f[k++], (
double)
f[k++], (double)
f[k++]);
548 lpos.R[0] = m3dg.m00;
549 lpos.R[1] = m3dg.m01;
550 lpos.R[2] = m3dg.m02;
551 lpos.R[3] = m3dg.m10;
552 lpos.R[4] = m3dg.m11;
553 lpos.R[5] = m3dg.m12;
554 lpos.R[6] = m3dg.m20;
555 lpos.R[7] = m3dg.m21;
556 lpos.R[8] = m3dg.m22;
565 for (
int j=0; j<sdata.q.length; j++) {
566 sdata.q[j] = (double)
f[k++];
567 sdata.u[j] = (double)
f[k++];
569 for (
int j=0; j<sdata.force.length; j++) {
570 for (
int m=0; m<sdata.force[j].length; m++)
571 sdata.force[j][m] = (
double)
f[k++];
573 for (
int j=0; j<sdata.rateGyro.length; j++) {
574 for (
int m=0; m<sdata.rateGyro[j].length; m++)
575 sdata.rateGyro[j][m] = (
double)
f[k++];
577 for (
int j=0; j<sdata.accel.length; j++) {
578 for (
int m=0; m<sdata.accel[j].length; m++)
579 sdata.accel[j][m] = (
double)
f[k++];
581 if (sdata.range !=
null){
582 for (
int j=0; j<sdata.range.length; j++) {
583 for (
int m=0; m<sdata.range[j].length; m++)
584 sdata.range[j][m] = (
double)
f[k++];
595 ByteBuffer
buffer = ByteBuffer.allocate(10);
597 int ss =
buffer.getInt(0);
608 }
catch (IOException e) {
617 IRunnableWithProgress op =
new IRunnableWithProgress() {
618 public void run(IProgressMonitor monitor)
throws InvocationTargetException, InterruptedException {
621 ZipFile
local =
new ZipFile(logFile);
624 }
catch ( IOException ex ){
625 ex.printStackTrace();
627 }
catch ( Exception ex){
628 ex.printStackTrace();
632 monitor.beginTask(
"Loading log as a file:"+logFile.getName(),
size + LOAD_LOG_MODITOR_DIM + 2);
633 _loadLog(logFile,monitor);
635 notifyObservers(
"LoadLog");
640 }
catch (InvocationTargetException e) {
642 }
catch (InterruptedException e) {
647 private void _loadLog(File logFile, IProgressMonitor monitor )
throws InterruptedException{
648 String fname = logFile.getAbsolutePath();
651 logFile =
new File(
"log"+File.separator+getName()+
".log");
653 if (!logFile.isFile())
657 tempDir_ = tempDirBase_ + getName();
659 logger_.
load(fname,
"");
677 Enumeration<? extends ZipEntry> e =
new ZipFile(fname).entries();
678 while (e.hasMoreElements()) {
680 if (monitor.isCanceled())
681 throw new InterruptedException();
682 String entry = ((ZipEntry)e.nextElement()).getName();
688 lastCharName_ =
new File(entry).getName().split(
".tmp")[0];
690 List<LinkPosition> lposList =
new ArrayList<LinkPosition>();
697 for (
int i=0;
i<format.length;
i++) {
698 String[] str = format[
i].split(
"[.]");
699 if (str.length <= 1) {
701 }
else if (str[1].equals(
"translation")) {
702 LinkPosition lpos =
new LinkPosition();
703 lpos.p =
new double[3];
704 lpos.R =
new double[9];
706 }
else if (str[1].equals(
"angle")) {
708 lposList.add(
new LinkPosition());
710 }
else if (str[1].equals(
"force")) {
712 }
else if (str[1].equals(
"angularVelocity")) {
714 }
else if (str[1].equals(
"acceleration")) {
716 }
else if (str[1].equals(
"range")){
723 cpos.
position = lposList.toArray(
new LinkPosition[0]);
724 if (jointCount+forceCount+accelCount+gyroCount+rangeCount > 1) {
725 SensorState sdata =
new SensorState();
726 sdata.q =
new double[jointCount];
727 sdata.u =
new double[jointCount];
728 sdata.force =
new double[forceCount][6];
729 sdata.rateGyro =
new double[gyroCount][3];
730 sdata.accel =
new double[accelCount][3];
731 sdata.range =
new double[rangeCount][];
735 preStat_.
charMap.put(lastCharName_, cpos);
741 int workdim[] =
new int[LOAD_LOG_MODITOR_DIM];
742 int workdimCounter = 0;
743 int localLength = workdim.length;
745 for(
int i = 0;
i < localLength; ++
i){
746 workdim[
i] = datLen * (
i + 1) / localLength;
753 for (
int i=0;
i<recDat_.length;
i++)
757 for (
int i=0;
i < datLen;
i++){
760 super.addValue(
null ,
null);
761 }
catch (Exception ex){
762 ex.printStackTrace();
769 if( Runtime.getRuntime().freeMemory() < HEAP_MEMORY_TOLERANCE){
771 _createOverLog( worldState.
time,
772 i == 0 ? worldState.
time : getTime(0),
778 super.addValue( worldState.
time , worldState.
clone());
779 }
catch (Exception ex){
780 ex.printStackTrace();
786 if ( workdim[workdimCounter] <
i){
800 MessageDialog.openError(
804 System.getProperty(
"line.separator") + fname);
807 MessageDialog.openError(
811 System.getProperty(
"line.separator") + fname);
812 }
catch (IOException e) {
814 MessageDialog.openError(
818 System.getProperty(
"line.separator") + fname);
825 fdlg.setFilterExtensions(
new String[]{
"*.log"});
826 fdlg.setFilterPath(getDefaultDir().getAbsolutePath());
827 final String fPath = fdlg.open();
829 Thread t =
new Thread() {
836 logger_.
save(fPath, getName()+
".prj");
839 LogManager temp = _restoreLogFileFromSuperLog();
841 temp.
save(fPath, getName()+
".prj");
847 }
catch (IOException ex){
848 ex.printStackTrace();
849 }
catch (Exception ex) {
850 ex.printStackTrace();
855 setDefaultDirectory(
new File(fPath).getParent());
860 private void _createOverLog(
double currentTime,
double startTime,
int changePos,
int overPos ){
861 changePos_ = changePos;
869 String overDir = tempDirBase_ + getName() + File.separator + OVER_HEAP_LOG_DIR_NAME;
881 }
catch (IOException ex) {
882 ex.printStackTrace();
893 }
catch (IOException ex) {
894 ex.printStackTrace();
898 for(
int i = 0;
i < overPos; ++
i){
900 super.addValue( (
double)logger_.
get(lastCharName_,
i)[0],
null);
901 }
catch (Exception ex){
902 ex.printStackTrace();
910 if( changePos_ < 0 ){
912 for(
int index = 0; index < getLogSize(); ++index){
913 TValue localTValue = getObject(index);
914 _addValueToLogFromSuperLog(localTValue.
getTime(), localTValue.
getValue(), logger_);
920 }
catch (IOException ex){
921 ex.printStackTrace();
922 }
catch (Exception ex) {
923 ex.printStackTrace();
931 }
catch (IOException ex){
932 ex.printStackTrace();
933 }
catch (Exception ex) {
934 ex.printStackTrace();
947 double localTime = getDbl(
"logTimeStep",0.001);
953 localTime = simItem.
getDbl(
"totalTime", DEFAULT_TOTAL_TIME);
956 localTime = DEFAULT_TOTAL_TIME;
966 }
catch (IOException e1) {
967 e1.printStackTrace();
971 for(
int index = 0; index < getLogSize(); ++index){
972 TValue localTValue = getObject(index);
975 _addValueToLogFromSuperLog(localTValue.
getTime(),
val, temp);
988 }
catch (IOException ex){
989 ex.printStackTrace();
990 }
catch (Exception ex) {
991 ex.printStackTrace();
1000 ddlg.setFilterPath(getDefaultDir().getCanonicalPath());
1001 }
catch (IOException e1) {
1003 e1.printStackTrace();
1005 final String dir = ddlg.open();
1007 Thread t =
new Thread() {
1012 temp = _restoreLogFileFromSuperLog();
1016 String fname = dir+File.separator+
name+
".csv";
1027 e.printStackTrace();
1030 if(temp !=
null && temp != logger_){
1036 setDefaultDirectory(dir);
1043 Double t = super.getTime(pos);
1044 if (t ==
null && lastCharName_ !=
null) {
1046 float[]
f = logger_.
get(lastCharName_, pos);
1049 }
catch (IOException e) {
1050 e.printStackTrace();
1061 double val = getDbl(
"logTimeStep",0.001);
1063 setDbl(
"logTimeStep",
val);
1083 if (super.setPosition(pos))
1084 notifyPosition(pos);
1089 if (super.setPosition(pos)){
1090 ListIterator<GrxPositionObserver> it = pos_obs_.listIterator();
1091 while (it.hasNext()) {
1099 private ArrayList<GrxPositionObserver> pos_obs_ =
new ArrayList<GrxPositionObserver>();
1110 ListIterator<GrxPositionObserver> it = pos_obs_.listIterator();
1111 while (it.hasNext()) {
1120 private List<CharacterStateEx>
charList =
new ArrayList<CharacterStateEx>();
1121 private Map<String, CharacterStateEx>
charMap =
new HashMap<String, CharacterStateEx>();
1141 c.characterName = charName;
1153 String[] chars =
new String[
charList.size()];
1162 for (
int i=0;
i<wstate.characterPositions.length;
i++) {
1164 wstate.characterPositions[
i].linkPositions;
1184 protected Object
clone() throws CloneNotSupportedException{
1188 ret.
collisions =
new Collision[]{
new Collision()};
1209 protected Object
clone() throws CloneNotSupportedException{
1226 if(sensorState !=
null){
1228 if( sensorState.accel !=
null){
1231 if(sensorState.force !=
null){
1234 if(sensorState.range !=
null){
1237 if(sensorState.rateGyro !=
null){
1240 if(sensorState.dq !=
null){
1241 ret.
sensorState.dq =
new double[sensorState.dq.length];
1244 if(sensorState.q !=
null){
1245 ret.
sensorState.q =
new double[sensorState.q.length];
1248 if(sensorState.u !=
null){
1249 ret.
sensorState.u =
new double[sensorState.u.length];
1253 if(targetState !=
null){
1257 if(servoState !=
null){
1261 if(powerState !=
null){
1262 ret.
powerState =
new double[powerState.length];
1301 int _this = getOrder(
type);
1302 int _s = getOrder(s.type);
1305 else if (_this == _s){
1315 if (
type.equals(
"Force"))
1317 else if (
type.equals(
"RateGyro"))
1319 else if (
type.equals(
"Acceleration"))
1321 else if (
type.equals(
"Vision"))
1323 else if (
type.equals(
"Range"))
1333 public void delete(){