18 package com.generalrobotix.ui.item;
20 import java.math.BigInteger;
21 import java.util.ArrayList;
22 import java.util.Collection;
23 import java.util.Date;
24 import java.util.List;
25 import java.util.Vector;
45 import org.eclipse.jface.dialogs.InputDialog;
46 import org.eclipse.jface.dialogs.MessageDialog;
47 import org.eclipse.swt.widgets.Display;
48 import org.eclipse.ui.IWorkbench;
49 import org.eclipse.ui.IWorkbenchPage;
50 import org.eclipse.ui.IWorkbenchWindow;
51 import org.eclipse.ui.PartInitException;
52 import org.eclipse.ui.PlatformUI;
53 import org.omg.CosNaming.NameComponent;
54 import org.omg.CosNaming.NamingContext;
73 @SuppressWarnings(
"serial")
75 public static final String TITLE =
"Simulation";
76 private static final String FORMAT1 =
"%8.3f";
77 private static final int WAIT_COUNT_ = 4;
80 private List<ControllerAttribute> controllers_ =
new ArrayList<ControllerAttribute>();
81 private WorldStateHolder stateH_ =
new WorldStateHolder();
82 private SensorStateHolder cStateH_ =
new SensorStateHolder();
83 private List<String> robotEntry_ =
new ArrayList<String>();
86 private boolean isInteractive_ =
true;
87 private boolean isExecuting_ =
false;
88 private boolean isSuspending_ =
false;
89 private double simulateTime_ = 0;
90 private boolean isIntegrate_ =
true;
91 private boolean isRealTime_ =
false;
92 private double stepTime_ = 0.001;
93 private double totalTime_ = 20;
94 private double logStepTime_ = 0.05;
96 private double viewSimulationStep_=0;
100 private static final int interval_ = 10;
104 private Object lock2_ =
new Object();
107 super(
name, manager);
109 setIcon(
"grxrobot.png");
114 setDbl(
"totalTime", 20.0);
115 setDbl(
"timeStep", 0.001);
116 setDbl(
"gravity", 9.8);
117 setProperty(
"method",
"RUNGE_KUTTA");
118 setBool(
"integrate",
true);
119 setBool(
"viewsimulate",
false);
120 setBool(
"realTime",
false);
130 private double simTime_ = 0.0;
132 private static final int EXEC = -1;
133 private static final int TIMEOVER = 0;
134 private static final int STOP = 1;
135 private static final int INTERRUPT = 2;
136 private int simThreadState_ = EXEC;
137 private Object lock_ =
new Object();
138 private Object lock3_ =
new Object();
139 private boolean viewSimulationUpdate_ =
false;
150 if (currentWorld_ ==
null) {
156 isInteractive_ = isInteractive;
158 if (isInteractive_ && currentWorld_.
getLogSize() > 0) {
167 if(openHRPView !=
null)
170 isIntegrate_ = isTrue(
"integrate",
true);
171 isRealTime_ = isTrue(
"realTime",
false);
172 totalTime_ = getDbl(
"totalTime", 20.0);
173 stepTime_ = getDbl(
"timeStep", 0.001);
174 logStepTime_ = currentWorld_.
getDbl(
"logTimeStep", 0.001);
175 isSimulatingView_ = isTrue(
"viewsimulate",
false);
176 if(stepTime_ > logStepTime_ ){
182 if (!initDynamicsSimulator()) {
186 if (!initController()) {
189 }
catch (Exception e) {
194 if(isSimulatingView_){
197 IWorkbench workbench = PlatformUI.getWorkbench();
198 IWorkbenchWindow window = workbench.getActiveWorkbenchWindow();
199 IWorkbenchPage page = window.getActivePage();
201 page.showView(
"com.generalrobotix.ui.view.Grx3DViewPart",
null, IWorkbenchPage.VIEW_CREATE);
202 }
catch (PartInitException e1) {
203 e1.printStackTrace();
209 if(!isSimulatingView_){
212 IWorkbench workbench = PlatformUI.getWorkbench();
213 IWorkbenchWindow window = workbench.getActiveWorkbenchWindow();
214 IWorkbenchPage page = window.getActivePage();
216 page.showView(
"com.generalrobotix.ui.view.GrxLoggerViewPart",
null, IWorkbenchPage.VIEW_CREATE);
217 }
catch (PartInitException e1) {
218 e1.printStackTrace();
222 notifyObservers(
"StartSimulation", isSimulatingView_);
228 currentWorld_.
init();
229 simThreadState_ = EXEC;
230 viewSimulationUpdate_ =
false;
231 simThread_ = _createSimulationThread();
234 Runnable run =
new Runnable(){
236 switch(simThreadState_){
239 simThreadState_=EXEC;
241 simThreadState_=STOP;
246 if(isSimulatingView_){
247 if(viewSimulationUpdate_){
252 synchronized(lock3_){
253 viewSimulationUpdate_=
false;
258 Display display = Display.getCurrent();
259 if ( display!=
null && !display.isDisposed()){
260 display.timerExec(interval_,
this);
265 synchronized(lock2_){
279 Display display = Display.getCurrent();
280 if ( display!=
null && !display.isDisposed()){
281 display.timerExec(interval_, run);
289 String updateTimeMsg(){
291 MessageBundle.
get(
"GrxOpenHRPView.dialog.message.simTime0") + String.format(FORMAT1, simTime_) +
MessageBundle.
get(
"GrxOpenHRPView.dialog.message.simTime1") +
292 MessageBundle.
get(
"GrxOpenHRPView.dialog.message.simTime2") + String.format(FORMAT1, simulateTime_) +
MessageBundle.
get(
"GrxOpenHRPView.dialog.message.simTime3") +
293 MessageBundle.
get(
"GrxOpenHRPView.dialog.message.simTime4") + String.format(FORMAT1, simulateTime_/simTime_);
298 Thread thread =
new Thread(){
302 long startT = System.currentTimeMillis();
304 while (isExecuting_) {
306 long s = System.currentTimeMillis();
308 suspendT += System.currentTimeMillis() - s;
310 if (!simulateOneStep()){
311 long s = System.currentTimeMillis();
313 simThreadState_ = TIMEOVER;
316 suspendT += System.currentTimeMillis() - s;
317 if(simThreadState_==STOP)
321 long s = System.currentTimeMillis();
322 long sleep = (long)(simTime_*1000.0) - (s - startT);
330 isExecuting_ =
false;
331 simulateTime_ += (System.currentTimeMillis() - startT - suspendT)/1000.0;
336 simThreadState_ = STOP;
337 }
catch (Exception e) {
339 isExecuting_ =
false;
340 simThreadState_ = INTERRUPT;
344 thread.setPriority(Thread.currentThread().getPriority() - 1);
349 simThread_ = _createSimulationThread();
355 isExecuting_ =
false;
363 System.out.println(
new java.util.Date()+timeMsg_.replace(
" ",
"").replace(
"\n",
" : "));
364 if (isInteractive_) {
365 isInteractive_ =
false;
366 execSWT(
new Runnable(){
372 Thread.currentThread() != simThread_
378 notifyObservers(
"StopSimulation");
389 if (simTime_ > totalTime_ ) {
394 for (
int i = 0;
i<controllers_.size();
i++) {
397 attr.
input(simTime_);
400 simTime_ += stepTime_;
403 for (
int i = 0;
i < controllers_.size();
i++) {
411 currentDynamics_.stepSimulation();
413 currentDynamics_.calcWorldForwardKinematics();
418 if ((simTime_ % logStepTime_) < stepTime_) {
419 currentDynamics_.getWorldState(stateH_);
421 for (
int i=0;
i<robotEntry_.size();
i++) {
422 String
name = robotEntry_.get(
i);
423 currentDynamics_.getCharacterSensorState(
name, cStateH_);
427 wsx_.
time = simTime_;
428 currentWorld_.
addValue(simTime_, wsx_);
432 if(isSimulatingView_){
433 if ((simTime_ % viewSimulationStep_) < stepTime_) {
435 currentDynamics_.getWorldState(stateH_);
437 for (
int i=0;
i<robotEntry_.size();
i++) {
438 String
name = robotEntry_.get(
i);
439 currentDynamics_.getCharacterSensorState(
name, cStateH_);
443 wsx_.
time = simTime_;
445 synchronized(lock3_){
447 viewSimulationUpdate_ =
true;
449 }
catch (InterruptedException e) {
457 for (
int i = 0;
i < controllers_.size();
i++) {
466 synchronized(lock2_){
469 }
catch (InterruptedException e) {
477 String controllerName_;
478 Controller controller_;
481 boolean doFlag_ =
false;
482 ControllerAttribute(String modelName, String controllerName, Controller controller,
double stepTime) {
483 modelName_ = modelName;
484 controllerName_ = controllerName;
485 controller_ = controller;
486 stepTime_ = stepTime;
489 private void reset(Controller controller,
double stepTime) {
490 controller_ = controller;
491 stepTime_ = stepTime;
499 if (doCount_ <= time/stepTime_) {
504 }
catch (Exception e) {
511 if (doFlag_) controller_.control();
512 }
catch (Exception e) {
519 if (doFlag_) controller_.output();
520 }
catch (Exception e) {
528 }
catch (Exception e) {
534 controller_.initialize();
535 }
catch(ControllerException e){
536 System.out.println(
"setupController:"+e.description);
538 }
catch (Exception e) {
545 void execSWT( Runnable r,
boolean execInCurrentThread ){
546 if( execInCurrentThread ) {
549 Display display = Display.getDefault();
550 if ( display!=
null && !display.isDisposed())
551 display.asyncExec(r);
556 if(clockGenerator_ ==
null){
563 GrxDebugUtil.
println(
"OpenHRPView : ClockGenerator is successfully registered to NamingService");
564 }
catch (Exception ex) {
577 GrxDebugUtil.
println(
"OpenHRPView : successfully ClockGenerator unbound to localhost NameService");
578 }
catch(Exception ex){
579 GrxDebugUtil.
println(
"OpenHRPView : failed to unbind ClockGenerator to localhost NameService");
581 clockGenerator_ =
null;
585 if (currentDynamics_ !=
null) {
587 currentDynamics_.destroy();
588 }
catch (Exception e) {
591 currentDynamics_ =
null;
593 Collection<GrxSimulationItem> col=(Collection<GrxSimulationItem>) manager_.getItemMap(
GrxSimulationItem.class).values();
594 if(col.size()==1 && col.contains(
this))
598 public void delete() {
604 getDynamicsSimulator(
true);
609 float cameraFrameRate = 1;
610 for (
int i=0;
i<modelList.size();
i++) {
617 if(bodyInfo==
null)
return false;
619 if(currentWorld_!=
null)
621 currentDynamics_.registerCharacter(model.
getName(), bodyInfo);
623 robotEntry_.add(model.
getName());
626 for (
int j=0; j<cameraList.size(); j++) {
629 cameraFrameRate = lcm(cameraFrameRate, frameRate);
632 viewSimulationStep_ = 1.0d/cameraFrameRate;
634 String smethod = getStr(
"method");
635 IntegrateMethod m=
null;
637 m=IntegrateMethod.RUNGE_KUTTA;
639 m=IntegrateMethod.EULER;
640 currentDynamics_.init(getDbl(
"timeStep", 0.001), m, SensorOption.ENABLE_SENSOR);
641 currentDynamics_.setGVector(
new double[] { 0.0, 0.0, getDbl(
"gravity", 9.8) });
643 for (
int i=0;
i<modelList.size();
i++) {
650 currentDynamics_.setCharacterLinkData(
655 currentDynamics_.setCharacterLinkData(
662 for (
int j=0; j<jms.length; j++) {
663 double[] mode =
new double[1];
665 currentDynamics_.setCharacterLinkData(model.
getName(), model.
links_.get(j).getName(), LinkDataType.POSITION_GIVEN, mode );
668 currentDynamics_.setCharacterAllJointModes(model.
getName(), JointDriveMode.HIGH_GAIN_MODE);
672 currentDynamics_.setCharacterAllLinkData(
673 model.
getName(), LinkDataType.JOINT_VALUE,
676 currentDynamics_.setCharacterAllLinkData(
677 model.
getName(), LinkDataType.JOINT_VELOCITY,
680 currentDynamics_.calcWorldForwardKinematics();
684 for (
int i=0;
i<collisionPair.size();
i++) {
686 currentDynamics_.registerCollisionCheckPair(
687 item.getStr(
"objectName1",
""),
688 item.getStr(
"jointName1",
""),
689 item.getStr(
"objectName2",
""),
690 item.getStr(
"jointName2",
""),
691 item.getDbl(
"staticFriction", 0.5),
692 item.getDbl(
"slidingFriction", 0.5),
693 item.getDblAry(
"springConstant",
new double[]{0.0,0.0,0.0,0.0,0.0,0.0}),
694 item.getDblAry(
"damperConstant",
new double[]{0.0,0.0,0.0,0.0,0.0,0.0}),
695 item.getDbl(
"cullingThresh", 0.01),
696 item.getDbl(
"Restitution", 0.0));
699 List<GrxBaseItem> extraJoints = manager_.getSelectedItemList(
GrxExtraJointItem.class);
700 for (
int i=0;
i<extraJoints.size();
i++) {
702 ExtraJointType jointType = ExtraJointType.EJ_XYZ;
703 if(
item.getStr(
"jointType",
"").equals(
"xyz"))
704 jointType = ExtraJointType.EJ_XYZ;
705 else if(
item.getStr(
"jointType",
"").equals(
"xy"))
706 jointType = ExtraJointType.EJ_XY;
707 else if(
item.getStr(
"jointType",
"").equals(
"z"))
708 jointType = ExtraJointType.EJ_Z;
709 currentDynamics_.registerExtraJoint(
710 item.getStr(
"object1Name",
""),
711 item.getStr(
"link1Name",
""),
712 item.getStr(
"object2Name",
""),
713 item.getStr(
"link2Name",
""),
714 item.getDblAry(
"link1LocalPos",
new double[]{0.0,0.0,0.0}),
715 item.getDblAry(
"link2LocalPos",
new double[]{0.0,0.0,0.0}),
717 item.getDblAry(
"jointAxis",
new double[]{0.0,0.0,0.0}),
720 currentDynamics_.initSimulation();
722 stateH_.value =
null;
723 }
catch (Exception e) {
724 GrxDebugUtil.printErr(
"initDynamicsSimulator:", e);
731 if (update && currentDynamics_ !=
null) {
733 currentDynamics_.destroy();
734 }
catch (Exception e) {
737 currentDynamics_ =
null;
740 if (currentDynamics_ ==
null) {
742 org.omg.CORBA.Object
obj =
744 DynamicsSimulatorFactory ifactory = DynamicsSimulatorFactoryHelper.narrow(
obj);
745 currentDynamics_ = ifactory.create();
746 currentDynamics_._non_existent();
748 }
catch (Exception e) {
751 currentDynamics_ =
null;
754 return currentDynamics_;
759 List<String> localStrList =
new Vector<String>();
762 if( model.isRobot() ){
763 localStrList.add( model.getProperty(
"controller") );
766 _refreshControllers( localStrList );
768 if( model.isRobot() ){
769 ret &= _setupController(model, _getControllerFromControllerName(model.getProperty(
"controller")));
778 if (
i.modelName_.equals(localID) ){
789 if (
i.controllerName_.equals(controllerName) ){
799 Vector<String> localStrVec =
new Vector<String>();
801 int index = refStrList.indexOf(
i.controllerName_);
803 localStrVec.add(
i.controllerName_);
805 for (String
i: localStrVec) {
808 if( proc !=
null && proc.
stop() ){
810 _getControllerFromControllerName(
i);
811 int index = controllers_.indexOf( _getControllerFromControllerName(
i) );
813 controllers_.remove(index);
819 String controllerName = model.getProperty(
"controller");
820 if (controllerName ==
null || controllerName.equals(
""))
823 double step = model.
getDbl(
"controlTime", 0.005);
824 if(stepTime_ > step){
829 String optionAdd =
null;
830 if (!isTrue(
"integrate",
true))
831 optionAdd =
" -nosim";
836 boolean doRestart =
false;
839 String dir = model.
getStr(
"setupDirectory",
"");
840 String
com = model.
getStr(
"setupCommand",
"");
844 cobj._non_existent();
845 if (isInteractive_ && (!
com.equals(
"") || proc !=
null)) {
847 MessageBundle.
get(
"GrxOpenHRPView.dialog.message.restartController0")+controllerName+
MessageBundle.
get(
"GrxOpenHRPView.dialog.message.restartController1") +
MessageBundle.
get(
"GrxOpenHRPView.dialog.message.restartController2") ,MessageDialog.QUESTION,
new String[]{MessageBundle.get(
"GrxOpenHRPView.dialog.button.yes"),MessageBundle.get(
"GrxOpenHRPView.dialog.button.no"),MessageBundle.get(
"GrxOpenHRPView.dialog.button.cancel")}, 2);
848 switch( dialog.open() ){
853 if(deactivatedController !=
null)
854 deactivatedController.
active();
860 if ((!
com.equals(
"") || proc !=
null) && deactivatedController !=
null)
861 deactivatedController.
active();
864 }
catch (Exception e) {
869 if (cobj ==
null || doRestart) {
873 if (!
com.equals(
"")) {
874 com = dir+java.io.File.separator+
com;
875 String osname = System.getProperty(
"os.name");
876 if(osname.indexOf(
"Windows") >= 0){
880 pi.
id = controllerName;
892 proc = pManager.
get(controllerName);
899 proc.
start(optionAdd);
903 Date before =
new Date();
904 for (
int j=0; ; j++) {
908 Controller controller = ControllerHelper.narrow(cobj);
909 controller.setModelName(model.
getName());
910 controller.setDynamicsSimulator(currentDynamics_);
911 if (isTrue(
"viewsimulate")){
913 ViewSimulator viewsim = ViewSimulatorHelper.narrow(cobj);
914 controller.setViewSimulator(viewsim);
917 if( refAttr ==
null){
920 refAttr.
reset(controller, step);
923 controller.setTimeStep(step);
924 controller.initialize();
927 }
catch (ControllerException e) {
928 System.out.println(
"setupController:"+e.description);
933 }
catch (Exception e) {
938 if (j > WAIT_COUNT_ || (
new Date().getTime() - before.getTime() > WAIT_COUNT_*1000)) {
941 MessageDialog dialog =
new MessageDialog(
GrxUIPerspectiveFactory.
getCurrentShell(),
MessageBundle.
get(
"GrxOpenHRPView.dialog.title.setupController"),
null,
MessageBundle.
get(
"GrxOpenHRPView.dialog.message.setupController0")+controllerName+
").\n" +
MessageBundle.
get(
"GrxOpenHRPView.dialog.message.setupController1"),MessageDialog.QUESTION,
new String[]{MessageBundle.get(
"GrxOpenHRPView.dialog.button.yes"),MessageBundle.get(
"GrxOpenHRPView.dialog.button.no"),MessageBundle.get(
"GrxOpenHRPView.dialog.button.cancel")}, 2);
942 int ans = dialog.open();
946 }
else if (ans == 1) {
954 try {Thread.sleep(1000);}
catch (Exception e) {}
971 int result = dialog.open();
972 str = dialog.getValue();
973 if (result == InputDialog.CANCEL)
976 double d = Double.parseDouble(str);
979 totalTime_ = totalTime_ + d;
980 setDbl(
"totalTime", totalTime_);
984 }
catch (NumberFormatException e) {}
994 super.restoreProperties();
995 String str = getProperty(
"totalTime");
997 setDbl(
"totalTime", 20.0);
998 str = getProperty(
"timeStep");
1000 setDbl(
"timeStep", 0.001);
1001 str = getProperty(
"gravity");
1003 setDbl(
"gravity", 9.8);
1004 str = getProperty(
"method");
1006 setProperty(
"method",
"RUNGE_KUTTA");
1007 str = getProperty(
"integrate");
1009 setBool(
"integrate",
true);
1010 str = getProperty(
"realTime");
1012 setBool(
"realTime",
false);
1013 str = getProperty(
"viewsimulate");
1015 setBool(
"viewsimulate",
false);
1019 if(key.equals(
"method")){
1021 }
else if(key.equals(
"integrate") || key.equals(
"viewsimulate") || key.equals(
"realTime")){
1024 return super.GetValueEditType(key);
1028 BigInteger bigA = BigInteger.valueOf(
a);
1029 BigInteger bigB = BigInteger.valueOf(
b);
1030 return bigA.gcd(bigB).intValue();
1034 return a*
b/gcd(
a,
b);
1038 return (
float)lcm((
int)
a, (
int)
b);